27 #define EXT_PORT_DATA_1 "external_port_data_1" 28 #define EXT_PORT_DATA_2 "external_port_data_2" 29 #define EXT_PORT_DATA_3 "external_port_data_3" 30 #define EXT_PORT_DATA_4 "external_port_data_4" 35 : control_cycle_msec_(0),
36 has_goal_joints_(false),
155 doc = YAML::LoadFile(path.c_str());
156 }
catch (
const std::exception& e)
164 mov_time = doc[
"mov_time"].as<
double>();
170 via_num = doc[
"via_num"].as<
int>();
175 std::vector<double> via_time;
176 via_time = doc[
"via_time"].as<std::vector<double> >();
179 for (
int num = 0; num < via_num; num++)
191 YAML::Node via_pose_node = doc[
"via_pose"];
192 for (YAML::iterator it = via_pose_node.begin(); it != via_pose_node.end(); ++it)
195 std::vector<double> value;
197 id = it->first.as<
int>();
198 value = it->second.as<std::vector<double> >();
200 for (
int num = 0; num < via_num; num++)
205 YAML::Node tar_pose_node = doc[
"tar_pose"];
206 for (YAML::iterator it = tar_pose_node.begin(); it != tar_pose_node.end(); ++it)
211 id = it->first.as<
int>();
212 value = it->second.as<
double>();
242 if (msg->data ==
"ini_pose")
286 via_value, d_via_value, dd_via_value, tar_value,
296 ROS_INFO(
"[start] send trajectory");
317 ROS_INFO_STREAM(
"[ID : " <<
id <<
"] ini_value : " << ini_value <<
" tar_value : " << tar_value);
328 ROS_INFO(
"[start] send trajectory");
337 Eigen::MatrixXd target_pose = Eigen::MatrixXd::Zero(
MAX_JOINT_ID + 1, 1);
339 for (std::map<std::string, double>::iterator it = joint_angle_pose.begin(); it != joint_angle_pose.end(); it++)
341 std::string joint_name = it->first;
342 double joint_angle_rad = it->second;
344 std::map<std::string, int>::iterator joint_name_to_id_it =
joint_name_to_id_.find(joint_name);
347 target_pose.coeffRef(joint_name_to_id_it->second, 0) = joint_angle_rad;
363 ROS_INFO_STREAM(
"[ID : " <<
id <<
"] ini_value : " << ini_value <<
" tar_value : " << tar_value);
374 ROS_INFO(
"[start] send trajectory");
383 std::map<std::string, double> sensors)
389 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter =
result_.begin();
390 state_iter !=
result_.end(); state_iter++)
392 std::string joint_name = state_iter->first;
395 std::map<std::string, robotis_framework::Dynamixel*>::iterator dxl_it = dxls.find(joint_name);
396 if (dxl_it != dxls.end())
397 dxl = dxl_it->second;
415 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO,
"Start Init Pose");
425 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter =
result_.begin();
426 state_iter !=
result_.end(); state_iter++)
428 std::string joint_name = state_iter->first;
439 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO,
"Finish Init Pose");
461 std_msgs::String control_msg;
469 robotis_controller_msgs::StatusMsg status;
472 status.module_name =
"Base";
473 status.status_msg = msg;
480 std_msgs::String movement_msg;
481 movement_msg.data = done_msg;
void poseGenerateProc(Eigen::MatrixXd joint_angle_pose)
ros::Publisher movement_done_pub_
std::map< std::string, DynamixelState * > result_
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, double pos_end, double vel_end, double accel_end, double smp_time, double mov_time)
void publish(const boost::shared_ptr< M > &message) const
BaseJointData curr_joint_state_[MAX_JOINT_ID+1]
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
Eigen::MatrixXd joint_via_pose_
void parseIniPoseData(const std::string &path)
boost::thread tra_gene_tread_
void publishStatusMsg(unsigned int type, std::string msg)
Eigen::MatrixXd joint_ini_pose_
ControlMode control_mode_
Eigen::MatrixXd calc_joint_tra_
void publishDoneMsg(const std::string done_msg)
Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num, double pos_start, double vel_start, double accel_start, Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, double pos_end, double vel_end, double accel_end, double smp_time, Eigen::MatrixXd via_time, double mov_time)
void setCallbackQueue(CallbackQueueInterface *queue)
void initPoseTrajGenerateProc()
BaseJointState * joint_state_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Eigen::MatrixXd joint_via_dpose_
ros::Publisher set_ctrl_module_pub_
ROSLIB_DECL std::string getPath(const std::string &package_name)
boost::thread queue_thread_
Eigen::MatrixXd joint_pose_
#define ROS_INFO_STREAM(args)
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
DynamixelState * dxl_state_
BaseJointData goal_joint_state_[MAX_JOINT_ID+1]
ros::Publisher status_msg_pub_
Eigen::MatrixXd joint_via_ddpose_
BaseModuleState * base_module_state_
std::map< std::string, int > joint_name_to_id_
void setCtrlModule(std::string module)
void initPoseMsgCallback(const std_msgs::String::ConstPtr &msg)
Eigen::MatrixXd via_time_