void tacta_output(void) { // int lf_lf = lh_lf_output[loc]; // int lf_rf = lh_rf_output[loc]; // int rf_lf = rh_lf_output[loc]; // int rf_rf = rh_rf_output[loc]; // ROS_WARN("SENDING [%d, %d, %d, %d]", lf_lf, lf_rf, rf_lf, rf_rf); data_amp[0] = TACTA_BELT_ADDRESS; data_amp[1] = TACTA_BELT_SET_OUTPUT; data_amp[2] = TACTA_BELT_ID; data_amp[3] = RH_RF; data_amp[4] = rh_rf_output[loc]; data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4]; device.write(data_amp, 6); data_amp[0] = TACTA_BELT_ADDRESS; data_amp[1] = TACTA_BELT_SET_OUTPUT; data_amp[2] = TACTA_BELT_ID; data_amp[3] = RH_LF; data_amp[4] = rh_lf_output[loc]; data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4]; device.write(data_amp, 6); data_amp[0] = TACTA_BELT_ADDRESS; data_amp[1] = TACTA_BELT_SET_OUTPUT; data_amp[2] = TACTA_BELT_ID; data_amp[3] = LH_RF; data_amp[4] = lh_rf_output[loc]; data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4]; device.write(data_amp, 6); data_amp[0] = TACTA_BELT_ADDRESS; data_amp[1] = TACTA_BELT_SET_OUTPUT; data_amp[2] = TACTA_BELT_ID; data_amp[3] = LH_LF; data_amp[4] = lh_lf_output[loc]; data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4]; device.write(data_amp, 6); loc++; if (loc >= 100) { loc = 0; } }
//receive cmds_vel from nav_stack void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel){ std::string result = drive(cmd_vel->linear.x, cmd_vel->angular.z, ID_Robot); ROS_INFO("[cmd_vel]: Vlinear = %f, Vangular = %f",cmd_vel->linear.x, cmd_vel->angular.z); ROS_INFO("Sending: %s\n", result.c_str()); serial_port.write( result.c_str() ); }
//Callback for use with the belts msg void belt_cb (feedback_devices::tacta_belt belt){ ROS_INFO("GOT A BELT MSG"); //For Each Motor for (int i = 0; i < belt.motors; i++){ ROS_INFO("MOTOR: %d, SET: %f", i, belt.values[i]); } ROS_INFO("--------------------"); //For Each Motor for (int i = 0; i < belt.motors; i++){ //Write the value to the belt data[0] = TACTA_BELT_ADDRESS; data[1] = TACTA_BELT_SET_OUTPUT; data[2] = TACTA_BELT_ID; data[3] = i; data[4] = belt.values[i] * 255; data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4]; device.write(data, 6); } }
/** * This tutorial demonstrates simple receipt of messages over the ROS system. */ void cmd_vel_Callback(const geometry_msgs::Twist::ConstPtr& msg) { double temp_val; rec_vel_x = msg->linear.x; rec_vel_y = msg->linear.y; base_angle = msg->angular.z; if(rec_vel_x == 0 && rec_vel_y ==0) { ang_send[0]='x'; base_vel = 0.0; base_angle = 0; } else{ if(base_angle>0){ ang_send[0] ='s'; base_vel = 0.3; } else if(base_angle<0){ ang_send[0]='a'; base_vel=0.3; } else if(base_angle==0){ ang_send[0]='z'; base_vel=0.22; } } device.write(ang_send,1); }
// Create a ROS publisher and suscriber for the tacta_belt int main(int argc, char** argv) { ros::init(argc, argv, "tacta_wrist"); ros::NodeHandle n; ros::Publisher state; ros::Rate r(TACTA_BELT_STATE_UPDATE); // Create a ROS subscriber for the input point cloud ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_wrist/input", 1, wrist_cb); // Change the next line according to your port name and baud rate try{ device.open("/dev/ttyUSB0", 19200); } catch(cereal::Exception& e) { ROS_FATAL("Failed to open the serial port!!!"); ROS_BREAK(); } ROS_INFO("The serial port is opened."); //On Init Enable B-Cast on All Channels data[0] = TACTA_BELT_ADDRESS; data[1] = TACTA_BELT_ENB_OUTPUT; data[2] = TACTA_BELT_BCAST; data[3] = TACTA_BELT_BCAST; data[4] = data[0] ^ data[1] ^ data[2] ^ data[3]; device.write(data, 5); ros::spin(); }
/** * @brief callback fonction pour envoyer la commande a la carte de robot. * @param hexapode_v2::sequence_moteur::Request& * @param hexapode_v2::sequence_moteur::Response& * @return bool */ bool sequence_moteur(hexapode_v2::sequence_moteur::Request &req, hexapode_v2::sequence_moteur::Response &res) { int i = 0; for(i = 0; i < 100; i++){ if (isSerialFree()){ break; } else{ usleep(5000); // 5 millisecondes } } strcpy(sequ, (req.sequence).c_str()); device.write(sequ, (int)(req.dim)); ROS_INFO("Ecriture sur le port serie "+req.dim); ROS_INFO(sequ); // Get the reply, the last value is the timeout in ms /* try{ device.read(reply, 256, TIMEOUT); } catch(cereal::TimeoutException& e) { ROS_ERROR("Timeout!"); } ROS_INFO("Reponse obtenu du port serie: %s", reply); */ ros::spinOnce(); return true; }
// Topic subscriber callbacks void commTxCallback(const hexacamina::commData trama) { char dato; // ROS_INFO("COMM: recibo dato"); dato = trama.data_hexapodo; cp.write(&dato, 1); ROS_INFO("COMM:\n"); ROS_INFO("[%s]: 0x%X.", COMM_MSG_TX, dato); }
// Create a ROS publisher and suscriber for the tacta_belt int main(int argc, char** argv) { ros::init(argc, argv, "tacta_fingers"); ros::NodeHandle n; // Create a ROS subscriber for the input point cloud ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_grippers/input", 1, grippers_cb); // Change the next line according to your port name and baud rate try{ device.open("/dev/ttyUSB0", 19200); } catch(cereal::Exception& e) { ROS_FATAL("Failed to open the serial port!!!"); ROS_BREAK(); } ROS_INFO("The serial port has been opened."); //On Init Enable B-Cast on All Channels data_amp[0] = TACTA_BELT_ADDRESS; data_amp[1] = TACTA_BELT_ENB_OUTPUT; data_amp[2] = TACTA_BELT_BCAST; data_amp[3] = TACTA_BELT_BCAST; data_amp[4] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3]; device.write(data_amp, 5); ros::Rate r(100); for (int i = 0; i < 100; i++){ //Right Hand output struct rh_rf_output[i] = 0; rh_lf_output[i] = 0; //Left Hand output struct lh_rf_output[i] = 0; lh_lf_output[i] = 0; } while(ros::ok()){ tacta_output(); ros::spinOnce(); r.sleep(); } ros::spin(); }
// Create a ROS publisher and suscriber for the tacta_belt int main(int argc, char** argv) { ros::init(argc, argv, "tacta_belt"); ros::NodeHandle n; ros::Publisher state; ros::Rate r(TACTA_BELT_STATE_UPDATE); // Create a ROS subscriber for the input point cloud ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_belt/input", 1, belt_cb); //Advertise the two publishers, one for the commands and one for the gui state = n.advertise<feedback_devices::tacta_belt>("/feedback_devices/tacta_belt/state", 1); // Change the next line according to your port name and baud rate try{ device.open("/dev/ttyUSB0", 19200); } catch(cereal::Exception& e) { ROS_FATAL("Failed to open the serial port!!!"); ROS_BREAK(); } ROS_INFO("The serial port is opened."); //On Init Enable B-Cast on All Channels data[0] = TACTA_BELT_ADDRESS; data[1] = TACTA_BELT_ENB_OUTPUT; data[2] = TACTA_BELT_BCAST; data[3] = TACTA_BELT_BCAST; data[4] = data[0] ^ data[1] ^ data[2] ^ data[3]; device.write(data, 5); ros::spin(); // while(ros::ok()) // { // // // Get the reply, the last value is the timeout in ms // try{ device.read(reply, REPLY_SIZE, TIMEOUT); } // catch(cereal::TimeoutException& e) // { // ROS_ERROR("Timeout!"); // } // ROS_INFO("Got this reply: %s", reply); // // ros::spinOnce(); // r.sleep(); // } }
/** * @brief fonction pour verifier si la porte serie est libre ou pas, ca depend de la carte de robot.. * @param void * @return bool */ bool isSerialFree(){ strcpy(sequ, "Q\r\n"); device.write(sequ, 3); memset(reply, 0, 4); try{ device.read(reply, 1, 10); } catch(cereal::TimeoutException& e){ ROS_ERROR("Timeout!"); } ROS_INFO("%s", reply); if ('+' == reply[0]){ // busy. return false; } else{//'.', free. return true; } }
void tacta_output(void) { static int loc = 0; for (int channel = 0; channel < CHANNELS; channel++){ if (active[channel] == feedback_devices::tacta_box::ACTIVE) { data_amp[0] = TACTA_BELT_ADDRESS; data_amp[1] = TACTA_BELT_SET_OUTPUT; data_amp[2] = TACTA_BELT_ID; data_amp[3] = channel; data_amp[4] = output[channel][loc]; data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4]; device.write(data_amp, 6); } } loc++; if (loc >= 100) { loc = 0; } }
int main(int argc, char** argv){ //typical usage: "./traxbot_node /dev/ttyACMx" ros::init(argc, argv, "traxbot_node"); ros::NodeHandle n; ros::NodeHandle pn("~"); std::string port; if (argc<2){ port="/dev/ttyACM0"; ROS_WARN("No Serial Port defined, defaulting to \"%s\"",port.c_str()); ROS_WARN("Usage: \"rosrun [pkg] robot_node /serial_port\""); }else{ port=(std::string)argv[1]; ROS_INFO("Serial port: %s",port.c_str()); } // ROS publishers and subscribers ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 100); tf::TransformBroadcaster odom_broadcaster; ros::Subscriber cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, cmdVelReceived); odom_pub_ptr = &odom_pub; odom_broadcaster_ptr = &odom_broadcaster; // baud_rate and serial port: int baudrate; pn.param<std::string>("port", port, port.c_str()); pn.param("baudrate", baudrate, 115200); // Open the serial port to the robot try{ serial_port.open((char*)port.c_str(), baudrate); } catch(cereal::Exception& e){ ROS_FATAL("Robot -- Failed to open serial port!"); ROS_BREAK(); } //wait (2.5 seconds) until serial port gets ready ros::Duration(2.5).sleep(); // Ask Robot ID from the Arduino board (stored in the EEPROM) ROS_INFO("Starting Traxbot..."); serial_port.write("@5e"); std::string reply; try{ serial_port.readBetween(&reply,'@','e'); } catch(cereal::TimeoutException& e){ ROS_ERROR("Initial Read Timeout!"); } int VDriver, Temperature, OMNI_Firmware, Battery; sscanf(reply.c_str(), "@5,%d,%d,%d,%d,%de", &Temperature, &OMNI_Firmware, &Battery, &VDriver, &ID_Robot); //encoder msg parsing ROS_INFO("Traxbot ID = %d", ID_Robot); if (ID_Robot < 1 || ID_Robot > 3){ ROS_WARN("Attention! Unexpected Traxbot ID!"); } ROS_INFO("OMNI Board Temperature = %.2f C", Temperature*0.01); ROS_INFO("OMNI Firmware = %.2f", OMNI_Firmware*0.01); ROS_INFO("Arduino Firmware Version = %d.00", VDriver); if (VDriver > 1500){ ROS_ERROR("Reset Robot Connection and try again."); return(0); } ROS_INFO("Battery = %.2f V", Battery*0.01 ); if (Battery < 100){ ROS_ERROR("Robot is off. Check power source!"); return(0); } if (Battery < 850 && Battery > 100){ ROS_WARN("Warning! Low Battery!"); } // Start receiving streaming data if( !serial_port.startReadBetweenStream(boost::bind(&robotDataCallback, _1), '@', 'e') ){ ROS_FATAL("Robot -- Failed to start streaming data!"); ROS_BREAK(); } serial_port.write("@18e"); ros::spin(); //trigger callbacks and prevents exiting return(0); }
//Callback for use with the belts msg void wrist_cb (feedback_devices::tacta_wrist wrist){ ROS_INFO("--------------------"); //Top data[0] = TACTA_BELT_ADDRESS; data[1] = TACTA_BELT_SET_OUTPUT; data[2] = TACTA_BELT_ID; data[3] = 0; data[4] = wrist.top; data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4]; device.write(data, 6); //Bottom data[0] = TACTA_BELT_ADDRESS; data[1] = TACTA_BELT_SET_OUTPUT; data[2] = TACTA_BELT_ID; data[3] = 1; data[4] = wrist.bottom; data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4]; device.write(data, 6); //Left data[0] = TACTA_BELT_ADDRESS; data[1] = TACTA_BELT_SET_OUTPUT; data[2] = TACTA_BELT_ID; data[3] = 2; data[4] = wrist.left; data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4]; device.write(data, 6); //Right data[0] = TACTA_BELT_ADDRESS; data[1] = TACTA_BELT_SET_OUTPUT; data[2] = TACTA_BELT_ID; data[3] = 3; data[4] = wrist.right; data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4]; device.write(data, 6); //Front data[0] = TACTA_BELT_ADDRESS; data[1] = TACTA_BELT_SET_OUTPUT; data[2] = TACTA_BELT_ID; data[3] = 4; data[4] = wrist.front; data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4]; device.write(data, 6); //Back data[0] = TACTA_BELT_ADDRESS; data[1] = TACTA_BELT_SET_OUTPUT; data[2] = TACTA_BELT_ID; data[3] = 5; data[4] = wrist.back; data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4]; device.write(data, 6); }