File content as of revision 0:dd3bf9e559fd:

/*
 * **System Structure**
 *
 *  remote_car_node
 *    pkg : robotics
 *    pub : "/cmd_ang_vel"                  (Vector3)
 *    sub : "/cmd_vel"     (Twist)
 *
 *  turtlebot_teleop_key
 *    pkg : turtlebot_teleop
 *    pub : "/cmd_vel"     (Twist)
 *    sub : --
 *
 *  STM
 *    pkg :
 *    pub : "/feedback_wheel_ang_vel"       (Twist)
 *    sub : "/cmd_ang_vel"                  (Vector3)
 *
 */

#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Twist.h"


// dimension of car (in meter)
#define wheel_radius    0.032f
#define center_distance 0.175f  // wheel to wheel distance


// declare global publisher and subscriber
ros::Publisher   cmd_ang_vel_pub;
ros::Subscriber  cmd_vel_sub;

void givecmd(const geometry_msgs::Twist &twist_received);

int main(int argc, char **argv)
{
// initial ROS node

	// create a node named "remote_car_node"
	ros::init(argc, argv, "remote_car_node");
	ros::NodeHandle n1, n2;

	// initial publisher and subscribe
	// publish cmd_ang_vel to STM board
	cmd_ang_vel_pub = n1.advertise<geometry_msgs::Vector3>("/cmd_ang_vel", 50);
	// receive cmd_vel from keyboard (turtlebot_teleop)
	cmd_vel_sub = n2.subscribe("/cmd_vel", 10, givecmd);


	// publish command 10 times per second
	ros::Rate loop_rate(10);

// keep running this program
	while(ros::ok())
	{
		ros::spinOnce();
		loop_rate.sleep();
	}
}

void givecmd(const geometry_msgs::Twist &twist_received)
{
	geometry_msgs::Vector3  vector3;

	double car_vel     = twist_received.linear.x;
	double car_ang_vel = twist_received.angular.z;

	double left_vel  = 0.0;
	double right_vel = 0.0;

	// convert
	// (car_vel, car_ang_vel) --> (left_ang_vel, right_ang_vel)
	left_vel  = car_vel + car_ang_vel * center_distance / 2;
	right_vel = car_vel - car_ang_vel * center_distance / 2;

	// publish to "cmd_ang_vel"
	vector3.x = left_vel / wheel_radius * 10;
	vector3.y = right_vel / wheel_radius * 10;

	cmd_ang_vel_pub.publish(vector3);

}