// 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();
}
// 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();
// }
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener_vel");
ros::NodeHandle n;
// char reply[REPLY_SIZE];
try{ device.open("/dev/ttyACM1", 9600); }
catch(cereal::Exception& e)
{
ROS_FATAL("Failed to open the serial port!!!");
ROS_BREAK();
}
ROS_INFO("The serial port is opened.");
ros::Subscriber sub = n.subscribe("/cmd_vel", 1000, cmd_vel_Callback);
ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("act_vel", 1000);
ros::Publisher ang_pub = n.advertise<std_msgs::Float64>("act_th", 1000);
// ros::Rate loop_rate(10);
ros::Rate loop_rate(18);
while(ros::ok())
{
ros::spinOnce();
std_msgs::Float64 th;
geometry_msgs::Twist vel;
vel.linear.x = rec_vel_x;
vel.linear.y = rec_vel_y;
th.data = base_angle;
vel_pub.publish(vel);
ang_pub.publish(th);
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "subscriber");
ros::NodeHandle n;
ros::ServiceServer service = n.advertiseService("sequence_", sequence_moteur);
ROS_INFO("Ouverture du port serie");
try{ device.open("/dev/ttyUSB0", 115200); }
catch(cereal::Exception& e)
{
ROS_FATAL("Echec de l'ouverture du port serie!!!");
ROS_BREAK();
}
ROS_INFO("Le port serie est ouvert.");
ROS_INFO("Ready to send data");
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
//char data;
ros::init(argc, argv, "comm");
ros::NodeHandle n;
// Abro el puerto serial
ROS_INFO("Abriendo puerto [%s] a [%d] bps", COMM_PORT, COMM_BAUDRATE);
ROS_INFO("Publicando en: [%s]", COMM_MSG_RX);
ROS_INFO("Subscrito a : [%s]", COMM_MSG_TX);
cp.open(COMM_PORT, COMM_BAUDRATE);
//Subscripciones y publicaciones
ros::Subscriber sub = n.subscribe("COMM_MSG_TX", 50, commTxCallback);
ros::Publisher chatter_pub = n.advertise<hexacamina::commData> ("COMM_MSG_RX", 50);
// boost::thread spin_thread(&spinThread);
// hexacamina::commData msg;
while (ros::ok())// && simulationRunning)
{
// char data;
// cp.readBytes(&data, 1, 0);
// msg.data_hexapodo = data;
// ROS_INFO("[%s]: 0x%x ", COMM_MSG_RX, data);
// ROS_INFO("comm: hola");
// chatter_pub.publish(msg);
ros::spinOnce();
}
ROS_INFO("Adios comm!");
ros::shutdown();
return(0);
// spin_thread.join();
return 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);
}
