19 #ifndef HEAD_CONTROL_MODULE_H_ 20 #define HEAD_CONTROL_MODULE_H_ 24 #include <boost/thread.hpp> 25 #include <eigen3/Eigen/Eigen> 29 #include <std_msgs/Empty.h> 30 #include <std_msgs/String.h> 31 #include <sensor_msgs/JointState.h> 33 #include "robotis_controller_msgs/StatusMsg.h" 47 void process(std::map<std::string, robotis_framework::Dynamixel *> dxls, std::map<std::string, double> sensors);
72 void setHeadJoint(
const sensor_msgs::JointState::ConstPtr &msg,
bool is_offset);
82 Eigen::MatrixXd
calcMinimumJerkTraPVA(
double pos_start,
double vel_start,
double accel_start,
double pos_end,
83 double vel_end,
double accel_end,
double smp_time,
double mov_time);
Eigen::MatrixXd calc_joint_vel_tra_
Eigen::MatrixXd calcMinimumJerkTraPVA(double pos_start, double vel_start, double accel_start, double pos_end, double vel_end, double accel_end, double smp_time, double mov_time)
std::map< std::string, int > using_joint_name_
Eigen::MatrixXd target_position_
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
void setHeadJointCallback(const sensor_msgs::JointState::ConstPtr &msg)
Eigen::MatrixXd calc_joint_accel_tra_
bool checkAngleLimit(const int joint_index, double &goal_position)
std::map< int, double > max_angle_
boost::thread * tra_gene_thread_
Eigen::MatrixXd goal_velocity_
virtual ~HeadControlModule()
ros::Publisher status_msg_pub_
boost::thread queue_thread_
Eigen::MatrixXd current_position_
std::map< int, double > min_angle_
void setHeadJointOffsetCallback(const sensor_msgs::JointState::ConstPtr &msg)
void jointTraGeneThread()
Eigen::MatrixXd calc_joint_tra_
void setHeadJoint(const sensor_msgs::JointState::ConstPtr &msg, bool is_offset)
void generateScanTra(const int head_direction)
void setHeadScanCallback(const std_msgs::String::ConstPtr &msg)
Eigen::MatrixXd goal_position_
void publishStatusMsg(unsigned int type, std::string msg)
Eigen::MatrixXd goal_acceleration_