32 std::ostringstream ostr;
89 for(std::map<std::string, robotis_framework::Dynamixel*>::iterator it = robot->
dxls_.begin(); it != robot->
dxls_.end(); it++)
91 std::string joint_name = it->first;
106 std::string action_file_path = ros_node.
param<std::string>(
"action_file_path", path);
137 thormang3_action_module_msgs::IsRunning::Response &res)
147 std::string status_msg =
"Action Module is not enabled";
149 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
157 else if(msg->data == -2)
164 joints_enable_it->second =
true;
166 if(
start(msg->data) ==
true)
170 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
176 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
187 std::string status_msg =
"Action Module is not enabled";
189 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
193 if(msg->page_num == -1)
197 else if(msg->page_num == -2)
204 joints_enable_it->second =
false;
206 int joint_name_array_size = msg->joint_name_array.size();
208 for(
int joint_idx = 0; joint_idx < joint_name_array_size; joint_idx++)
213 std::string status_msg =
"Invalid Joint Name : " + msg->joint_name_array[joint_idx];
215 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
221 joints_enable_it->second =
true;
225 if(
start(msg->page_num) ==
true)
227 std::string status_msg =
"Succeed to start page " +
convertIntToString(msg->page_num);
229 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
235 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
241 void ActionModule::process(std::map<std::string, robotis_framework::Dynamixel *> dxls, std::map<std::string, double> sensors)
248 for(std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.begin() ; dxls_it != dxls.end(); dxls_it++)
250 std::string joint_name = dxls_it->first;
252 std::map<std::string, robotis_framework::DynamixelState*>::iterator result_it =
result_.find(joint_name);
257 result_it->second->goal_position_ = dxls_it->second->dxl_state_->goal_position_;
258 action_result_[joint_name]->goal_position_ = dxls_it->second->dxl_state_->goal_position_;
268 if(action_enable_it->second ==
true)
269 result_[action_enable_it->first]->goal_position_ =
action_result_[action_enable_it->first]->goal_position_;
279 std::string status_msg =
"Action_Start";
281 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
285 for(std::map<std::string, robotis_framework::DynamixelState*>::iterator action_result_it =
action_result_.begin(); action_result_it !=
action_result_.end(); action_result_it++)
286 action_result_it->second->goal_position_ =
result_[action_result_it->first]->goal_position_;
288 std::string status_msg =
"Action_Finish";
290 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, status_msg);
319 return (
int)((rad + M_PI)*2048.0/M_PI);
324 return (w4095 - 2048)*M_PI/2048.0;
329 unsigned char checksum = 0x00;
330 unsigned char* pt = (
unsigned char*)page;
346 unsigned char checksum = 0x00;
347 unsigned char* pt = (
unsigned char*)page;
362 FILE* action = fopen( file_name.c_str(),
"r+b" );
365 std::string status_msg =
"Can not open Action file!";
367 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
371 fseek( action, 0, SEEK_END );
374 std::string status_msg =
"It's not an Action file!";
376 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
390 FILE* action = fopen( file_name.c_str(),
"ab" );
393 std::string status_msg =
"Can not create Action file!";
395 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
421 if(
loadPage(page_number, &page) ==
false )
424 return start(page_number, &page);
437 if(strcmp(page_name.c_str(), (
char*)page.
header.
name) == 0)
441 if(index == action_file_define::MAXNUM_PAGE)
446 return start(index, &page);
453 std::string status_msg =
"Action Module is disabled";
455 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
461 std::string status_msg =
"Can not play page " +
convertIntToString(page_number) +
".(Now playing)";
463 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
471 std::string status_msg =
"Page " +
convertIntToString(page_number) +
" has no action\n";
473 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_ERROR, status_msg);
491 if(playing_page_num != 0)
494 if(playing_step_num != 0)
537 unsigned char *pt = (
unsigned char*)page;
579 uint32_t total_time_256t;
580 uint32_t pre_section_time_256t;
581 uint32_t main_time_256t;
582 int32_t start_speed1024_pre_time_256t;
583 int32_t moving_angle_speed1024_scale_256t_2t;
584 int32_t divider1,divider2;
589 int16_t prev_target_angle;
590 int16_t curr_target_angle;
591 int16_t next_target_angle;
592 uint8_t direction_changed;
606 static uint16_t unit_time_count;
607 static uint16_t unit_time_num;
608 static uint16_t pause_time;
609 static uint16_t unit_time_total_num;
610 static uint16_t accel_step;
611 static uint8_t section;
612 static uint8_t play_repeat_count;
613 static uint16_t next_play_page;
645 std::string joint_name =
"";
651 joint_name = id_to_name_it->second;
654 std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.find(joint_name);
655 if(dxls_it == dxls.end())
659 double goal_joint_angle_rad = dxls_it->second->dxl_state_->goal_position_;
661 last_out_speed[id] = 0;
662 moving_angle[id] = 0;
669 if( unit_time_count < unit_time_num )
680 std::string joint_name =
"";
686 joint_name = id_to_name_it->second;
689 std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.find(joint_name);
690 if(dxls_it == dxls.end())
696 if( moving_angle[
id] == 0 )
704 speed_n = (short)( ( (
long)(main_speed[
id] - last_out_speed[
id]) * unit_time_count ) / unit_time_num );
705 goal_speed[id] = last_out_speed[id] + speed_n;
706 accel_angle[id] = (short)( ( ( (
long)( last_out_speed[
id] + (speed_n >> 1) ) * unit_time_count * 144 ) / 15 ) >> 9);
712 action_result_[joint_name]->goal_position_ =
convertw4095ToRad(start_angle[
id] + (
short int)(((
long)(main_angle[
id])*unit_time_count) / unit_time_num));
714 goal_speed[id] = main_speed[id];
718 if( unit_time_count == (unit_time_num-1) )
727 speed_n = (
short int)(((
long)(0 - last_out_speed[
id]) * unit_time_count) / unit_time_num);
728 goal_speed[id] = last_out_speed[id] + speed_n;
731 =
convertw4095ToRad(start_angle[
id] + (
short)((((
long)(last_out_speed[
id] + (speed_n>>1)) * unit_time_count * 144) / 15) >> 9));
739 =
convertw4095ToRad(start_angle[
id] + (
short int)(((
long)(main_angle[
id]) * unit_time_count) / unit_time_num));
741 goal_speed[id] = main_speed[id];
751 else if( unit_time_count >= unit_time_num )
758 std::string joint_name =
"";
763 joint_name = id_to_name_it->second;
766 std::map<std::string, robotis_framework::Dynamixel *>::iterator dxls_it = dxls.find(joint_name);
767 if(dxls_it == dxls.end())
771 double _goal_joint_angle_rad = dxls_it->second->dxl_state_->goal_position_;
773 last_out_speed[id] = goal_speed[id];
782 unit_time_num = unit_time_total_num - (accel_step << 1);
790 if( (unit_time_total_num - accel_step) == 0 )
793 main_angle[id] = (short)((((
long)(moving_angle[
id] - accel_angle[
id])) * unit_time_num) / (unit_time_total_num - accel_step));
796 main_angle[id] = moving_angle[id] - accel_angle[id] - (
short int)((((
long)main_speed[id] * accel_step * 12) / 5) >> 8);
803 unit_time_num = accel_step;
808 main_angle[id] = moving_angle[id] - main_angle[id] - accel_angle[id];
817 unit_time_num = pause_time;
832 last_out_speed[id] = 0;
867 if( play_repeat_count > 0 )
873 if( next_play_page == 0 )
905 curr_target_angle = target_angle[id];
910 start_angle[id] = target_angle[id];
911 prev_target_angle = target_angle[id];
912 target_angle[id] = curr_target_angle;
915 moving_angle[id] = (int)(target_angle[
id] - start_angle[
id]);
921 next_target_angle = curr_target_angle;
925 next_target_angle = curr_target_angle;
933 next_target_angle = curr_target_angle;
939 if( ((prev_target_angle < curr_target_angle) && (curr_target_angle < next_target_angle))
940 || ((prev_target_angle > curr_target_angle) && (curr_target_angle > next_target_angle)) )
943 direction_changed = 0;
947 direction_changed = 1;
959 if( moving_angle[
id] < 0 )
960 tmp = -moving_angle[id];
962 tmp = moving_angle[id];
964 if( tmp > max_angle )
977 unit_time_total_num = max_speed;
979 unit_time_total_num = (max_angle * 40) / (max_speed * 3);
982 if( unit_time_total_num <= (accel_step << 1) )
984 if( unit_time_total_num == 0 )
990 accel_step = (unit_time_total_num - 1) >> 1;
991 if( accel_step == 0 )
992 unit_time_total_num = 0;
996 total_time_256t = ((
unsigned long)unit_time_total_num) << 1;
997 pre_section_time_256t = ((
unsigned long)accel_step) << 1;
998 main_time_256t = total_time_256t - pre_section_time_256t;
999 divider1 = pre_section_time_256t + (main_time_256t << 1);
1000 divider2 = (main_time_256t << 1);
1011 start_speed1024_pre_time_256t = (long)last_out_speed[
id] * pre_section_time_256t;
1012 moving_angle_speed1024_scale_256t_2t = (((long)moving_angle[
id]) * 2560L) / 12;
1015 main_speed[id] = (
short int)((moving_angle_speed1024_scale_256t_2t - start_speed1024_pre_time_256t) / divider2);
1017 main_speed[id] = (
short int)((moving_angle_speed1024_scale_256t_2t - start_speed1024_pre_time_256t) / divider1);
1019 if( main_speed[
id] > 1023 )
1020 main_speed[id] = 1023;
1022 if( main_speed[
id] < -1023 )
1023 main_speed[id] = -1023;
1025 unit_time_num = accel_step;
1033 robotis_controller_msgs::StatusMsg status;
1036 status.module_name =
"Action";
1037 status.status_msg = msg;
1044 std_msgs::String done_msg;
1045 done_msg.data = msg;
void publishStatusMsg(unsigned int type, std::string msg)
const int SPEED_BASE_SCHEDULE
action_file_define::Page play_page_
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
void setChecksum(action_file_define::Page *page)
std::map< std::string, DynamixelState * > result_
void publish(const boost::shared_ptr< M > &message) const
unsigned short position[MAXNUM_JOINTS]
bool first_driving_start_
void startActionCallback(const thormang3_action_module_msgs::StartAction::ConstPtr &msg)
void pageNumberCallback(const std_msgs::Int32::ConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Publisher status_msg_pub_
void actionPlayProcess(std::map< std::string, robotis_framework::Dynamixel * > dxls)
bool loadFile(std::string file_name)
bool savePage(int page_number, action_file_define::Page *page)
std::map< int, std::string > joint_id_to_name_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::map< std::string, int > joint_name_to_id_
void publishDoneMsg(std::string msg)
ControlMode control_mode_
const int TIME_BASE_SCHEDULE
void resetPage(action_file_define::Page *page)
std::map< std::string, Dynamixel * > dxls_
ros::Publisher done_msg_pub_
std::map< std::string, bool > action_joints_enable_
bool loadPage(int page_number, action_file_define::Page *page)
bool verifyChecksum(action_file_define::Page *page)
boost::thread queue_thread_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void setCallbackQueue(CallbackQueueInterface *queue)
int convertRadTow4095(double rad)
action_file_define::Page next_play_page_
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const int INVALID_BIT_MASK
ROSLIB_DECL std::string getPath(const std::string &package_name)
#define ROS_INFO_STREAM(args)
bool isRunningServiceCallback(thormang3_action_module_msgs::IsRunning::Request &req, thormang3_action_module_msgs::IsRunning::Response &res)
DynamixelState * dxl_state_
const int NONE_ZERO_FINISH
bool createFile(std::string file_name)
double convertw4095ToRad(int w4095)
bool start(int page_number)
#define ROS_ERROR_STREAM(args)
std::string convertIntToString(int n)
bool action_module_enabled_
std::map< std::string, robotis_framework::DynamixelState * > action_result_