24 #ifndef THORMANG3_ACTION_MODULE_ACTION_MODULE_H_ 25 #define THORMANG3_ACTION_MODULE_ACTION_MODULE_H_ 27 #define _USE_MATH_DEFINES 34 #include <std_msgs/Int32.h> 35 #include <std_msgs/String.h> 36 #include <boost/thread.hpp> 38 #include "robotis_controller_msgs/StatusMsg.h" 39 #include "thormang3_action_module_msgs/IsRunning.h" 40 #include "thormang3_action_module_msgs/StartAction.h" 56 void process(std::map<std::string, robotis_framework::Dynamixel *> dxls, std::map<std::string, double> sensors);
61 bool loadFile(std::string file_name);
64 bool start(
int page_number);
65 bool start(std::string page_name);
72 bool isRunning(
int* playing_page_num,
int* playing_step_num);
78 void actionPlayProcess(std::map<std::string, robotis_framework::Dynamixel *> dxls);
90 thormang3_action_module_msgs::IsRunning::Response &res);
void publishStatusMsg(unsigned int type, std::string msg)
action_file_define::Page play_page_
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
void setChecksum(action_file_define::Page *page)
bool first_driving_start_
void startActionCallback(const thormang3_action_module_msgs::StartAction::ConstPtr &msg)
void pageNumberCallback(const std_msgs::Int32::ConstPtr &msg)
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_
std::map< std::string, int > joint_name_to_id_
void publishDoneMsg(std::string msg)
void resetPage(action_file_define::Page *page)
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_
action_file_define::Step current_step_
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)
bool isRunningServiceCallback(thormang3_action_module_msgs::IsRunning::Request &req, thormang3_action_module_msgs::IsRunning::Response &res)
const int NONE_ZERO_FINISH
bool createFile(std::string file_name)
double convertw4095ToRad(int w4095)
bool start(int page_number)
std::string convertIntToString(int n)
bool action_module_enabled_
std::map< std::string, robotis_framework::DynamixelState * > action_result_