Diff: remote.cpp

Revision:

0:dd3bf9e559fd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/remote.cpp	Fri Apr 20 09:51:28 2018 +0000
@@ -0,0 +1,93 @@
+/*
+ * **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);
+
+}
+
+
+
+
+
+
+
+
+