action_module.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2017 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 /* Authors: Kayman, Jay Song */
18 
19 #include <stdio.h>
20 #include <sstream>
22 
23 namespace robotis_op
24 {
25 
27 {
28  std::ostringstream ostr;
29  ostr << n;
30  return ostr.str();
31 }
32 
35  PRE_SECTION(0),
36  MAIN_SECTION(1),
37  POST_SECTION(2),
38  PAUSE_SECTION(3),
39  ZERO_FINISH(0),
41  DEBUG_PRINT(false)
42 {
44  /**************************************
45  * Section /----\
46  * /| |\
47  * /+---------/ | | \
48  * / | | | | \
49  * -----/ | | | | \----
50  * PRE MAIN PRE MAIN POST PAUSE
51  ***************************************/
52 
53  enable_ = false;
54  module_name_ = "action_module"; // set unique module name
56 
58  action_file_ = 0;
59  playing_ = false;
60  first_driving_start_ = false;
61  playing_finished_ = true;
62  page_step_count_ = 0;
63  play_page_idx_ = 0;
64  stop_playing_ = true;
65 
66  action_module_enabled_ = false;
67  previous_running_ = false;
68  present_running_ = false;
69 }
70 
72 {
73  queue_thread_.join();
74 
76  if (action_file_ != 0)
77  fclose(action_file_);
78 }
79 
80 void ActionModule::initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
81 {
82  control_cycle_msec_ = control_cycle_msec;
83  queue_thread_ = boost::thread(boost::bind(&ActionModule::queueThread, this));
84 
85  // init result, joint_id_table
86  for (std::map<std::string, robotis_framework::Dynamixel*>::iterator it = robot->dxls_.begin();
87  it != robot->dxls_.end(); it++)
88  {
89  std::string joint_name = it->first;
90  robotis_framework::Dynamixel* dxl_info = it->second;
91 
92  joint_name_to_id_[joint_name] = dxl_info->id_;
93  joint_id_to_name_[dxl_info->id_] = joint_name;
95  action_result_[joint_name]->goal_position_ = dxl_info->dxl_state_->goal_position_;
96  result_[joint_name] = new robotis_framework::DynamixelState();
97  result_[joint_name]->goal_position_ = dxl_info->dxl_state_->goal_position_;
98  action_joints_enable_[joint_name] = false;
99  }
100 
101  ros::NodeHandle ros_node;
102 
103  std::string path = ros::package::getPath("op3_action_module") + "/data/motion_4095.bin";
104  std::string action_file_path = ros_node.param<std::string>("action_file_path", path);
105 
106  loadFile(action_file_path);
107 
108  playing_ = false;
109 }
110 
112 {
113  ros::NodeHandle ros_node;
114  ros::CallbackQueue callback_queue;
115 
116  ros_node.setCallbackQueue(&callback_queue);
117 
118  /* publisher */
119  status_msg_pub_ = ros_node.advertise<robotis_controller_msgs::StatusMsg>("/robotis/status", 0);
120  done_msg_pub_ = ros_node.advertise<std_msgs::String>("/robotis/movement_done", 1);
121 
122  /* subscriber */
123  ros::Subscriber action_page_sub = ros_node.subscribe("/robotis/action/page_num", 0, &ActionModule::pageNumberCallback,
124  this);
125  ros::Subscriber start_action_sub = ros_node.subscribe("/robotis/action/start_action", 0,
127 
128  /* ROS Service Callback Functions */
129  ros::ServiceServer is_running_server = ros_node.advertiseService("/robotis/action/is_running",
131 
132  ros::WallDuration duration(control_cycle_msec_ / 1000.0);
133  while (ros_node.ok())
134  callback_queue.callAvailable(duration);
135 }
136 
137 bool ActionModule::isRunningServiceCallback(op3_action_module_msgs::IsRunning::Request &req,
138  op3_action_module_msgs::IsRunning::Response &res)
139 {
140  res.is_running = isRunning();
141  return true;
142 }
143 
144 void ActionModule::pageNumberCallback(const std_msgs::Int32::ConstPtr& msg)
145 {
146  if (enable_ == false)
147  {
148  std::string status_msg = "Action Module is not enabled";
149  ROS_INFO_STREAM(status_msg);
150  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
151  return;
152  }
153 
154  if (msg->data == -1)
155  {
156  stop();
157  }
158  else if (msg->data == -2)
159  {
160  brake();
161  }
162  else
163  {
164  for (std::map<std::string, bool>::iterator joints_enable_it = action_joints_enable_.begin();
165  joints_enable_it != action_joints_enable_.end(); joints_enable_it++)
166  joints_enable_it->second = true;
167 
168  if (start(msg->data) == true)
169  {
170  std::string status_msg = "Succeed to start page " + convertIntToString(msg->data);
171  ROS_INFO_STREAM(status_msg);
172  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
173  }
174  else
175  {
176  std::string status_msg = "Failed to start page " + convertIntToString(msg->data);
177  ROS_ERROR_STREAM(status_msg);
178  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
179  publishDoneMsg("action_failed");
180  }
181  }
182 }
183 
184 void ActionModule::startActionCallback(const op3_action_module_msgs::StartAction::ConstPtr& msg)
185 {
186  if (enable_ == false)
187  {
188  std::string status_msg = "Action Module is not enabled";
189  ROS_INFO_STREAM(status_msg);
190  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
191  return;
192  }
193 
194  if (msg->page_num == -1)
195  {
196  stop();
197  }
198  else if (msg->page_num == -2)
199  {
200  brake();
201  }
202  else
203  {
204  for (std::map<std::string, bool>::iterator joints_enable_it = action_joints_enable_.begin();
205  joints_enable_it != action_joints_enable_.end(); joints_enable_it++)
206  joints_enable_it->second = false;
207 
208  int joint_name_array_size = msg->joint_name_array.size();
209  std::map<std::string, bool>::iterator joints_enable_it = action_joints_enable_.begin();
210  for (int joint_idx = 0; joint_idx < joint_name_array_size; joint_idx++)
211  {
212  joints_enable_it = action_joints_enable_.find(msg->joint_name_array[joint_idx]);
213  if (joints_enable_it == action_joints_enable_.end())
214  {
215  std::string status_msg = "Invalid Joint Name : " + msg->joint_name_array[joint_idx];
216  ROS_INFO_STREAM(status_msg);
217  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
218  publishDoneMsg("action_failed");
219  return;
220  }
221  else
222  {
223  joints_enable_it->second = true;
224  }
225  }
226 
227  if (start(msg->page_num) == true)
228  {
229  std::string status_msg = "Succeed to start page " + convertIntToString(msg->page_num);
230  ROS_INFO_STREAM(status_msg);
231  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
232  }
233  else
234  {
235  std::string status_msg = "Failed to start page " + convertIntToString(msg->page_num);
236  ROS_ERROR_STREAM(status_msg);
237  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
238  publishDoneMsg("action_failed");
239  }
240  }
241 }
242 
243 void ActionModule::process(std::map<std::string, robotis_framework::Dynamixel *> dxls,
244  std::map<std::string, double> sensors)
245 {
246  if (enable_ == false)
247  return;
248 
249  if (action_module_enabled_ == true)
250  {
251  for (std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.begin(); dxls_it != dxls.end();
252  dxls_it++)
253  {
254  std::string joint_name = dxls_it->first;
255 
256  std::map<std::string, robotis_framework::DynamixelState*>::iterator result_it = result_.find(joint_name);
257  if (result_it == result_.end())
258  continue;
259  else
260  {
261  result_it->second->goal_position_ = dxls_it->second->dxl_state_->goal_position_;
262  action_result_[joint_name]->goal_position_ = dxls_it->second->dxl_state_->goal_position_;
263  }
264  }
265  action_module_enabled_ = false;
266  }
267 
268  actionPlayProcess(dxls);
269 
270  for (std::map<std::string, bool>::iterator action_enable_it = action_joints_enable_.begin();
271  action_enable_it != action_joints_enable_.end(); action_enable_it++)
272  {
273  if (action_enable_it->second == true)
274  result_[action_enable_it->first]->goal_position_ = action_result_[action_enable_it->first]->goal_position_;
275  }
276 
279 
281  {
282  if (present_running_ == true)
283  {
284  std::string status_msg = "Action_Start";
285  //ROS_INFO_STREAM(status_msg);
286  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
287  }
288  else
289  {
290  for (std::map<std::string, robotis_framework::DynamixelState*>::iterator action_result_it =
291  action_result_.begin(); action_result_it != action_result_.end(); action_result_it++)
292  action_result_it->second->goal_position_ = result_[action_result_it->first]->goal_position_;
293 
294  std::string status_msg = "Action_Finish";
295  //ROS_INFO_STREAM(status_msg);
296  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
297  publishDoneMsg("action");
298  }
299  }
300 }
301 
303 {
304  action_module_enabled_ = true;
305 }
306 
308 {
309  action_module_enabled_ = false;
310  brake();
311 }
312 
314 {
315  stop_playing_ = true;
316 }
317 
319 {
320  return playing_;
321 }
322 
324 {
325  return (int) ((rad + M_PI) * 2048.0 / M_PI);
326 }
327 
329 {
330  return (w4095 - 2048) * M_PI / 2048.0;
331 }
332 
334 {
335  unsigned char checksum = 0x00;
336  unsigned char* pt = (unsigned char*) page;
337 
338  for (unsigned int i = 0; i < sizeof(action_file_define::Page); i++)
339  {
340  checksum += *pt;
341  pt++;
342  }
343 
344  if (checksum != 0xff)
345  return false;
346 
347  return true;
348 }
349 
351 {
352  unsigned char checksum = 0x00;
353  unsigned char* pt = (unsigned char*) page;
354 
355  page->header.checksum = 0x00;
356 
357  for (unsigned int i = 0; i < sizeof(action_file_define::Page); i++)
358  {
359  checksum += *pt;
360  pt++;
361  }
362 
363  page->header.checksum = (unsigned char) (0xff - checksum);
364 }
365 
366 bool ActionModule::loadFile(std::string file_name)
367 {
368  FILE* action = fopen(file_name.c_str(), "r+b");
369  if (action == 0)
370  {
371  std::string status_msg = "Can not open Action file!";
372  ROS_ERROR_STREAM(status_msg);
373  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
374  return false;
375  }
376 
377  fseek(action, 0, SEEK_END);
378  if (ftell(action) != (long) (sizeof(action_file_define::Page) * action_file_define::MAXNUM_PAGE))
379  {
380  std::string status_msg = "It's not an Action file!";
381  ROS_ERROR_STREAM(status_msg);
382  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
383  fclose(action);
384  return false;
385  }
386 
387  if (action_file_ != 0)
388  fclose(action_file_);
389 
390  action_file_ = action;
391  return true;
392 }
393 
394 bool ActionModule::createFile(std::string file_name)
395 {
396  FILE* action = fopen(file_name.c_str(), "ab");
397  if (action == 0)
398  {
399  std::string status_msg = "Can not create Action file!";
400  ROS_ERROR_STREAM(status_msg);
401  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
402  return false;
403  }
404 
406  resetPage(&page);
407 
408  for (int i = 0; i < action_file_define::MAXNUM_PAGE; i++)
409  fwrite((const void *) &page, 1, sizeof(action_file_define::Page), action);
410 
411  if (action_file_ != 0)
412  fclose(action_file_);
413 
414  action_file_ = action;
415 
416  return true;
417 }
418 
419 bool ActionModule::start(int page_number)
420 {
421  if (page_number < 1 || page_number >= action_file_define::MAXNUM_PAGE)
422  {
423 
424  std::string status_msg = "Can not play page.(" + convertIntToString(page_number) + " is invalid index)";
425  ROS_ERROR_STREAM(status_msg);
426  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
427  return false;
428  }
429 
431  if (loadPage(page_number, &page) == false)
432  return false;
433 
434  return start(page_number, &page);
435 }
436 
437 bool ActionModule::start(std::string page_name)
438 {
439  int index;
441 
442  for (index = 1; index < action_file_define::MAXNUM_PAGE; index++)
443  {
444  if (loadPage(index, &page) == false)
445  return false;
446 
447  if (strcmp(page_name.c_str(), (char*) page.header.name) == 0)
448  break;
449  }
450 
451  if (index == action_file_define::MAXNUM_PAGE)
452  {
453  std::string str_name_page = page_name;
454  std::string status_msg = "Can not play page.(" + str_name_page + " is invalid name)\n";
455  ROS_ERROR_STREAM(status_msg);
456  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
457  return false;
458  }
459  else
460  return start(index, &page);
461 }
462 
463 bool ActionModule::start(int page_number, action_file_define::Page* page)
464 {
465  if (enable_ == false)
466  {
467  std::string status_msg = "Action Module is disabled";
468  ROS_ERROR_STREAM(status_msg);
469  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
470  return false;
471  }
472 
473  if (playing_ == true)
474  {
475  std::string status_msg = "Can not play page " + convertIntToString(page_number) + ".(Now playing)";
476  ROS_ERROR_STREAM(status_msg);
477  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
478  return false;
479  }
480 
481  play_page_ = *page;
482 
483  if (play_page_.header.repeat == 0 || play_page_.header.stepnum == 0)
484  {
485  std::string status_msg = "Page " + convertIntToString(page_number) + " has no action\n";
486  ROS_ERROR_STREAM(status_msg);
487  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
488  return false;
489  }
490 
491  play_page_idx_ = page_number;
492  first_driving_start_ = true;
493  playing_ = true;
494 
495  return true;
496 }
497 
499 {
500  playing_ = false;
501 }
502 
503 bool ActionModule::isRunning(int* playing_page_num, int* playing_step_num)
504 {
505  if (playing_page_num != 0)
506  *playing_page_num = play_page_idx_;
507 
508  if (playing_step_num != 0)
509  *playing_step_num = page_step_count_ - 1;
510 
511  return isRunning();
512 }
513 
515 {
516  if (page_number < 0 || page_number >= action_file_define::MAXNUM_PAGE)
517  return false;
518 
519  long position = (long) (sizeof(action_file_define::Page) * page_number);
520 
521  if (fseek(action_file_, position, SEEK_SET) != 0)
522  return false;
523 
524  if (fread(page, 1, sizeof(action_file_define::Page), action_file_) != sizeof(action_file_define::Page))
525  return false;
526 
527  if (verifyChecksum(page) == false)
528  resetPage(page);
529 
530  return true;
531 }
532 
534 {
535  long position = (long) (sizeof(action_file_define::Page) * page_number);
536 
537  if (verifyChecksum(page) == false)
538  setChecksum(page);
539 
540  if (fseek(action_file_, position, SEEK_SET) != 0)
541  return false;
542 
543  if (fwrite(page, 1, sizeof(action_file_define::Page), action_file_) != sizeof(action_file_define::Page))
544  return false;
545 
546  return true;
547 }
548 
550 {
551  unsigned char *pt = (unsigned char*) page;
552 
553  for (unsigned int i = 0; i < sizeof(action_file_define::Page); i++)
554  {
555  *pt = 0x00;
556  pt++;
557  }
558 
559  page->header.schedule = action_file_define::TIME_BASE_SCHEDULE; // default time base
560  page->header.repeat = 1;
561  page->header.speed = 32;
562  page->header.accel = 32;
563 
564  for (int i = 0; i < action_file_define::MAXNUM_JOINTS; i++)
565  page->header.pgain[i] = 0x55;
566 
567  for (int i = 0; i < action_file_define::MAXNUM_STEP; i++)
568  {
569  for (int j = 0; j < action_file_define::MAXNUM_JOINTS; j++)
571 
572  page->step[i].pause = 0;
573  page->step[i].time = 0;
574  }
575 
576  setChecksum(page);
577 }
578 
580 {
581  for (std::map<std::string, bool>::iterator it = action_joints_enable_.begin(); it != action_joints_enable_.end();
582  it++)
583  {
584  it->second = true;
585  }
586 }
587 
588 void ActionModule::actionPlayProcess(std::map<std::string, robotis_framework::Dynamixel *> dxls)
589 {
591  uint8_t id;
592  uint32_t total_time_256t;
593  uint32_t pre_section_time_256t;
594  uint32_t main_time_256t;
595  int32_t start_speed1024_pre_time_256t;
596  int32_t moving_angle_speed1024_scale_256t_2t;
597  int32_t divider1, divider2;
598 
599  int16_t max_angle;
600  int16_t max_speed;
601  int16_t tmp;
602  int16_t prev_target_angle; // Start position
603  int16_t curr_target_angle; // Target position
604  int16_t next_target_angle; // Next target position
605  uint8_t direction_changed;
606  int16_t speed_n;
607 
609  static uint16_t start_angle[action_file_define::MAXNUM_JOINTS]; // Start point of interpolation
610  static uint16_t target_angle[action_file_define::MAXNUM_JOINTS]; // Target point of interpolation
611  static int16_t moving_angle[action_file_define::MAXNUM_JOINTS]; // Total Moving Angle
612  static int16_t main_angle[action_file_define::MAXNUM_JOINTS]; // Moving angle at Constant Velocity Section
613  static int16_t accel_angle[action_file_define::MAXNUM_JOINTS]; // Moving angle at Acceleration Section
614  static int16_t main_speed[action_file_define::MAXNUM_JOINTS]; // Target constant velocity
615  static int16_t last_out_speed[action_file_define::MAXNUM_JOINTS]; // Velocity of Previous State
616  static int16_t goal_speed[action_file_define::MAXNUM_JOINTS]; // Target velocity
617  static uint8_t finish_type[action_file_define::MAXNUM_JOINTS]; // Desired State at Target angle
618 
619  static uint16_t unit_time_count;
620  static uint16_t unit_time_num;
621  static uint16_t pause_time;
622  static uint16_t unit_time_total_num;
623  static uint16_t accel_step;
624  static uint8_t section;
625  static uint8_t play_repeat_count;
626  static uint16_t next_play_page;
627 
629  /**************************************
630  * Section /----\
631  * /| |\
632  * /+---------/ | | \
633  * / | | | | \
634  * -----/ | | | | \----
635  * PRE MAIN PRE MAIN POST PAUSE
636  ***************************************/
637 
638  if (playing_ == false)
639  {
640  for (std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.begin(); dxls_it != dxls.end();
641  dxls_it++)
642  {
643  std::string joint_name = dxls_it->first;
644 
645  std::map<std::string, robotis_framework::DynamixelState*>::iterator result_it = action_result_.find(joint_name);
646  if (result_it == result_.end())
647  continue;
648  else
649  {
650  result_it->second->goal_position_ = dxls_it->second->dxl_state_->goal_position_;
651  }
652  }
653  return;
654  }
655 
656  if (first_driving_start_ == true) // First start
657  {
658  first_driving_start_ = false; //First Process end
659  playing_finished_ = false;
660  stop_playing_ = false;
661  unit_time_count = 0;
662  unit_time_num = 0;
663  pause_time = 0;
664  section = PAUSE_SECTION;
665  page_step_count_ = 0;
666  play_repeat_count = play_page_.header.repeat;
667  next_play_page = 0;
668 
669  for (unsigned int joint_index = 0; joint_index < action_file_define::MAXNUM_JOINTS; joint_index++)
670  {
671  id = joint_index;
672  std::string joint_name = "";
673 
674  std::map<int, std::string>::iterator id_to_name_it = joint_id_to_name_.find(id);
675  if (id_to_name_it == joint_id_to_name_.end())
676  continue;
677  else
678  joint_name = id_to_name_it->second;
679 
680  std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.find(joint_name);
681  if (dxls_it == dxls.end())
682  continue;
683  else
684  {
685  double goal_joint_angle_rad = dxls_it->second->dxl_state_->goal_position_;
686  target_angle[id] = convertRadTow4095(goal_joint_angle_rad);
687  last_out_speed[id] = 0;
688  moving_angle[id] = 0;
689  goal_speed[id] = 0;
690  }
691  }
692  }
693 
694  if (unit_time_count < unit_time_num) // Ongoing
695  {
696  unit_time_count++;
697  if (section == PAUSE_SECTION)
698  {
699  }
700  else
701  {
702  for (unsigned int joint_index = 0; joint_index < action_file_define::MAXNUM_JOINTS; joint_index++)
703  {
704  id = joint_index;
705  std::string joint_name = "";
706 
707  std::map<int, std::string>::iterator id_to_name_it = joint_id_to_name_.find(id);
708  if (id_to_name_it == joint_id_to_name_.end())
709  continue;
710  else
711  joint_name = id_to_name_it->second;
712 
713  std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.find(joint_name);
714  if (dxls_it == dxls.end())
715  {
716  continue;
717  }
718  else
719  {
720  if (moving_angle[id] == 0)
721  {
722  action_result_[joint_name]->goal_position_ = convertw4095ToRad(start_angle[id]);
723  }
724  else
725  {
726  if (section == PRE_SECTION)
727  {
728  speed_n = (short) (((long) (main_speed[id] - last_out_speed[id]) * unit_time_count) / unit_time_num);
729  goal_speed[id] = last_out_speed[id] + speed_n;
730  accel_angle[id] = (short) ((((long) (last_out_speed[id] + (speed_n >> 1)) * unit_time_count * 144) / 15)
731  >> 9);
732 
733  action_result_[joint_name]->goal_position_ = convertw4095ToRad(start_angle[id] + accel_angle[id]);
734  }
735  else if (section == MAIN_SECTION)
736  {
737  action_result_[joint_name]->goal_position_ = convertw4095ToRad(
738  start_angle[id] + (short int) (((long) (main_angle[id]) * unit_time_count) / unit_time_num));
739 
740  goal_speed[id] = main_speed[id];
741  }
742  else // POST_SECTION
743  {
744  if (unit_time_count == (unit_time_num - 1))
745  {
746  // use target angle in order to reduce the last step error
747  action_result_[joint_name]->goal_position_ = convertw4095ToRad(target_angle[id]);
748  }
749  else
750  {
751  if (finish_type[id] == ZERO_FINISH)
752  {
753  speed_n = (short int) (((long) (0 - last_out_speed[id]) * unit_time_count) / unit_time_num);
754  goal_speed[id] = last_out_speed[id] + speed_n;
755 
756  action_result_[joint_name]->goal_position_ =
758  start_angle[id]
759  + (short) ((((long) (last_out_speed[id] + (speed_n >> 1)) * unit_time_count * 144) / 15)
760  >> 9));
761 
762  }
763  else // NONE_ZERO_FINISH
764  {
765  // Same as MAIN Section
766  // because some servos need to be rotate, others do not.
767  action_result_[joint_name]->goal_position_ = convertw4095ToRad(
768  start_angle[id] + (short int) (((long) (main_angle[id]) * unit_time_count) / unit_time_num));
769 
770  goal_speed[id] = main_speed[id];
771  }
772  }
773  }
774  }
775  }
776  }
777  }
778  }
779  else if (unit_time_count >= unit_time_num) // If current section is completed
780  {
781  unit_time_count = 0;
782 
783  for (unsigned int joint_index = 0; joint_index < action_file_define::MAXNUM_JOINTS; joint_index++)
784  {
785  id = joint_index;
786  std::string joint_name = "";
787  std::map<int, std::string>::iterator id_to_name_it = joint_id_to_name_.find(id);
788  if (id_to_name_it == joint_id_to_name_.end())
789  continue;
790  else
791  joint_name = id_to_name_it->second;
792 
793  std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.find(joint_name);
794  if (dxls_it == dxls.end())
795  continue;
796  else
797  {
798  double _goal_joint_angle_rad = dxls_it->second->dxl_state_->goal_position_;
799  start_angle[id] = convertRadTow4095(_goal_joint_angle_rad);
800  last_out_speed[id] = goal_speed[id];
801  }
802  }
803 
804  // Update section ( PRE -> MAIN -> POST -> (PAUSE or PRE) ... )
805  if (section == PRE_SECTION)
806  {
807  // Prepare for MAIN Section
808  section = MAIN_SECTION;
809  unit_time_num = unit_time_total_num - (accel_step << 1);
810 
811  for (unsigned int joint_index = 0; joint_index < action_file_define::MAXNUM_JOINTS; joint_index++)
812  {
813  id = joint_index;
814 
815  if (finish_type[id] == NONE_ZERO_FINISH)
816  {
817  if ((unit_time_total_num - accel_step) == 0) // if there is not any constant velocity section
818  main_angle[id] = 0;
819  else
820  main_angle[id] = (short) ((((long) (moving_angle[id] - accel_angle[id])) * unit_time_num)
821  / (unit_time_total_num - accel_step));
822  }
823  else
824  // ZERO_FINISH
825  main_angle[id] = moving_angle[id] - accel_angle[id]
826  - (short int) ((((long) main_speed[id] * accel_step * 12) / 5) >> 8);
827  }
828  }
829  else if (section == MAIN_SECTION)
830  {
831  //preparations for POST Section
832  section = POST_SECTION;
833  unit_time_num = accel_step;
834 
835  for (unsigned int joint_index = 0; joint_index < action_file_define::MAXNUM_JOINTS; joint_index++)
836  {
837  id = joint_index;
838  main_angle[id] = moving_angle[id] - main_angle[id] - accel_angle[id];
839  }
840  }
841  else if (section == POST_SECTION)
842  {
843  //it will be decided by Pause time exist or not
844  if (pause_time)
845  {
846  section = PAUSE_SECTION;
847  unit_time_num = pause_time;
848  }
849  else
850  {
851  section = PRE_SECTION;
852  }
853  }
854  else if (section == PAUSE_SECTION)
855  {
856  //preparations for PRE Section
857  section = PRE_SECTION;
858 
859  for (unsigned int joint_index = 0; joint_index < action_file_define::MAXNUM_JOINTS; joint_index++)
860  {
861  id = joint_index;
862  last_out_speed[id] = 0;
863  }
864  }
865 
866  // Ready for all in PRE Section
867  if (section == PRE_SECTION)
868  {
869  if (playing_finished_ == true) // If motion is finished
870  {
871  playing_ = false;
872  return;
873  }
874 
876 
877  if (page_step_count_ > play_page_.header.stepnum) // If motion playing of present page is finished
878  {
879  // copy next page
881  if (play_page_idx_ != next_play_page)
882  play_repeat_count = play_page_.header.repeat;
883  page_step_count_ = 1;
884  play_page_idx_ = next_play_page;
885  }
886 
887  if (page_step_count_ == play_page_.header.stepnum) // If this is last step
888  {
889  // load next page
890  if (stop_playing_ == true) // STOP command
891  {
892  next_play_page = play_page_.header.exit; // Go to Exit page
893  }
894  else
895  {
896  play_repeat_count--;
897  if (play_repeat_count > 0) // if repeat count is remained
898  next_play_page = play_page_idx_; // Set next page to present page
899  else
900  // Complete repeat
901  next_play_page = play_page_.header.next; // set next page
902  }
903 
904  if (next_play_page == 0) // If there is no NEXT page, the motion playing will be finished after current step.
905  playing_finished_ = true;
906  else
907  {
908  // load next page
909  if (play_page_idx_ != next_play_page)
910  loadPage(next_play_page, &next_play_page_);
911  else
913 
914  // If there is no playing information, the motion playing will be finished after current step.
916  playing_finished_ = true;
917  }
918  }
919 
921  pause_time = (((unsigned short) play_page_.step[page_step_count_ - 1].pause) << 5) / play_page_.header.speed;
922  max_speed = ((unsigned short) play_page_.step[page_step_count_ - 1].time
923  * (unsigned short) play_page_.header.speed) >> 5;
924  if (max_speed == 0)
925  max_speed = 1;
926  max_angle = 0;
927 
929  for (unsigned int joint_index = 0; joint_index < action_file_define::MAXNUM_JOINTS; joint_index++)
930  {
931  id = joint_index;
932  //Calculate the trajectory using previous, present and future
933  accel_angle[id] = 0;
934 
935  // Find current target angle
937  curr_target_angle = target_angle[id];
938  else
939  curr_target_angle = play_page_.step[page_step_count_ - 1].position[id];
940 
941  // Update start, prev_target, curr_target
942  start_angle[id] = target_angle[id];
943  prev_target_angle = target_angle[id];
944  target_angle[id] = curr_target_angle;
945 
946  // Find Moving offset
947  moving_angle[id] = (int) (target_angle[id] - start_angle[id]);
948 
949  // Find Next target angle
950  if (page_step_count_ == play_page_.header.stepnum) // If current step is last step
951  {
952  if (playing_finished_ == true) // If it will be finished
953  next_target_angle = curr_target_angle;
954  else
955  {
956  if (next_play_page_.step[0].position[id] & action_file_define::INVALID_BIT_MASK)
957  next_target_angle = curr_target_angle;
958  else
959  next_target_angle = next_play_page_.step[0].position[id];
960  }
961  }
962  else
963  {
964  if (play_page_.step[page_step_count_].position[id] & action_file_define::INVALID_BIT_MASK)
965  next_target_angle = curr_target_angle;
966  else
967  next_target_angle = play_page_.step[page_step_count_].position[id];
968  }
969 
970  // Find direction change
971  if (((prev_target_angle < curr_target_angle) && (curr_target_angle < next_target_angle))
972  || ((prev_target_angle > curr_target_angle) && (curr_target_angle > next_target_angle)))
973  {
974  // same direction
975  direction_changed = 0;
976  }
977  else
978  {
979  direction_changed = 1;
980  }
981 
982  // Find finish type
983  if (direction_changed || pause_time || playing_finished_ == true)
984  finish_type[id] = ZERO_FINISH;
985  else
986  finish_type[id] = NONE_ZERO_FINISH;
987 
989  {
990  //MaxAngle1024 update
991  if (moving_angle[id] < 0)
992  tmp = -moving_angle[id];
993  else
994  tmp = moving_angle[id];
995 
996  if (tmp > max_angle)
997  max_angle = tmp;
998  }
999 
1000  }
1001 
1002  // calculation the time. And, the calculated time will be divided by 7.8msec(<<7)- calculate there are how many 7.8msec
1003  // after unit conversion, calculate angle/velocity, and the following code computes how many units of 7.8s occurs within the specified time
1004  // unit conversion --- angle :1024->300deg, velocity: 256 ->720
1005  // wUnitTimeNum = ((wMaxAngle1024*300/1024) /(wMaxSpeed256 * 720/256)) /7.8msec;
1006  // = ((128*wMaxAngle1024*300/1024) /(wMaxSpeed256 * 720/256)) ; (/7.8msec == *128)
1007  // = (wMaxAngle1024*40) /(wMaxSpeed256 *3);
1009  unit_time_total_num = max_speed; //TIME BASE 051025
1010  else
1011  unit_time_total_num = (max_angle * 40) / (max_speed * 3);
1012 
1013  accel_step = play_page_.header.accel;
1014  if (unit_time_total_num <= (accel_step << 1))
1015  {
1016  if (unit_time_total_num == 0)
1017  {
1018  accel_step = 0;
1019  }
1020  else
1021  {
1022  accel_step = (unit_time_total_num - 1) >> 1;
1023  if (accel_step == 0)
1024  unit_time_total_num = 0; // Acceleration and constant velocity steps have to be more than one in order to move
1025  }
1026  }
1027 
1028  total_time_256t = ((unsigned long) unit_time_total_num) << 1; // /128 * 256
1029  pre_section_time_256t = ((unsigned long) accel_step) << 1; // /128 * 256
1030  main_time_256t = total_time_256t - pre_section_time_256t;
1031  divider1 = pre_section_time_256t + (main_time_256t << 1);
1032  divider2 = (main_time_256t << 1);
1033 
1034  if (divider1 == 0)
1035  divider1 = 1;
1036 
1037  if (divider2 == 0)
1038  divider2 = 1;
1039 
1040  for (unsigned int joint_index = 0; joint_index < action_file_define::MAXNUM_JOINTS; joint_index++)
1041  {
1042  id = joint_index;
1043  start_speed1024_pre_time_256t = (long) last_out_speed[id] * pre_section_time_256t; // *300/1024 * 1024/720 * 256 * 2
1044  moving_angle_speed1024_scale_256t_2t = (((long) moving_angle[id]) * 2560L) / 12;
1045 
1046  if (finish_type[id] == ZERO_FINISH)
1047  main_speed[id] = (short int) ((moving_angle_speed1024_scale_256t_2t - start_speed1024_pre_time_256t)
1048  / divider2);
1049  else
1050  main_speed[id] = (short int) ((moving_angle_speed1024_scale_256t_2t - start_speed1024_pre_time_256t)
1051  / divider1);
1052 
1053  if (main_speed[id] > 1023)
1054  main_speed[id] = 1023;
1055 
1056  if (main_speed[id] < -1023)
1057  main_speed[id] = -1023;
1058  }
1059  unit_time_num = accel_step; //PreSection
1060  }
1061  }
1062 }
1063 
1064 void ActionModule::publishStatusMsg(unsigned int type, std::string msg)
1065 {
1066  robotis_controller_msgs::StatusMsg status;
1067  status.header.stamp = ros::Time::now();
1068  status.type = type;
1069  status.module_name = "Action";
1070  status.status_msg = msg;
1071 
1072  status_msg_pub_.publish(status);
1073 }
1074 
1075 void ActionModule::publishDoneMsg(std::string msg)
1076 {
1077  std_msgs::String done_msg;
1078  done_msg.data = msg;
1079 
1080  done_msg_pub_.publish(done_msg);
1081 }
1082 }
double convertw4095ToRad(int w4095)
ros::Publisher status_msg_pub_
bool loadFile(std::string file_name)
std::string convertIntToString(int n)
std::map< std::string, DynamixelState * > result_
std::map< int, std::string > joint_id_to_name_
boost::thread queue_thread_
void publish(const boost::shared_ptr< M > &message) const
void resetPage(action_file_define::Page *page)
std::map< std::string, bool > action_joints_enable_
Definition: action_module.h:99
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void actionPlayProcess(std::map< std::string, robotis_framework::Dynamixel * > dxls)
void setChecksum(action_file_define::Page *page)
bool createFile(std::string file_name)
unsigned short position[MAXNUM_JOINTS]
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
bool verifyChecksum(action_file_define::Page *page)
std::map< std::string, Dynamixel * > dxls_
bool start(int page_number)
void publishStatusMsg(unsigned int type, std::string msg)
action_file_define::Page next_play_page_
void startActionCallback(const op3_action_module_msgs::StartAction::ConstPtr &msg)
void publishDoneMsg(std::string msg)
bool isRunningServiceCallback(op3_action_module_msgs::IsRunning::Request &req, op3_action_module_msgs::IsRunning::Response &res)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void setCallbackQueue(CallbackQueueInterface *queue)
int convertRadTow4095(double rad)
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool loadPage(int page_number, action_file_define::Page *page)
ROSLIB_DECL std::string getPath(const std::string &package_name)
#define ROS_INFO_STREAM(args)
ros::Publisher done_msg_pub_
DynamixelState * dxl_state_
std::map< std::string, robotis_framework::DynamixelState * > action_result_
static Time now()
std::map< std::string, int > joint_name_to_id_
action_file_define::Page play_page_
bool ok() const
#define ROS_ERROR_STREAM(args)
void pageNumberCallback(const std_msgs::Int32::ConstPtr &msg)
bool savePage(int page_number, action_file_define::Page *page)


op3_action_module
Author(s): Kayman , Jay Song
autogenerated on Mon Jun 10 2019 14:41:06