added callback function for control on the web

op3_demo/include/op3_demo/action_demo.h

@@ -88,12 +88,14 @@ class ActionDemo : public OPDemo
 
   void actionSetNameCallback(const std_msgs::String::ConstPtr& msg);
   void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
+  void demoCommandCallback(const std_msgs::String::ConstPtr &msg);
 
   ros::Publisher module_control_pub_;
   ros::Publisher motion_index_pub_;
   ros::Publisher play_sound_pub_;
 
   ros::Subscriber buttuon_sub_;
+  ros::Subscriber demo_command_sub_;
 
   ros::ServiceClient is_running_client_;
 

op3_demo/include/op3_demo/vision_demo.h

@@ -56,6 +56,7 @@ class VisionDemo : public OPDemo
 
   void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
   void facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg);
+  void demoCommandCallback(const std_msgs::String::ConstPtr &msg);
 
   void setModuleToDemo(const std::string &module_name);
 

op3_demo/src/action/action_demo.cpp

@@ -37,6 +37,8 @@ ActionDemo::ActionDemo()
   std::string default_play_list = "default";
   play_list_name_ = nh.param<std::string>("action_script_play_list", default_play_list);
 
+  demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, &ActionDemo::demoCommandCallback, this);
+
   parseActionScript (script_path_);
 
   boost::thread queue_thread = boost::thread(boost::bind(&ActionDemo::callbackThread, this));
@@ -360,4 +362,19 @@ void ActionDemo::setModuleToDemo(const std::string &module_name)
   std::cout << "enable module : " << module_name << std::endl;
 }
 
+void ActionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg)
+{
+  if (enable_ == false)
+    return;
+
+  if (msg->data == "start")
+  {
+    resumeProcess();
+  }
+  else if (msg->data == "stop")
+  {
+    pauseProcess();
+  }
+}
+
 } /* namespace robotis_op */

op3_demo/src/demo_node.cpp

@@ -41,6 +41,8 @@ void setLED(int led);
 bool checkManagerRunning(std::string& manager_name);
 void dxlTorqueChecker();
 
+void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg);
+
 const int SPIN_RATE = 30;
 const bool DEBUG_PRINT = false;
 
@@ -73,6 +75,7 @@ int main(int argc, char **argv)
   led_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
   dxl_torque_pub = nh.advertise<std_msgs::String>("/robotis/dxl_torque", 0);
   ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback);
+  ros::Subscriber mode_command_sub = nh.subscribe("/robotis/command_mode", 1, demoModeCommandCallback);
 
   default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/";
 
@@ -313,3 +316,55 @@ void dxlTorqueChecker()
 
   dxl_torque_pub.publish(check_msg);
 }
+
+void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg)
+{
+  // In demo mode
+  if (current_status != Ready)
+  {
+    if (msg->data == "ready")
+    {
+      // go to mode selection status
+      desired_status = Ready;
+      apply_desired = true;
+
+      playSound(default_mp3_path + "Demonstration ready mode.mp3");
+      setLED(0x01 | 0x02 | 0x04);
+    }
+  }
+  // In ready mode
+  else
+  {
+    if(msg->data == "soccer")
+    {
+      desired_status = SoccerDemo;
+      apply_desired = true;
+
+      // play sound
+      dxlTorqueChecker();
+      playSound(default_mp3_path + "Start soccer demonstration.mp3");
+      ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
+    }
+    else if(msg->data == "vision")
+    {
+      desired_status = VisionDemo;
+      apply_desired = true;
+
+      // play sound
+      dxlTorqueChecker();
+      playSound(default_mp3_path + "Start vision processing demonstration.mp3");
+      ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
+    }
+    else if(msg->data == "action")
+    {
+      desired_status = ActionDemo;
+      apply_desired = true;
+
+      // play sound
+      dxlTorqueChecker();
+      playSound(default_mp3_path + "Start motion demonstration.mp3");
+      ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
+    }
+  }
+}
+

op3_demo/src/soccer/soccer_demo.cpp

@@ -208,7 +208,7 @@ void SoccerDemo::callbackThread()
   rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
 
   buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &SoccerDemo::buttonHandlerCallback, this);
-  demo_command_sub_ = nh.subscribe("/ball_tracker/command", 1, &SoccerDemo::demoCommandCallback, this);
+  demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, &SoccerDemo::demoCommandCallback, this);
   imu_data_sub_ = nh.subscribe("/robotis/open_cr/imu", 1, &SoccerDemo::imuDataCallback, this);
 
   is_running_client_ = nh.serviceClient<op3_action_module_msgs::IsRunning>("/robotis/action/is_running");

op3_demo/src/vision/vision_demo.cpp

@@ -141,29 +141,7 @@ void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
 
   if (msg->data == "start")
   {
-//    switch (play_status_)
-//      {
-//        case PlayAction:
-//        {
-//          pauseProcess();
-//          break;
-//        }
-//
-//        case PauseAction:
-//        {
-//          resumeProcess();
-//          break;
-//        }
-//
-//        case StopAction:
-//        {
-//          resumeProcess();
-//          break;
-//        }
-//
-//        default:
-//          break;
-//      }
+
   }
   else if (msg->data == "mode")
   {
@@ -171,6 +149,21 @@ void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
   }
 }
 
+void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg)
+{
+  if (enable_ == false)
+    return;
+
+  if (msg->data == "start")
+  {
+
+  }
+  else if (msg->data == "stop")
+  {
+
+  }
+}
+
 void VisionDemo::setModuleToDemo(const std::string &module_name)
 {
   std_msgs::String control_msg;