From 1d8ada838daf7c5c5c7b91710ecdaa541173743d Mon Sep 17 00:00:00 2001
From: Guillaume Courrier <guillaume.courrier@supelec.fr>
Date: Mon, 9 Dec 2019 21:47:10 +0100
Subject: [PATCH] finalisation du launch et du controller

---
 .../launch/gesture_control.launch             |  8 +++--
 ROS/gesture_based_control/src/controller.cpp  | 33 +++++++++++++++----
 2 files changed, 33 insertions(+), 8 deletions(-)

diff --git a/ROS/gesture_based_control/launch/gesture_control.launch b/ROS/gesture_based_control/launch/gesture_control.launch
index 7bfe4d6..316aa9a 100644
--- a/ROS/gesture_based_control/launch/gesture_control.launch
+++ b/ROS/gesture_based_control/launch/gesture_control.launch
@@ -1,6 +1,10 @@
 <launch>
-  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node"/>
+  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node">
+    <param name="pixel_format" value="yuyv"/>
+  </node>
   <node name="rqt_image_view" pkg="rqt_image_view" type="rqt_image_view"/>
-  <node name="controller" pkg="gesture_based_control" type="controller"/>
+  <node name="controller" pkg="gesture_based_control" type="controller" output="screen">
+    <remap from="/cmd_vel" to="/turtle1/cmd_vel"/>
+  </node>
   <node name="turtle" pkg="turtlesim" type="turtlesim_node"/>
 </launch>
diff --git a/ROS/gesture_based_control/src/controller.cpp b/ROS/gesture_based_control/src/controller.cpp
index d0639d8..6223ccb 100644
--- a/ROS/gesture_based_control/src/controller.cpp
+++ b/ROS/gesture_based_control/src/controller.cpp
@@ -1,17 +1,38 @@
 #include <ros/ros.h>
 #include <std_msgs/String.h>
+#include <geometry_msgs/Twist.h>
 
-void callback(const std_msgs::String& msg) {
-  //ROS_INFO("coucou");
+void callback(const std_msgs::String& msg, geometry_msgs::Twist& cmd) {
+  if (msg.data == "forward") {
+    cmd.linear.x = 1.0;
+    cmd.angular.z = 0.0;
+  } else if (msg.data == "stop") {
+    cmd = geometry_msgs::Twist();
+  } else if (msg.data == "left") {
+    cmd.angular.z = 1.0;
+    cmd.linear.x = 0.0;
+  } else if (msg.data == "right") {
+    cmd.angular.z = -1.0;
+    cmd.linear.x = 0.0;
+  }
 }
 
 int main(int argc, char** argv) {
   ros::init(argc, argv, "controller");
-  /*
 
   ros::NodeHandle n;
-  ros::Subscriber sub = n.subscribe("/order", 0, callback);
-  ros::spin();
-  */
+  ros::Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 0);
+
+  geometry_msgs::Twist cmd;
+  ros::Rate rate(10);
+
+  boost::function<void(const std_msgs::String&)> cb = boost::bind(callback, _1, boost::ref(cmd));
+  ros::Subscriber sub = n.subscribe<std_msgs::String&>("/order", 0, cb);
+
+  while(ros::ok()) {
+    pub.publish(cmd);
+    ros::spinOnce();
+    rate.sleep();
+  }
   return 0;
 }