28 std::ostringstream ostr;
86 for (std::map<std::string, robotis_framework::Dynamixel*>::iterator it = robot->
dxls_.begin();
87 it != robot->
dxls_.end(); it++)
89 std::string joint_name = it->first;
104 std::string action_file_path = ros_node.
param<std::string>(
"action_file_path", path);
133 while (ros_node.
ok())
138 op3_action_module_msgs::IsRunning::Response &res)
148 std::string status_msg =
"Action Module is not enabled";
150 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
158 else if (msg->data == -2)
166 joints_enable_it->second =
true;
168 if (
start(msg->data) ==
true)
172 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
178 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
188 std::string status_msg =
"Action Module is not enabled";
190 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
194 if (msg->page_num == -1)
198 else if (msg->page_num == -2)
206 joints_enable_it->second =
false;
208 int joint_name_array_size = msg->joint_name_array.size();
210 for (
int joint_idx = 0; joint_idx < joint_name_array_size; joint_idx++)
215 std::string status_msg =
"Invalid Joint Name : " + msg->joint_name_array[joint_idx];
217 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
223 joints_enable_it->second =
true;
227 if (
start(msg->page_num) ==
true)
229 std::string status_msg =
"Succeed to start page " +
convertIntToString(msg->page_num);
231 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
237 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
244 std::map<std::string, double> sensors)
251 for (std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.begin(); dxls_it != dxls.end();
254 std::string joint_name = dxls_it->first;
256 std::map<std::string, robotis_framework::DynamixelState*>::iterator result_it =
result_.find(joint_name);
257 if (result_it ==
result_.end())
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_;
273 if (action_enable_it->second ==
true)
274 result_[action_enable_it->first]->goal_position_ =
action_result_[action_enable_it->first]->goal_position_;
284 std::string status_msg =
"Action_Start";
286 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
290 for (std::map<std::string, robotis_framework::DynamixelState*>::iterator action_result_it =
292 action_result_it->second->goal_position_ =
result_[action_result_it->first]->goal_position_;
294 std::string status_msg =
"Action_Finish";
296 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
325 return (
int) ((rad + M_PI) * 2048.0 / M_PI);
330 return (w4095 - 2048) * M_PI / 2048.0;
335 unsigned char checksum = 0x00;
336 unsigned char* pt = (
unsigned char*) page;
344 if (checksum != 0xff)
352 unsigned char checksum = 0x00;
353 unsigned char* pt = (
unsigned char*) page;
368 FILE* action = fopen(file_name.c_str(),
"r+b");
371 std::string status_msg =
"Can not open Action file!";
373 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
377 fseek(action, 0, SEEK_END);
380 std::string status_msg =
"It's not an Action file!";
382 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
396 FILE* action = fopen(file_name.c_str(),
"ab");
399 std::string status_msg =
"Can not create Action file!";
401 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
424 std::string status_msg =
"Can not play page.(" +
convertIntToString(page_number) +
" is invalid index)";
426 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
431 if (
loadPage(page_number, &page) ==
false)
434 return start(page_number, &page);
444 if (
loadPage(index, &page) ==
false)
447 if (strcmp(page_name.c_str(), (
char*) page.
header.
name) == 0)
451 if (index == action_file_define::MAXNUM_PAGE)
453 std::string str_name_page = page_name;
454 std::string status_msg =
"Can not play page.(" + str_name_page +
" is invalid name)\n";
456 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
460 return start(index, &page);
467 std::string status_msg =
"Action Module is disabled";
469 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
475 std::string status_msg =
"Can not play page " +
convertIntToString(page_number) +
".(Now playing)";
477 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
485 std::string status_msg =
"Page " +
convertIntToString(page_number) +
" has no action\n";
487 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
505 if (playing_page_num != 0)
508 if (playing_step_num != 0)
551 unsigned char *pt = (
unsigned char*) page;
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;
602 int16_t prev_target_angle;
603 int16_t curr_target_angle;
604 int16_t next_target_angle;
605 uint8_t direction_changed;
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;
640 for (std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.begin(); dxls_it != dxls.end();
643 std::string joint_name = dxls_it->first;
645 std::map<std::string, robotis_framework::DynamixelState*>::iterator result_it =
action_result_.find(joint_name);
646 if (result_it ==
result_.end())
650 result_it->second->goal_position_ = dxls_it->second->dxl_state_->goal_position_;
672 std::string joint_name =
"";
678 joint_name = id_to_name_it->second;
680 std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.find(joint_name);
681 if (dxls_it == dxls.end())
685 double goal_joint_angle_rad = dxls_it->second->dxl_state_->goal_position_;
687 last_out_speed[id] = 0;
688 moving_angle[id] = 0;
694 if (unit_time_count < unit_time_num)
705 std::string joint_name =
"";
711 joint_name = id_to_name_it->second;
713 std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.find(joint_name);
714 if (dxls_it == dxls.end())
720 if (moving_angle[
id] == 0)
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)
738 start_angle[
id] + (
short int) (((
long) (main_angle[
id]) * unit_time_count) / unit_time_num));
740 goal_speed[id] = main_speed[id];
744 if (unit_time_count == (unit_time_num - 1))
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;
759 + (
short) ((((
long) (last_out_speed[
id] + (speed_n >> 1)) * unit_time_count * 144) / 15)
768 start_angle[
id] + (
short int) (((
long) (main_angle[
id]) * unit_time_count) / unit_time_num));
770 goal_speed[id] = main_speed[id];
779 else if (unit_time_count >= unit_time_num)
786 std::string joint_name =
"";
791 joint_name = id_to_name_it->second;
793 std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.find(joint_name);
794 if (dxls_it == dxls.end())
798 double _goal_joint_angle_rad = dxls_it->second->dxl_state_->goal_position_;
800 last_out_speed[id] = goal_speed[id];
809 unit_time_num = unit_time_total_num - (accel_step << 1);
817 if ((unit_time_total_num - accel_step) == 0)
820 main_angle[id] = (short) ((((
long) (moving_angle[
id] - accel_angle[
id])) * unit_time_num)
821 / (unit_time_total_num - accel_step));
825 main_angle[id] = moving_angle[id] - accel_angle[id]
826 - (
short int) ((((
long) main_speed[id] * accel_step * 12) / 5) >> 8);
833 unit_time_num = accel_step;
838 main_angle[id] = moving_angle[id] - main_angle[id] - accel_angle[id];
847 unit_time_num = pause_time;
862 last_out_speed[id] = 0;
897 if (play_repeat_count > 0)
904 if (next_play_page == 0)
937 curr_target_angle = target_angle[id];
942 start_angle[id] = target_angle[id];
943 prev_target_angle = target_angle[id];
944 target_angle[id] = curr_target_angle;
947 moving_angle[id] = (int) (target_angle[
id] - start_angle[
id]);
953 next_target_angle = curr_target_angle;
957 next_target_angle = curr_target_angle;
965 next_target_angle = curr_target_angle;
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)))
975 direction_changed = 0;
979 direction_changed = 1;
991 if (moving_angle[
id] < 0)
992 tmp = -moving_angle[id];
994 tmp = moving_angle[id];
1009 unit_time_total_num = max_speed;
1011 unit_time_total_num = (max_angle * 40) / (max_speed * 3);
1014 if (unit_time_total_num <= (accel_step << 1))
1016 if (unit_time_total_num == 0)
1022 accel_step = (unit_time_total_num - 1) >> 1;
1023 if (accel_step == 0)
1024 unit_time_total_num = 0;
1028 total_time_256t = ((
unsigned long) unit_time_total_num) << 1;
1029 pre_section_time_256t = ((
unsigned long) accel_step) << 1;
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);
1043 start_speed1024_pre_time_256t = (long) last_out_speed[
id] * pre_section_time_256t;
1044 moving_angle_speed1024_scale_256t_2t = (((long) moving_angle[
id]) * 2560L) / 12;
1047 main_speed[id] = (
short int) ((moving_angle_speed1024_scale_256t_2t - start_speed1024_pre_time_256t)
1050 main_speed[id] = (
short int) ((moving_angle_speed1024_scale_256t_2t - start_speed1024_pre_time_256t)
1053 if (main_speed[
id] > 1023)
1054 main_speed[id] = 1023;
1056 if (main_speed[
id] < -1023)
1057 main_speed[id] = -1023;
1059 unit_time_num = accel_step;
1066 robotis_controller_msgs::StatusMsg status;
1069 status.module_name =
"Action";
1070 status.status_msg = msg;
1077 std_msgs::String done_msg;
1078 done_msg.data = msg;
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_
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)
ControlMode control_mode_
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)
const int SPEED_BASE_SCHEDULE
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 action_module_enabled_
bool param(const std::string ¶m_name, T ¶m_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)
const int NONE_ZERO_FINISH
ros::Publisher done_msg_pub_
DynamixelState * dxl_state_
std::map< std::string, robotis_framework::DynamixelState * > action_result_
std::map< std::string, int > joint_name_to_id_
action_file_define::Page play_page_
const int INVALID_BIT_MASK
#define ROS_ERROR_STREAM(args)
void pageNumberCallback(const std_msgs::Int32::ConstPtr &msg)
bool first_driving_start_
bool savePage(int page_number, action_file_define::Page *page)
const int TIME_BASE_SCHEDULE