Robotic Team 1 / ROS_node

ROS node source code

remote.cpp@0:dd3bf9e559fd, 2018-04-20 (annotated)

Committer:
m56542321
Date:
Fri Apr 20 09:51:28 2018 +0000
Revision:
0:dd3bf9e559fd
ROS node source code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m56542321 0:dd3bf9e559fd 1 /*
m56542321 0:dd3bf9e559fd 2 * **System Structure**
m56542321 0:dd3bf9e559fd 3 *
m56542321 0:dd3bf9e559fd 4 * remote_car_node
m56542321 0:dd3bf9e559fd 5 * pkg : robotics
m56542321 0:dd3bf9e559fd 6 * pub : "/cmd_ang_vel" (Vector3)
m56542321 0:dd3bf9e559fd 7 * sub : "/cmd_vel" (Twist)
m56542321 0:dd3bf9e559fd 8 *
m56542321 0:dd3bf9e559fd 9 * turtlebot_teleop_key
m56542321 0:dd3bf9e559fd 10 * pkg : turtlebot_teleop
m56542321 0:dd3bf9e559fd 11 * pub : "/cmd_vel" (Twist)
m56542321 0:dd3bf9e559fd 12 * sub : --
m56542321 0:dd3bf9e559fd 13 *
m56542321 0:dd3bf9e559fd 14 * STM
m56542321 0:dd3bf9e559fd 15 * pkg :
m56542321 0:dd3bf9e559fd 16 * pub : "/feedback_wheel_ang_vel" (Twist)
m56542321 0:dd3bf9e559fd 17 * sub : "/cmd_ang_vel" (Vector3)
m56542321 0:dd3bf9e559fd 18 *
m56542321 0:dd3bf9e559fd 19 */
m56542321 0:dd3bf9e559fd 20
m56542321 0:dd3bf9e559fd 21 #include "ros/ros.h"
m56542321 0:dd3bf9e559fd 22 #include "geometry_msgs/Vector3.h"
m56542321 0:dd3bf9e559fd 23 #include "geometry_msgs/Twist.h"
m56542321 0:dd3bf9e559fd 24
m56542321 0:dd3bf9e559fd 25
m56542321 0:dd3bf9e559fd 26 // dimension of car (in meter)
m56542321 0:dd3bf9e559fd 27 #define wheel_radius 0.032f
m56542321 0:dd3bf9e559fd 28 #define center_distance 0.175f // wheel to wheel distance
m56542321 0:dd3bf9e559fd 29
m56542321 0:dd3bf9e559fd 30
m56542321 0:dd3bf9e559fd 31 // declare global publisher and subscriber
m56542321 0:dd3bf9e559fd 32 ros::Publisher cmd_ang_vel_pub;
m56542321 0:dd3bf9e559fd 33 ros::Subscriber cmd_vel_sub;
m56542321 0:dd3bf9e559fd 34
m56542321 0:dd3bf9e559fd 35 void givecmd(const geometry_msgs::Twist &twist_received);
m56542321 0:dd3bf9e559fd 36
m56542321 0:dd3bf9e559fd 37 int main(int argc, char **argv)
m56542321 0:dd3bf9e559fd 38 {
m56542321 0:dd3bf9e559fd 39 // initial ROS node
m56542321 0:dd3bf9e559fd 40
m56542321 0:dd3bf9e559fd 41 // create a node named "remote_car_node"
m56542321 0:dd3bf9e559fd 42 ros::init(argc, argv, "remote_car_node");
m56542321 0:dd3bf9e559fd 43 ros::NodeHandle n1, n2;
m56542321 0:dd3bf9e559fd 44
m56542321 0:dd3bf9e559fd 45 // initial publisher and subscribe
m56542321 0:dd3bf9e559fd 46 // publish cmd_ang_vel to STM board
m56542321 0:dd3bf9e559fd 47 cmd_ang_vel_pub = n1.advertise<geometry_msgs::Vector3>("/cmd_ang_vel", 50);
m56542321 0:dd3bf9e559fd 48 // receive cmd_vel from keyboard (turtlebot_teleop)
m56542321 0:dd3bf9e559fd 49 cmd_vel_sub = n2.subscribe("/cmd_vel", 10, givecmd);
m56542321 0:dd3bf9e559fd 50
m56542321 0:dd3bf9e559fd 51
m56542321 0:dd3bf9e559fd 52 // publish command 10 times per second
m56542321 0:dd3bf9e559fd 53 ros::Rate loop_rate(10);
m56542321 0:dd3bf9e559fd 54
m56542321 0:dd3bf9e559fd 55 // keep running this program
m56542321 0:dd3bf9e559fd 56 while(ros::ok())
m56542321 0:dd3bf9e559fd 57 {
m56542321 0:dd3bf9e559fd 58 ros::spinOnce();
m56542321 0:dd3bf9e559fd 59 loop_rate.sleep();
m56542321 0:dd3bf9e559fd 60 }
m56542321 0:dd3bf9e559fd 61 }
m56542321 0:dd3bf9e559fd 62
m56542321 0:dd3bf9e559fd 63 void givecmd(const geometry_msgs::Twist &twist_received)
m56542321 0:dd3bf9e559fd 64 {
m56542321 0:dd3bf9e559fd 65 geometry_msgs::Vector3 vector3;
m56542321 0:dd3bf9e559fd 66
m56542321 0:dd3bf9e559fd 67 double car_vel = twist_received.linear.x;
m56542321 0:dd3bf9e559fd 68 double car_ang_vel = twist_received.angular.z;
m56542321 0:dd3bf9e559fd 69
m56542321 0:dd3bf9e559fd 70 double left_vel = 0.0;
m56542321 0:dd3bf9e559fd 71 double right_vel = 0.0;
m56542321 0:dd3bf9e559fd 72
m56542321 0:dd3bf9e559fd 73 // convert
m56542321 0:dd3bf9e559fd 74 // (car_vel, car_ang_vel) --> (left_ang_vel, right_ang_vel)
m56542321 0:dd3bf9e559fd 75 left_vel = car_vel + car_ang_vel * center_distance / 2;
m56542321 0:dd3bf9e559fd 76 right_vel = car_vel - car_ang_vel * center_distance / 2;
m56542321 0:dd3bf9e559fd 77
m56542321 0:dd3bf9e559fd 78 // publish to "cmd_ang_vel"
m56542321 0:dd3bf9e559fd 79 vector3.x = left_vel / wheel_radius * 10;
m56542321 0:dd3bf9e559fd 80 vector3.y = right_vel / wheel_radius * 10;
m56542321 0:dd3bf9e559fd 81
m56542321 0:dd3bf9e559fd 82 cmd_ang_vel_pub.publish(vector3);
m56542321 0:dd3bf9e559fd 83
m56542321 0:dd3bf9e559fd 84 }
m56542321 0:dd3bf9e559fd 85
m56542321 0:dd3bf9e559fd 86
m56542321 0:dd3bf9e559fd 87
m56542321 0:dd3bf9e559fd 88
m56542321 0:dd3bf9e559fd 89
m56542321 0:dd3bf9e559fd 90
m56542321 0:dd3bf9e559fd 91
m56542321 0:dd3bf9e559fd 92
m56542321 0:dd3bf9e559fd 93