26 : control_cycle_msec_(0),
27 has_goal_joints_(false),
49 for (std::map<std::string, robotis_framework::Dynamixel*>::iterator it = robot->
dxls_.begin();
50 it != robot->
dxls_.end(); it++)
52 std::string joint_name = it->first;
73 doc = YAML::LoadFile(path.c_str());
74 }
catch (
const std::exception& e)
82 mov_time = doc[
"mov_time"].as<
double>();
88 via_num = doc[
"via_num"].as<
int>();
93 std::vector<double> via_time;
94 via_time = doc[
"via_time"].as<std::vector<double> >();
97 for (
int num = 0; num < via_num; num++)
109 YAML::Node via_pose_node = doc[
"via_pose"];
110 for (YAML::iterator yaml_it = via_pose_node.begin(); yaml_it != via_pose_node.end(); ++yaml_it)
113 std::vector<double> value;
115 id = yaml_it->first.as<
int>();
116 value = yaml_it->second.as<std::vector<double> >();
118 for (
int num = 0; num < via_num; num++)
123 YAML::Node tar_pose_node = doc[
"tar_pose"];
124 for (YAML::iterator yaml_it = tar_pose_node.begin(); yaml_it != tar_pose_node.end(); ++yaml_it)
129 id = yaml_it->first.as<
int>();
130 value = yaml_it->second.as<
double>();
152 while (ros_node.
ok())
160 if (msg->data ==
"ini_pose")
204 via_value, d_via_value, dd_via_value, tar_value, 0.0,
215 ROS_INFO(
"[start] send trajectory");
237 ROS_INFO_STREAM(
"[ID : " <<
id <<
"] ini_value : " << ini_value <<
" tar_value : " << tar_value);
249 ROS_INFO(
"[start] send trajectory");
259 Eigen::MatrixXd target_pose = Eigen::MatrixXd::Zero(
MAX_JOINT_ID + 1, 1);
261 for (std::map<std::string, double>::iterator joint_angle_it = joint_angle_pose.begin();
262 joint_angle_it != joint_angle_pose.end(); joint_angle_it++)
264 std::string joint_name = joint_angle_it->first;
265 double joint_angle_rad = joint_angle_it->second;
267 std::map<std::string, int>::iterator joint_name_to_id_it =
joint_name_to_id_.find(joint_name);
270 target_pose.coeffRef(joint_name_to_id_it->second, 0) = joint_angle_rad;
286 ROS_INFO_STREAM(
"[ID : " <<
id <<
"] ini_value : " << ini_value <<
" tar_value : " << tar_value);
298 ROS_INFO(
"[start] send trajectory");
307 std::map<std::string, double> sensors)
313 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter =
result_.begin();
314 state_iter !=
result_.end(); state_iter++)
316 std::string joint_name = state_iter->first;
319 std::map<std::string, robotis_framework::Dynamixel*>::iterator dxl_it = dxls.find(joint_name);
320 if (dxl_it != dxls.end())
321 dxl = dxl_it->second;
338 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO,
"Start Init Pose");
347 for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter =
result_.begin();
348 state_iter !=
result_.end(); state_iter++)
350 std::string joint_name = state_iter->first;
361 publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO,
"Finish Init Pose");
392 std_msgs::String control_msg;
400 robotis_controller_msgs::SetModule set_module_srv;
401 set_module_srv.request.module_name = module_name;
414 robotis_controller_msgs::StatusMsg status_msg;
416 status_msg.type = type;
417 status_msg.module_name =
"Base";
418 status_msg.status_msg = msg;
Eigen::MatrixXd joint_via_dpose_
void poseGenerateProc(Eigen::MatrixXd joint_angle_pose)
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
BaseJointState * joint_state_
Eigen::MatrixXd calc_joint_tra_
std::map< std::string, DynamixelState * > result_
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
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
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_
bool call(MReq &req, MRes &res)
void callServiceSettingModule(const std::string &module_name)
Eigen::MatrixXd joint_ini_pose_
ControlMode control_mode_
std::map< std::string, Dynamixel * > dxls_
BaseJointData curr_joint_state_[MAX_JOINT_ID+1]
boost::thread queue_thread_
void parseInitPoseData(const std::string &path)
ros::ServiceClient set_module_client_
void initPoseMsgCallback(const std_msgs::String::ConstPtr &msg)
Eigen::MatrixXd via_time_
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)
BaseJointData goal_joint_state_[MAX_JOINT_ID+1]
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishStatusMsg(unsigned int type, std::string msg)
ROSLIB_DECL std::string getPath(const std::string &package_name)
BaseModuleState * base_module_state_
std::map< std::string, int > joint_name_to_id_
void initPoseTrajGenerateProc()
void setCtrlModule(std::string module)
#define ROS_INFO_STREAM(args)
Eigen::MatrixXd joint_via_ddpose_
DynamixelState * dxl_state_
Eigen::MatrixXd joint_via_pose_
ros::Publisher set_ctrl_module_pub_
Eigen::MatrixXd joint_pose_
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
boost::thread tra_gene_tread_