action_script_player.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /*
18  * thormang3_action_script_player.cpp
19  *
20  * Created on: Mar 25, 2016
21  * Author: Jay Song
22  */
23 
24 #include <ros/ros.h>
25 #include <ros/package.h>
26 #include <std_msgs/Int32.h>
27 #include <std_msgs/String.h>
28 #include <boost/thread.hpp>
29 #include <yaml-cpp/yaml.h>
30 #include "robotis_controller_msgs/StatusMsg.h"
31 #include "thormang3_action_module_msgs/IsRunning.h"
32 #include "thormang3_action_module_msgs/StartAction.h"
33 
34 #define JOINT_NAME_KEY "joint_name"
35 #define ACTION_PLAY_CMD_NAME "play"
36 #define MP3_PLAY_CMD_NAME "mp3"
37 #define WAIT_ACTION_PLAY_FINISH_CMD_NAME "wait"
38 #define SLEEP_CMD_NAME "sleep"
39 
45 
46 thormang3_action_module_msgs::IsRunning g_is_running_srv;
47 
49 
51 
52 typedef struct
53 {
54  std::string cmd_name;
55  std::string cmd_arg_str;
58 
59 std::vector<std::string> g_joint_name_list;
60 std::vector<action_script_cmd> g_action_script_data;
61 
62 std::string convertIntToString(int n)
63 {
64  std::ostringstream ostr;
65  ostr << n;
66  return ostr.str();
67 }
68 
69 int convertStringToInt(std::string str)
70 {
71  return atoi(str.c_str());
72 }
73 
74 bool isActionRunning(void)
75 {
76  if (g_is_running_client.call(g_is_running_srv) == false)
77  {
78  ROS_ERROR("Failed to get action status");
79  return true;
80  }
81  else
82  {
83  if (g_is_running_srv.response.is_running == true)
84  {
85  return true;
86  }
87  }
88 
89  return false;
90 }
91 
92 bool parseActionScript(int action_script_index)
93 {
94  g_action_script_data.clear();
95 
96  YAML::Node action_script_file_doc;
97  try
98  {
99  // load yaml
100  action_script_file_doc = YAML::LoadFile(g_action_script_file_path.c_str());
101  } catch (const std::exception& e)
102  {
103  ROS_ERROR("Failed to load action script file.");
104  return false;
105  }
106 
107  //find action script
108  std::string script_index_key = "script" + convertIntToString(action_script_index);
109  YAML::Node action_script_doc = action_script_file_doc[script_index_key];
110  if (action_script_doc == NULL)
111  {
112  std::string status_msg = "Failed to find action script #" + convertIntToString(action_script_index);
113  ROS_ERROR_STREAM(status_msg);
114  return false;
115  }
116 
117  int cmd_num = 1;
118  std::string cmd_key = "";
119  try
120  {
121  g_joint_name_list.clear();
122  YAML::Node joint_name_doc = action_script_doc[JOINT_NAME_KEY];
123  if (joint_name_doc != NULL)
124  g_joint_name_list = joint_name_doc.as< std::vector<std::string> >();
125 
126  while (true)
127  {
128  //check cmd exist
129  cmd_key = "cmd" + convertIntToString(cmd_num);
130  YAML::Node action_script_cmd_doc = action_script_doc[cmd_key];
131  if (action_script_cmd_doc == NULL)
132  {
133  break;
134  }
135 
136  //check validity of cmd_name
137  action_script_cmd temp_cmd;
138  if (action_script_cmd_doc["cmd_name"] == NULL)
139  {
140  std::string status_msg = "cmd#" + convertIntToString(cmd_num) + " of " + "script#" + convertIntToString(action_script_index) + " is invalid.";
141  ROS_ERROR_STREAM(status_msg);
142  return false;
143  }
144 
145  //check validity of cmd_arg
146  temp_cmd.cmd_name = action_script_cmd_doc["cmd_name"].as<std::string>();
147  if ((temp_cmd.cmd_name != "wait") && (action_script_cmd_doc["cmd_arg"] == NULL))
148  {
149  std::string status_msg = "cmd#" + convertIntToString(cmd_num) + " of " + "script#" + convertIntToString(action_script_index) + " is invalid.";
150  ROS_ERROR_STREAM(status_msg);
151  return false;
152  }
153 
154  //get cmd_arg
155  if (temp_cmd.cmd_name == ACTION_PLAY_CMD_NAME)
156  {
157  temp_cmd.cmd_arg_int = action_script_cmd_doc["cmd_arg"].as<int>();
158  }
159  else if (temp_cmd.cmd_name == MP3_PLAY_CMD_NAME)
160  {
161  temp_cmd.cmd_arg_str = action_script_cmd_doc["cmd_arg"].as<std::string>();
162  }
163  else if (temp_cmd.cmd_name == WAIT_ACTION_PLAY_FINISH_CMD_NAME)
164  {
165  temp_cmd.cmd_arg_str = "";
166  temp_cmd.cmd_arg_int = 0;
167  }
168  else if (temp_cmd.cmd_name == SLEEP_CMD_NAME)
169  {
170  temp_cmd.cmd_arg_int = action_script_cmd_doc["cmd_arg"].as<int>();
171  if (temp_cmd.cmd_arg_int < 0)
172  {
173  std::string status_msg = "cmd#" + convertIntToString(cmd_num) + " of " + "script#" + convertIntToString(action_script_index) + " is invalid.";
174  ROS_ERROR_STREAM(status_msg);
175  g_action_script_data.clear();
176  return false;
177  }
178  }
179  else
180  {
181  std::string status_msg = "cmd#" + convertIntToString(cmd_num) + " of " + "script#" + convertIntToString(action_script_index) + " is invalid.";
182  ROS_ERROR_STREAM(status_msg);
183  g_action_script_data.clear();
184  return false;
185  }
186 
187  g_action_script_data.push_back(temp_cmd);
188  cmd_num++;
189  }
190  } catch (const std::exception& e)
191  {
192  std::string status_msg = "cmd#" + convertIntToString(cmd_num) + " of " + "script#" + convertIntToString(action_script_index) + " is invalid.";
193  ROS_ERROR_STREAM(status_msg);
194  g_action_script_data.clear();
195  return false;
196  }
197 
198  return true;
199 }
200 
201 void actionScriptPlayThreadFunc(int action_script_index)
202 {
203  try
204  {
205  if (action_script_index < 0)
206  {
207  std::string status_msg = "Invalid Action Script Index";
208  ROS_ERROR_STREAM(status_msg);
209  return;
210  }
211 
212  if (isActionRunning() == true)
213  {
214  std::string status_msg = "Previous action playing is not finished.";
215  ROS_ERROR_STREAM(status_msg);
216  return;
217  }
218 
219  if (parseActionScript(action_script_index) == false)
220  return;
221 
222  std_msgs::Int32 action_page_num_msg;
223  std_msgs::String sound_file_name_msg;
224  thormang3_action_module_msgs::StartAction start_action_msg;
225  if (g_joint_name_list.size() != 0)
226  {
227  for (unsigned int joint_name_idx = 0; joint_name_idx < g_joint_name_list.size(); joint_name_idx++)
228  start_action_msg.joint_name_array.push_back(g_joint_name_list[joint_name_idx]);
229  }
230 
231  for(unsigned int action_script_data_idx = 0; action_script_data_idx < g_action_script_data.size(); action_script_data_idx++)
232  {
233  std::string cmd_name = g_action_script_data[action_script_data_idx].cmd_name;
234 
235  boost::this_thread::interruption_point();
236  if (cmd_name == ACTION_PLAY_CMD_NAME)
237  {
238  if (g_joint_name_list.size() != 0)
239  {
240  start_action_msg.page_num = g_action_script_data[action_script_data_idx].cmd_arg_int;
241  g_start_action_pub.publish(start_action_msg);
242  }
243  else
244  {
245  action_page_num_msg.data = g_action_script_data[action_script_data_idx].cmd_arg_int;
246  g_action_page_num_pub.publish(action_page_num_msg);
247  }
248  }
249  else if (cmd_name == MP3_PLAY_CMD_NAME)
250  {
251  sound_file_name_msg.data = g_action_script_data[action_script_data_idx].cmd_arg_str;
252  g_sound_file_name_pub.publish(sound_file_name_msg);
253  }
254  else if (cmd_name == WAIT_ACTION_PLAY_FINISH_CMD_NAME)
255  {
256  while (true)
257  {
258  if (isActionRunning() == false)
259  break;
260 
261  boost::this_thread::sleep(boost::posix_time::milliseconds(32));
262  }
263  }
264  else if (cmd_name == SLEEP_CMD_NAME)
265  {
266  boost::this_thread::sleep(boost::posix_time::milliseconds(g_action_script_data[action_script_data_idx].cmd_arg_int));
267  }
268  else
269  {
270  boost::this_thread::interruption_point();
271  continue;
272  }
273  }
274 
275  } catch (boost::thread_interrupted&)
276  {
277  ROS_INFO("Action Script Thread is Interrupted");
278  return;
279  }
280 }
281 
282 void actionScriptNumberCallback(const std_msgs::Int32::ConstPtr& msg)
283 {
284  if ((msg->data == -1) || (msg->data == -2)) //Stop or Break
285  {
286  std_msgs::Int32 action_page_num_msg;
287  action_page_num_msg.data = msg->data;
288  g_action_page_num_pub.publish(action_page_num_msg);
289 
290 // if((g_action_script_play_thread != 0) && (g_action_script_play_thread->get_thread_info() != 0))
291 // {
292 // if(g_action_script_play_thread->get_thread_info()->done == false)
293 // {
294 // g_action_script_play_thread->interrupt();
295 // g_action_script_play_thread->join();
296 // }
297 // }
298  if ((g_action_script_play_thread != 0))
299  {
300  g_action_script_play_thread->interrupt();
304  }
305  }
306  else
307  {
308  if ((g_action_script_play_thread == 0))
309  {
310  g_action_script_play_thread = new boost::thread(actionScriptPlayThreadFunc, msg->data);
311  }
312 // else if(g_action_script_play_thread->get_thread_info() == 0)
313 // {
314 // g_action_script_play_thread = new boost::thread(actionScriptPlayThreadFunc, msg->data);
315 // }
316  //else if(g_action_script_play_thread->get_thread_info()->done == true)
317  //else if(g_action_script_play_thread->get_id() == false)
318  else if (g_action_script_play_thread->timed_join(boost::posix_time::milliseconds(32)) == true)
319  {
321  g_action_script_play_thread = new boost::thread(actionScriptPlayThreadFunc, msg->data);
322  }
323  else
324  {
325  std::string status_msg = "Previous action script is not finished.";
326  ROS_ERROR_STREAM(status_msg);
327  }
328  }
329 }
330 
331 int main(int argc, char **argv)
332 {
333  ros::init(argc, argv, "thormang3_action_script_player");
334  ros::NodeHandle ros_node_handle;
335 
337 
338  g_action_script_num_sub = ros_node_handle.subscribe("/robotis/demo/action_index", 0, &actionScriptNumberCallback);
339  g_action_page_num_pub = ros_node_handle.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
340  g_start_action_pub = ros_node_handle.advertise<thormang3_action_module_msgs::StartAction>("/robotis/action/start_action", 0);
341  g_sound_file_name_pub = ros_node_handle.advertise<std_msgs::String>("/play_sound_file", 0);
342  g_is_running_client = ros_node_handle.serviceClient<thormang3_action_module_msgs::IsRunning>("/robotis/action/is_running");
343 
344  //Setting action script file path
345  std::string temp_action_script_file_path = ros::package::getPath("thormang3_action_script_player") + "/list/action_script.yaml";
346  if (ros_node_handle.getParam("action_script_file_path", g_action_script_file_path) == false)
347  {
348  g_action_script_file_path = temp_action_script_file_path;
349  ROS_WARN("Failed to get action script_file_path.");
350  ROS_WARN("The default action script file path will be used.");
351  }
352 
353  ROS_INFO("Start ThorMang3 Action Script Player");
354 
355  ros::spin();
356 }
357 
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
#define WAIT_ACTION_PLAY_FINISH_CMD_NAME
void publish(const boost::shared_ptr< M > &message) const
#define JOINT_NAME_KEY
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::ServiceClient g_is_running_client
ros::Subscriber g_action_script_num_sub
void actionScriptPlayThreadFunc(int action_script_index)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
std::string convertIntToString(int n)
#define ROS_WARN(...)
int main(int argc, char **argv)
int convertStringToInt(std::string str)
std::vector< action_script_cmd > g_action_script_data
#define ROS_INFO(...)
ros::Publisher g_start_action_pub
boost::thread * g_action_script_play_thread
void actionScriptNumberCallback(const std_msgs::Int32::ConstPtr &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
bool isActionRunning(void)
ros::Publisher g_action_page_num_pub
#define MP3_PLAY_CMD_NAME
ROSLIB_DECL std::string getPath(const std::string &package_name)
std::vector< std::string > g_joint_name_list
bool getParam(const std::string &key, std::string &s) const
#define SLEEP_CMD_NAME
bool parseActionScript(int action_script_index)
ros::Publisher g_sound_file_name_pub
#define ROS_ERROR_STREAM(args)
std::string g_action_script_file_path
thormang3_action_module_msgs::IsRunning g_is_running_srv
#define ROS_ERROR(...)
#define ACTION_PLAY_CMD_NAME


thormang3_action_script_player
Author(s): Kayman , Jay Song
autogenerated on Mon Jun 10 2019 15:38:26