base_module.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2017 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /* Author: Kayman, SCH */
18 
19 #include <stdio.h>
21 
22 namespace robotis_op
23 {
24 
26  : control_cycle_msec_(0),
27  has_goal_joints_(false),
28  ini_pose_only_(false)
29 {
30  enable_ = false;
31  module_name_ = "base_module";
33 
36 }
37 
39 {
40  queue_thread_.join();
41 }
42 
43 void BaseModule::initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
44 {
45  control_cycle_msec_ = control_cycle_msec;
46  queue_thread_ = boost::thread(boost::bind(&BaseModule::queueThread, this));
47 
48  // init result, joint_id_table
49  for (std::map<std::string, robotis_framework::Dynamixel*>::iterator it = robot->dxls_.begin();
50  it != robot->dxls_.end(); it++)
51  {
52  std::string joint_name = it->first;
53  robotis_framework::Dynamixel* dxl_info = it->second;
54 
55  joint_name_to_id_[joint_name] = dxl_info->id_;
56  result_[joint_name] = new robotis_framework::DynamixelState();
57  result_[joint_name]->goal_position_ = dxl_info->dxl_state_->goal_position_;
58  }
59 
60  ros::NodeHandle ros_node;
61 
62  /* publish topics */
63  status_msg_pub_ = ros_node.advertise<robotis_controller_msgs::StatusMsg>("/robotis/status", 1);
64  set_ctrl_module_pub_ = ros_node.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 1);
65 }
66 
67 void BaseModule::parseInitPoseData(const std::string &path)
68 {
69  YAML::Node doc;
70  try
71  {
72  // load yaml
73  doc = YAML::LoadFile(path.c_str());
74  } catch (const std::exception& e)
75  {
76  ROS_ERROR("Fail to load yaml file.");
77  return;
78  }
79 
80  // parse movement time
81  double mov_time;
82  mov_time = doc["mov_time"].as<double>();
83 
84  base_module_state_->mov_time_ = mov_time;
85 
86  // parse via-point number
87  int via_num;
88  via_num = doc["via_num"].as<int>();
89 
90  base_module_state_->via_num_ = via_num;
91 
92  // parse via-point time
93  std::vector<double> via_time;
94  via_time = doc["via_time"].as<std::vector<double> >();
95 
96  base_module_state_->via_time_.resize(via_num, 1);
97  for (int num = 0; num < via_num; num++)
98  base_module_state_->via_time_.coeffRef(num, 0) = via_time[num];
99 
100  // parse via-point pose
101  base_module_state_->joint_via_pose_.resize(via_num, MAX_JOINT_ID + 1);
102  base_module_state_->joint_via_dpose_.resize(via_num, MAX_JOINT_ID + 1);
103  base_module_state_->joint_via_ddpose_.resize(via_num, MAX_JOINT_ID + 1);
104 
108 
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)
111  {
112  int id;
113  std::vector<double> value;
114 
115  id = yaml_it->first.as<int>();
116  value = yaml_it->second.as<std::vector<double> >();
117 
118  for (int num = 0; num < via_num; num++)
119  base_module_state_->joint_via_pose_.coeffRef(num, id) = value[num] * DEGREE2RADIAN;
120  }
121 
122  // parse target pose
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)
125  {
126  int id;
127  double value;
128 
129  id = yaml_it->first.as<int>();
130  value = yaml_it->second.as<double>();
131 
132  base_module_state_->joint_ini_pose_.coeffRef(id, 0) = value * DEGREE2RADIAN;
133  }
134 
137 }
138 
140 {
141  ros::NodeHandle ros_node;
142  ros::CallbackQueue callback_queue;
143 
144  ros_node.setCallbackQueue(&callback_queue);
145 
146  /* subscribe topics */
147  ros::Subscriber ini_pose_msg_sub = ros_node.subscribe("/robotis/base/ini_pose", 5, &BaseModule::initPoseMsgCallback,
148  this);
149  set_module_client_ = ros_node.serviceClient<robotis_controller_msgs::SetModule>("/robotis/set_present_ctrl_modules");
150 
151  ros::WallDuration duration(control_cycle_msec_ / 1000.0);
152  while (ros_node.ok())
153  callback_queue.callAvailable(duration);
154 }
155 
156 void BaseModule::initPoseMsgCallback(const std_msgs::String::ConstPtr& msg)
157 {
158  if (base_module_state_->is_moving_ == false)
159  {
160  if (msg->data == "ini_pose")
161  {
162  // set module of all joints -> this module
164 
165  // wait for changing the module to base_module and getting the goal position
166  while (enable_ == false || has_goal_joints_ == false)
167  usleep(8 * 1000);
168 
169  // parse initial pose
170  std::string init_pose_path = ros::package::getPath("op3_base_module") + "/data/ini_pose.yaml";
171  parseInitPoseData(init_pose_path);
172 
173  // generate trajectory
174  tra_gene_tread_ = boost::thread(boost::bind(&BaseModule::initPoseTrajGenerateProc, this));
175  }
176  }
177  else
178  ROS_INFO("previous task is alive");
179 
180  return;
181 }
182 
184 {
185  for (int id = 1; id <= MAX_JOINT_ID; id++)
186  {
187  double ini_value = joint_state_->goal_joint_state_[id].position_;
188  double tar_value = base_module_state_->joint_ini_pose_.coeff(id, 0);
189 
190  Eigen::MatrixXd tra;
191 
192  if (base_module_state_->via_num_ == 0)
193  {
194  tra = robotis_framework::calcMinimumJerkTra(ini_value, 0.0, 0.0, tar_value, 0.0, 0.0,
196  }
197  else
198  {
199  Eigen::MatrixXd via_value = base_module_state_->joint_via_pose_.col(id);
200  Eigen::MatrixXd d_via_value = base_module_state_->joint_via_dpose_.col(id);
201  Eigen::MatrixXd dd_via_value = base_module_state_->joint_via_ddpose_.col(id);
202 
204  via_value, d_via_value, dd_via_value, tar_value, 0.0,
208  }
209 
211  }
212 
215  ROS_INFO("[start] send trajectory");
216 }
217 
218 void BaseModule::poseGenerateProc(Eigen::MatrixXd joint_angle_pose)
219 {
221 
222  while (enable_ == false || has_goal_joints_ == false)
223  usleep(8 * 1000);
224 
227 
229 
230  base_module_state_->joint_pose_ = joint_angle_pose;
231 
232  for (int id = 1; id <= MAX_JOINT_ID; id++)
233  {
234  double ini_value = joint_state_->goal_joint_state_[id].position_;
235  double tar_value = base_module_state_->joint_pose_.coeff(id, 0);
236 
237  ROS_INFO_STREAM("[ID : " << id << "] ini_value : " << ini_value << " tar_value : " << tar_value);
238 
239  Eigen::MatrixXd tra = robotis_framework::calcMinimumJerkTra(ini_value, 0.0, 0.0, tar_value, 0.0, 0.0,
242 
244  }
245 
248  ini_pose_only_ = true;
249  ROS_INFO("[start] send trajectory");
250 }
251 
252 void BaseModule::poseGenerateProc(std::map<std::string, double>& joint_angle_pose)
253 {
255 
256  while (enable_ == false || has_goal_joints_ == false)
257  usleep(8 * 1000);
258 
259  Eigen::MatrixXd target_pose = Eigen::MatrixXd::Zero( MAX_JOINT_ID + 1, 1);
260 
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++)
263  {
264  std::string joint_name = joint_angle_it->first;
265  double joint_angle_rad = joint_angle_it->second;
266 
267  std::map<std::string, int>::iterator joint_name_to_id_it = joint_name_to_id_.find(joint_name);
268  if (joint_name_to_id_it != joint_name_to_id_.end())
269  {
270  target_pose.coeffRef(joint_name_to_id_it->second, 0) = joint_angle_rad;
271  }
272  }
273 
274  base_module_state_->joint_pose_ = target_pose;
275 
278 
280 
281  for (int id = 1; id <= MAX_JOINT_ID; id++)
282  {
283  double ini_value = joint_state_->goal_joint_state_[id].position_;
284  double tar_value = base_module_state_->joint_pose_.coeff(id, 0);
285 
286  ROS_INFO_STREAM("[ID : " << id << "] ini_value : " << ini_value << " tar_value : " << tar_value);
287 
288  Eigen::MatrixXd tra = robotis_framework::calcMinimumJerkTra(ini_value, 0.0, 0.0, tar_value, 0.0, 0.0,
291 
293  }
294 
297  ini_pose_only_ = true;
298  ROS_INFO("[start] send trajectory");
299 }
300 
302 {
304 }
305 
306 void BaseModule::process(std::map<std::string, robotis_framework::Dynamixel *> dxls,
307  std::map<std::string, double> sensors)
308 {
309  if (enable_ == false)
310  return;
311 
312  /*----- write curr position -----*/
313  for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter = result_.begin();
314  state_iter != result_.end(); state_iter++)
315  {
316  std::string joint_name = state_iter->first;
317 
318  robotis_framework::Dynamixel *dxl = NULL;
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;
322  else
323  continue;
324 
325  double joint_curr_position = dxl->dxl_state_->present_position_;
326  double joint_goal_position = dxl->dxl_state_->goal_position_;
327 
328  joint_state_->curr_joint_state_[joint_name_to_id_[joint_name]].position_ = joint_curr_position;
329  joint_state_->goal_joint_state_[joint_name_to_id_[joint_name]].position_ = joint_goal_position;
330  }
331 
332  has_goal_joints_ = true;
333 
334  /* ----- send trajectory ----- */
335  if (base_module_state_->is_moving_ == true)
336  {
337  if (base_module_state_->cnt_ == 1)
338  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "Start Init Pose");
339 
340  for (int id = 1; id <= MAX_JOINT_ID; id++)
342 
344  }
345 
346  /*----- set joint data -----*/
347  for (std::map<std::string, robotis_framework::DynamixelState *>::iterator state_iter = result_.begin();
348  state_iter != result_.end(); state_iter++)
349  {
350  std::string joint_name = state_iter->first;
351 
352  result_[joint_name]->goal_position_ = joint_state_->goal_joint_state_[joint_name_to_id_[joint_name]].position_;
353  }
354 
355  /*---------- initialize count number ----------*/
356 
358  {
359  ROS_INFO("[end] send trajectory");
360 
361  publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "Finish Init Pose");
362 
365 
366  // set all joints -> none
367  if (ini_pose_only_ == true)
368  {
369  setCtrlModule("none");
370  ini_pose_only_ = false;
371  }
372  }
373 }
374 
376 {
377  return;
378 }
379 
381 {
382  ROS_INFO("Base Module is enabled");
383 }
384 
386 {
387  has_goal_joints_ = false;
388 }
389 
390 void BaseModule::setCtrlModule(std::string module)
391 {
392  std_msgs::String control_msg;
393  control_msg.data = module_name_;
394 
395  set_ctrl_module_pub_.publish(control_msg);
396 }
397 
398 void BaseModule::callServiceSettingModule(const std::string &module_name)
399 {
400  robotis_controller_msgs::SetModule set_module_srv;
401  set_module_srv.request.module_name = module_name;
402 
403  if (set_module_client_.call(set_module_srv) == false)
404  {
405  ROS_ERROR("Failed to set module");
406  return;
407  }
408 
409  return ;
410 }
411 
412 void BaseModule::publishStatusMsg(unsigned int type, std::string msg)
413 {
414  robotis_controller_msgs::StatusMsg status_msg;
415  status_msg.header.stamp = ros::Time::now();
416  status_msg.type = type;
417  status_msg.module_name = "Base";
418  status_msg.status_msg = msg;
419 
420  status_msg_pub_.publish(status_msg);
421 }
422 }
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_
Definition: base_module.h:96
std::map< std::string, DynamixelState * > result_
void initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
Definition: base_module.cpp:43
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_
Definition: base_module.h:109
bool call(MReq &req, MRes &res)
void callServiceSettingModule(const std::string &module_name)
std::map< std::string, Dynamixel * > dxls_
BaseJointData curr_joint_state_[MAX_JOINT_ID+1]
Definition: base_module.h:63
boost::thread queue_thread_
Definition: base_module.h:106
void parseInitPoseData(const std::string &path)
Definition: base_module.cpp:67
ros::ServiceClient set_module_client_
Definition: base_module.h:112
void initPoseMsgCallback(const std_msgs::String::ConstPtr &msg)
#define ROS_INFO(...)
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)
#define DEGREE2RADIAN
BaseJointData goal_joint_state_[MAX_JOINT_ID+1]
Definition: base_module.h:64
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_
Definition: base_module.h:95
std::map< std::string, int > joint_name_to_id_
Definition: base_module.h:114
void setCtrlModule(std::string module)
#define ROS_INFO_STREAM(args)
DynamixelState * dxl_state_
ros::Publisher set_ctrl_module_pub_
Definition: base_module.h:110
static Time now()
void process(std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
bool ok() const
#define MAX_JOINT_ID
#define ROS_ERROR(...)
boost::thread tra_gene_tread_
Definition: base_module.h:107


op3_base_module
Author(s): Kayman , SCH
autogenerated on Mon Jun 10 2019 14:41:14