23 #include <boost/thread.hpp> 24 #include <yaml-cpp/yaml.h> 29 #include <std_msgs/Int16.h> 30 #include <std_msgs/Float64.h> 31 #include <std_msgs/String.h> 32 #include <geometry_msgs/Pose.h> 35 #include "robotis_controller_msgs/JointCtrlModule.h" 36 #include "robotis_controller_msgs/SetModule.h" 37 #include "robotis_controller_msgs/StatusMsg.h" 77 void process(std::map<std::string, robotis_framework::Dynamixel *> dxls, std::map<std::string, double> sensors);
82 void onModuleEnable();
83 void onModuleDisable();
86 void initPoseMsgCallback(
const std_msgs::String::ConstPtr& msg);
89 void initPoseTrajGenerateProc();
91 void poseGenerateProc(Eigen::MatrixXd joint_angle_pose);
92 void poseGenerateProc(std::map<std::string, double>& joint_angle_pose);
100 void setCtrlModule(std::string module);
101 void callServiceSettingModule(
const std::string &module_name);
102 void parseInitPoseData(
const std::string &path);
103 void publishStatusMsg(
unsigned int type, std::string msg);
BaseJointState * joint_state_
ROSCONSOLE_DECL void initialize()
ros::Publisher status_msg_pub_
boost::thread queue_thread_
ros::ServiceClient set_module_client_
BaseModuleState * base_module_state_
std::map< std::string, int > joint_name_to_id_
ros::Publisher set_ctrl_module_pub_
boost::thread tra_gene_tread_