footstep_marker.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
40 #include <boost/format.hpp>
41 #include <pcl/common/eigen.h>
42 #include <angles/angles.h>
45 #include <jsk_interactive_marker/SnapFootPrint.h>
46 #include <jsk_footstep_planner/CollisionBoundingBoxInfo.h>
47 #include <dynamic_reconfigure/Reconfigure.h>
48 
49 #define printAffine(af) { \
50  geometry_msgs::Pose __geom_pose;\
51  tf::poseEigenToMsg(af, __geom_pose);\
52  std::cerr << __geom_pose.position.x << ", ";\
53  std::cerr << __geom_pose.position.y << ", ";\
54  std::cerr << __geom_pose.position.z << " / ";\
55  std::cerr << __geom_pose.orientation.w << ", ";\
56  std::cerr << __geom_pose.orientation.x << ", ";\
57  std::cerr << __geom_pose.orientation.y << ", ";\
58  std::cerr << __geom_pose.orientation.z << std::endl; }
59 
60 namespace jsk_recognition_utils
61 {
62  void convertEigenAffine3(const Eigen::Affine3f& from,
63  Eigen::Affine3f& to)
64  {
65  to = from;
66  }
67 }
68 
69 namespace jsk_footstep_planner
70 {
71 
72  void add3Dof2DControl( visualization_msgs::InteractiveMarker &msg, bool fixed)
73  {
74  visualization_msgs::InteractiveMarkerControl control;
75 
76  if(fixed)
77  control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
78 
79  control.orientation.w = 1;
80  control.orientation.x = 1;
81  control.orientation.y = 0;
82  control.orientation.z = 0;
83  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
84  // msg.controls.push_back(control);
85  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
86  msg.controls.push_back(control);
87 
88  control.orientation.w = 1;
89  control.orientation.x = 0;
90  control.orientation.y = 1;
91  control.orientation.z = 0;
92  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
93  msg.controls.push_back(control);
94  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
95  // msg.controls.push_back(control);
96 
97  control.orientation.w = 1;
98  control.orientation.x = 0;
99  control.orientation.y = 0;
100  control.orientation.z = 1;
101  // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
102  // msg.controls.push_back(control);
103  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
104  msg.controls.push_back(control);
105 
106  }
107 
108  PosePair::PosePair(const FootstepTrans& first, const std::string& first_name,
109  const FootstepTrans& second, const std::string& second_name):
110  first_(first), first_name_(first_name),
111  second_(second), second_name_(second_name)
112  {
113 
114  }
115 
116  FootstepTrans PosePair::getByName(const std::string& name)
117  {
118  if (first_name_ == name) {
119  return first_;
120  }
121  else if (second_name_ == name) {
122  return second_;
123  }
124  else {
125  throw UnknownPoseName();
126  }
127  }
128 
130  {
131  FootstepTranslation pos((FootstepVec(first_.translation()) + FootstepVec(second_.translation())) / 2.0);
132  FootstepQuaternion rot = FootstepQuaternion(first_.rotation()).slerp(0.5, FootstepQuaternion(second_.rotation()));
133  return pos * rot;
134  }
135 
137  pnh_("~"), ac_planner_("footstep_planner", true), ac_exec_("footstep_controller", true),
138  pub_marker_array_(pnh_, "marker_array"),
139  is_2d_mode_(true), is_cube_mode_(false), command_mode_(SINGLE), have_last_step_(false),
140  planning_state_(NOT_STARTED)
141  {
142  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (pnh_);
143  typename dynamic_reconfigure::Server<Config>::CallbackType f =
144  boost::bind (&FootstepMarker::configCallback, this, _1, _2);
145  srv_->setCallback (f);
146 
147  tf_client_.reset(new tf2_ros::BufferClient("tf2_buffer_server"));
149  pnh_.param("frame_id", odom_frame_id_, std::string("odom"));
150  pnh_.param("lleg_end_coords", lleg_end_coords_, std::string("lleg_end_coords"));
151  pnh_.param("rleg_end_coords", rleg_end_coords_, std::string("rleg_end_coords"));
152  pnh_.param("footstep_size_x", foot_size_x_, 0.24);
153  pnh_.param("footstep_size_y", foot_size_y_, 0.14);
154  pnh_.param("footstep_size_z", foot_size_z_, 0.01);
155  std::vector<double> lleg_footstep_offset, rleg_footstep_offset;
156  if (jsk_topic_tools::readVectorParameter(pnh_, "lleg_footstep_offset", lleg_footstep_offset)) {
157  lleg_footstep_offset_[0] = lleg_footstep_offset[0];
158  lleg_footstep_offset_[1] = lleg_footstep_offset[1];
159  lleg_footstep_offset_[2] = lleg_footstep_offset[2];
160  }
161  if (jsk_topic_tools::readVectorParameter(pnh_, "rleg_footstep_offset", rleg_footstep_offset)) {
162  rleg_footstep_offset_[0] = rleg_footstep_offset[0];
163  rleg_footstep_offset_[1] = rleg_footstep_offset[1];
164  rleg_footstep_offset_[2] = rleg_footstep_offset[2];
165  }
166 
167  // query bbox size and offset
168  ros::NodeHandle nh;
169  ros::ServiceClient bbox_client = nh.serviceClient<jsk_footstep_planner::CollisionBoundingBoxInfo>("footstep_planner/collision_bounding_box_info", false);
170  ROS_INFO("waiting for %s", bbox_client.getService().c_str());
171  bbox_client.waitForExistence();
172  jsk_footstep_planner::CollisionBoundingBoxInfo bbox_info;
173  if (bbox_client.call(bbox_info)) {
174  collision_bbox_size_[0] = bbox_info.response.box_dimensions.x;
175  collision_bbox_size_[1] = bbox_info.response.box_dimensions.y;
176  collision_bbox_size_[2] = bbox_info.response.box_dimensions.z;
177  tf::poseMsgToEigen(bbox_info.response.box_offset, collision_bbox_offset_);
178  }
179 
180  // pose stamped command interface
182 
183  // service servers
184  srv_reset_fs_marker_ = pnh_.advertiseService("reset_marker",
186  srv_toggle_fs_com_mode_ = pnh_.advertiseService("toggle_footstep_marker_mode",
188  srv_execute_footstep_ = pnh_.advertiseService("execute_footstep",
190  srv_wait_for_exec_fs_ = pnh_.advertiseService("wait_for_execute",
192  srv_wait_for_fs_plan_ = pnh_.advertiseService("wait_for_plan",
194  srv_get_fs_marker_pose_ = pnh_.advertiseService("get_footstep_marker_pose",
196  srv_stack_marker_pose_ = pnh_.advertiseService("stack_marker_pose",
198 
199  pub_plan_result_ = pnh_.advertise<jsk_footstep_msgs::FootstepArray>("output/plan_result", 1);
200  pub_current_marker_mode_ = pnh_.advertise<jsk_rviz_plugins::OverlayText>("marker_mode", 1, true);
201 
202  //ROS_INFO("waiting for footstep_planner");
203  //ac_planner_.waitForServer();
204  //ROS_INFO("waiting for footstep_controller");
205  //ac_exec_.waitForServer();
206  // initialize interactive marker
207  // build menu handler
211  ROS_INFO("initialization done");
212  }
213 
215  {
218  ros::Duration(1.0).sleep();
219  }
220 
221  visualization_msgs::Marker FootstepMarker::makeFootstepMarker(FootstepTrans pose, unsigned char leg)
222  {
223  FootstepTrans footpose = pose;
224  if (leg == jsk_footstep_msgs::Footstep::LLEG) {
226  footpose = pose * ltrans;
227  } else if (leg == jsk_footstep_msgs::Footstep::RLEG) {
229  footpose = pose * rtrans;
230  } else {
231  ROS_ERROR ("makeFootstepMarker not implemented leg (%d)", leg);
232  }
233  visualization_msgs::Marker marker;
234  if (is_cube_mode_) {
235  marker.type = visualization_msgs::Marker::CUBE;
236  marker.scale.x = foot_size_x_;
237  marker.scale.y = foot_size_y_;
238  marker.scale.z = foot_size_z_;
239  tf::poseEigenToMsg(footpose, marker.pose);
240  }
241  else {
242  marker.type = visualization_msgs::Marker::LINE_STRIP;
243  marker.scale.x = 0.01;
244  FootstepVec local_a( foot_size_x_ / 2.0, foot_size_y_ / 2.0, 0.0);
245  FootstepVec local_b(-foot_size_x_ / 2.0, foot_size_y_ / 2.0, 0.0);
246  FootstepVec local_c(-foot_size_x_ / 2.0, -foot_size_y_ / 2.0, 0.0);
247  FootstepVec local_d( foot_size_x_ / 2.0, -foot_size_y_ / 2.0, 0.0);
248  FootstepVec a = footpose * local_a;
249  FootstepVec b = footpose * local_b;
250  FootstepVec c = footpose * local_c;
251  FootstepVec d = footpose * local_d;
252  geometry_msgs::Point ros_a, ros_b, ros_c, ros_d;
253  ros_a.x = a[0]; ros_a.y = a[1]; ros_a.z = a[2];
254  ros_b.x = b[0]; ros_b.y = b[1]; ros_b.z = b[2];
255  ros_c.x = c[0]; ros_c.y = c[1]; ros_c.z = c[2];
256  ros_d.x = d[0]; ros_d.y = d[1]; ros_d.z = d[2];
257  marker.points.push_back(ros_a);
258  marker.points.push_back(ros_b);
259  marker.points.push_back(ros_c);
260  marker.points.push_back(ros_d);
261  marker.points.push_back(ros_a);
262  }
263  marker.color.a = 1.0;
264  return marker;
265  }
266 
268  visualization_msgs::InteractiveMarker& int_marker)
269  {
270  FootstepTrans midcoords = leg_poses->midcoords();
271  tf::poseEigenToMsg(midcoords, int_marker.pose);
272  visualization_msgs::Marker left_box_marker = makeFootstepMarker(midcoords.inverse() * leg_poses->getByName(lleg_end_coords_),
273  jsk_footstep_msgs::Footstep::LLEG);
274  left_box_marker.color.g = 1.0;
275  visualization_msgs::Marker right_box_marker = makeFootstepMarker(midcoords.inverse() * leg_poses->getByName(rleg_end_coords_),
276  jsk_footstep_msgs::Footstep::RLEG);
277  right_box_marker.color.r = 1.0;
278  visualization_msgs::InteractiveMarkerControl left_box_control;
279  left_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
280  left_box_control.always_visible = true;
281  left_box_control.markers.push_back(left_box_marker);
282  int_marker.controls.push_back(left_box_control);
283  visualization_msgs::InteractiveMarkerControl right_box_control;
284  right_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
285  right_box_control.always_visible = true;
286  right_box_control.markers.push_back(right_box_marker);
287  int_marker.controls.push_back(right_box_control);
288  int_marker.name = "initial_footstep_marker";
289  int_marker.description = "Initial Footsteps";
290  }
291 
293  visualization_msgs::InteractiveMarker& int_goal_marker)
294  {
295  int_goal_marker.name = "movable_footstep_marker";
296  int_goal_marker.description = "Goal Footsteps";
297  tf::poseEigenToMsg(pose, int_goal_marker.pose);
298  current_lleg_offset_ = pose.inverse() * lleg_goal_pose_;
299  current_rleg_offset_ = pose.inverse() * rleg_goal_pose_;
300  visualization_msgs::Marker left_box_marker = makeFootstepMarker(current_lleg_offset_, jsk_footstep_msgs::Footstep::LLEG);
301  left_box_marker.color.g = 1.0;
302  visualization_msgs::Marker right_box_marker = makeFootstepMarker(current_rleg_offset_, jsk_footstep_msgs::Footstep::RLEG);
303  right_box_marker.color.r = 1.0;
304  visualization_msgs::InteractiveMarkerControl left_box_control;
305  left_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
306  left_box_control.always_visible = true;
307  left_box_control.markers.push_back(left_box_marker);
308  int_goal_marker.controls.push_back(left_box_control);
309  visualization_msgs::InteractiveMarkerControl right_box_control;
310  right_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
311  right_box_control.always_visible = true;
312  right_box_control.markers.push_back(right_box_marker);
313  int_goal_marker.controls.push_back(right_box_control);
314  }
315 
317  {
318  ROS_INFO("reset marker");
319  PosePair::Ptr leg_poses;
320  if (disable_tf_) {
321  leg_poses = getDefaultFootstepPair();
322  }
323  else {
325  leg_poses = getLatestCurrentFootstepPoses();
326  } else { // !single_mode && have_last_step_
327  FootstepTrans lleg_pose;
328  FootstepTrans rleg_pose;
329  tf::poseMsgToEigen(last_steps_[0].pose, lleg_pose);
330  tf::poseMsgToEigen(last_steps_[1].pose, rleg_pose);
331  leg_poses =
332  PosePair::Ptr(new PosePair(lleg_pose, lleg_end_coords_,
333  rleg_pose, rleg_end_coords_));
334  }
335  }
336  original_foot_poses_ = leg_poses;
337  visualization_msgs::InteractiveMarker int_marker;
338  int_marker.header.frame_id = odom_frame_id_;
339 
340  setupInitialMarker(leg_poses, int_marker);
341  server_->insert(int_marker,
342  boost::bind(&FootstepMarker::processFeedbackCB, this, _1));
343 
344  visualization_msgs::InteractiveMarker int_goal_marker;
345  int_goal_marker.header.frame_id = odom_frame_id_;
346  lleg_goal_pose_ = leg_poses->getByName(lleg_end_coords_);
347  rleg_goal_pose_ = leg_poses->getByName(rleg_end_coords_);
348  setupGoalMarker(leg_poses->midcoords(), int_goal_marker);
349  if (is_2d_mode_) {
350  add3Dof2DControl(int_goal_marker, false);
351  }
352  else {
353  im_helpers::add6DofControl(int_goal_marker, false);
354  }
355 
356  server_->insert(int_goal_marker,
357  boost::bind(&FootstepMarker::processFeedbackCB, this, _1));
358  menu_handler_.apply(*server_, "movable_footstep_marker");
359  menu_handler_.apply(*server_, "initial_footstep_marker");
360  server_->applyChanges();
361  updateMarkerArray(int_marker.header, int_goal_marker.pose);
362  }
363 
365  {
366  menu_handler_.insert("Reset Marker", boost::bind(&FootstepMarker::resetMarkerCB, this, _1));
367  menu_handler_.insert("Execute Footstep", boost::bind(&FootstepMarker::executeFootstepCB, this, _1));
368  stack_btn_ = menu_handler_.insert("Stack Footstep", boost::bind(&FootstepMarker::stackFootstepCB, this, _1));
371  entry_2d_mode_ = menu_handler_.insert(mode_handle, "2D mode", boost::bind(&FootstepMarker::enable2DCB, this, _1));
372  entry_3d_mode_ = menu_handler_.insert(mode_handle, "3D mode", boost::bind(&FootstepMarker::enable3DCB, this, _1));
373  if (is_2d_mode_) {
378  }
379  else {
384  }
385 
386  interactive_markers::MenuHandler::EntryHandle vis_mode_handle = menu_handler_.insert("Visualization");
387  cube_mode_ = menu_handler_.insert(vis_mode_handle, "Cube", boost::bind(&FootstepMarker::enableCubeCB, this, _1));
388  line_mode_ = menu_handler_.insert(vis_mode_handle, "Line", boost::bind(&FootstepMarker::enableLineCB, this, _1));
389  if (is_cube_mode_) {
392  }
393  else {
396  }
397 
399  single_mode_ = menu_handler_.insert(p_mode_handle, "Single", boost::bind(&FootstepMarker::enableSingleCB, this, _1));
400  cont_mode_ = menu_handler_.insert(p_mode_handle, "Continuous", boost::bind(&FootstepMarker::enableContinuousCB, this, _1));
401  stack_mode_ = menu_handler_.insert(p_mode_handle, "Stack", boost::bind(&FootstepMarker::enableStackCB, this, _1)); // TODO
405  switch(command_mode_) {
406  case SINGLE:
408  break;
409  case CONTINUOUS:
411  break;
412  case STACK:
413  stacked_poses_.resize(0);
414  stacked_poses_.push_back(original_foot_poses_->midcoords());
417  break;
418  }
419  }
420 
422  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
423  {
425  if (state.isDone()) { // Do not reset last step while footsteps are executed
426  have_last_step_ = false;
427  }
429  }
430 
432  const ExecResult::ConstPtr &result)
433  {
434  ROS_INFO("Done footsteps");
436  }
437 
439  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
440  {
443  FootstepTrans midcoords = goal_pose_pair->midcoords();
444  stacked_poses_.push_back(midcoords);
445 
446  updateMarkerArray(feedback->header, feedback->pose);
447  }
448 
450  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
451  {
452  // Lock footstep planner
453  boost::mutex::scoped_lock lock(planner_mutex_);
454  if (planning_state_ == FINISHED) {
455  jsk_footstep_msgs::ExecFootstepsGoal goal;
457  goal.footstep = plan_result_;
458  if (command_mode_ == SINGLE) {
459  //if(ac_exec_.getState() == actionlib::SimpleClientGoalState::ACTIVE) {
460  // ac_exec_.waitForResult(ros::Duration(120.0));
461  // return;
462  //}
463  goal.strategy = jsk_footstep_msgs::ExecFootstepsGoal::NEW_TARGET;
464  have_last_step_ = false;
465 
466  ROS_INFO("Execute footsteps single");
467  ac_exec_.sendGoal(goal, boost::bind(&FootstepMarker::executeDoneCB, this, _1, _2));
468  } else { // continuous mode
469  if (have_last_step_ ) {
470  goal.strategy = jsk_footstep_msgs::ExecFootstepsGoal::RESUME;
471  } else {
472  goal.strategy = jsk_footstep_msgs::ExecFootstepsGoal::NEW_TARGET;
473  }
474 
475  int size = plan_result_.footsteps.size();
476  {
477  if(plan_result_.footsteps[size-1].leg == jsk_footstep_msgs::Footstep::LEFT) {
478  last_steps_[0] = plan_result_.footsteps[size-1]; // left
479  last_steps_[1] = plan_result_.footsteps[size-2]; // right
480  } else {
481  last_steps_[0] = plan_result_.footsteps[size-2]; // left
482  last_steps_[1] = plan_result_.footsteps[size-1]; // right
483  }
484  }
485  have_last_step_ = true;
486  if (goal.strategy == jsk_footstep_msgs::ExecFootstepsGoal::NEW_TARGET) {
487  ROS_INFO("Execute footsteps continuous(new)");
488  } else {
489  ROS_INFO("Execute footsteps continuous(added)");
490  }
491  // wait result or ...
492  if (ac_exec_.isServerConnected()) {
493  ac_exec_.sendGoal(goal, boost::bind(&FootstepMarker::executeDoneCB, this, _1, _2));
494  } else {
495  ROS_FATAL("actionlib server is not connected");
496  }
498  }
499  }
500  else if (planning_state_ == ON_GOING) {
501  ROS_FATAL("cannot execute footstep because planning state is ON_GOING");
502  }
503  else if (planning_state_ == NOT_STARTED) {
504  ROS_FATAL("cannot execute footstep because planning state is NOT_STARTED");
505  }
506  }
507 
509  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
510  {
512  menu_handler_.getCheckState(cube_mode_, check_state);
513  if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
517  is_cube_mode_ = true;
519  }
520  }
521 
523  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
524  {
526  menu_handler_.getCheckState(line_mode_, check_state);
527  if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
531  is_cube_mode_ = false;
533  }
534  }
535 
537  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
538  {
541  if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
545  // remove 6dof marker and use 3dof marker
546  is_2d_mode_ = true;
548  }
549  }
550 
552  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
553  {
556  if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
560  // remove 3dof marker and use 6dof marker
561  is_2d_mode_ = false;
563  }
564  }
565 
567  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
568  {
571  if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
580  { // change heuristic
581  dynamic_reconfigure::Reconfigure rconf;
582  dynamic_reconfigure::StrParameter spara;
583  spara.name = "heuristic";
584  spara.value = "path_cost";
585  rconf.request.config.strs.push_back(spara);
586  if (!ros::service::call("footstep_planner/set_parameters", rconf)) {
587  // ERROR
588  ROS_ERROR("Dynamic reconfigure: set parameters failed");
589  return;
590  }
591  }
592  }
593  }
594 
596  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
597  {
599  menu_handler_.getCheckState(cont_mode_, check_state);
600  if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
609  { // change heuristic
610  dynamic_reconfigure::Reconfigure rconf;
611  dynamic_reconfigure::StrParameter spara;
612  spara.name = "heuristic";
613  spara.value = "path_cost";
614  rconf.request.config.strs.push_back(spara);
615  if (!ros::service::call("footstep_planner/set_parameters", rconf)) {
616  // ERROR
617  ROS_ERROR("Dynamic reconfigure: set parameters failed");
618  return;
619  }
620  }
621  }
622  }
623 
625  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
626  {
629  if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
636  stacked_poses_.resize(0);
637  stacked_poses_.push_back(original_foot_poses_->midcoords());
640  { // change heuristic
641  dynamic_reconfigure::Reconfigure rconf;
642  dynamic_reconfigure::StrParameter spara;
643  spara.name = "heuristic";
644  spara.value = "follow_path";
645  rconf.request.config.strs.push_back(spara);
646  if (!ros::service::call("footstep_planner/set_parameters", rconf)) {
647  // ERROR
648  ROS_ERROR("Dynamic reconfigure: set parameters failed");
649  return;
650  }
651  }
652  }
653  }
654 
656  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
657  {
658  }
659 
661  {
662  boost::mutex::scoped_lock lock(planner_mutex_);
663  if (planning_state_ == ON_GOING) {
666  }
667  }
668 
669  jsk_footstep_msgs::FootstepArray FootstepMarker::footstepArrayFromPosePair(PosePair::Ptr pose_pair,
670  const std_msgs::Header& header,
671  bool is_lleg_first)
672  {
673  jsk_footstep_msgs::FootstepArray footstep_array;
674  footstep_array.header = header;
675  jsk_footstep_msgs::Footstep lleg = footstepFromEigenPose(pose_pair->getByName(lleg_end_coords_));
676  jsk_footstep_msgs::Footstep rleg = footstepFromEigenPose(pose_pair->getByName(rleg_end_coords_));
677  lleg.leg = jsk_footstep_msgs::Footstep::LEFT;
678  rleg.leg = jsk_footstep_msgs::Footstep::RIGHT;
679  lleg.dimensions.x = foot_size_x_;
680  lleg.dimensions.y = foot_size_y_;
681  lleg.dimensions.z = foot_size_z_;
682  rleg.dimensions.x = foot_size_x_;
683  rleg.dimensions.y = foot_size_y_;
684  rleg.dimensions.z = foot_size_z_;
685  if (is_lleg_first) {
686  footstep_array.footsteps.push_back(lleg);
687  footstep_array.footsteps.push_back(rleg);
688  }
689  else {
690  footstep_array.footsteps.push_back(rleg);
691  footstep_array.footsteps.push_back(lleg);
692  }
693  return footstep_array;
694  }
695 
696  void FootstepMarker::plan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
697  {
698  std_msgs::Header header;
699  header.frame_id = odom_frame_id_;
700  header.stamp = ros::Time::now();
701  // first, project footstep using footstep_planner/project_footprint_with_local_search API.
702  jsk_interactive_marker::SnapFootPrint srv_arg;
703 
706  srv_arg.request.input_pose.header = header;
707  FootstepTrans midcoords = goal_pose_pair->midcoords();
708  FootstepTrans lleg_trans = midcoords.inverse() * lleg_goal_pose_;
709  FootstepTrans rleg_trans = midcoords.inverse() * rleg_goal_pose_;
710  tf::poseEigenToMsg(midcoords, srv_arg.request.input_pose.pose);
711  tf::poseEigenToMsg(lleg_trans, srv_arg.request.lleg_pose);
712  tf::poseEigenToMsg(rleg_trans, srv_arg.request.rleg_pose);
713  if (ros::service::call("footstep_planner/project_footprint_with_local_search", srv_arg)) {
714  if (srv_arg.response.success) {
715  FootstepTrans new_center_pose;
716  tf::poseMsgToEigen(srv_arg.response.snapped_pose.pose, new_center_pose);
717  goal_pose_pair.reset(new PosePair(new_center_pose * lleg_trans, lleg_end_coords_,
718  new_center_pose * rleg_trans, rleg_end_coords_));
719  }
720  else {
721  ROS_ERROR("Failed to project goal"); // should display message
722  return;
723  }
724  }
725  jsk_footstep_msgs::PlanFootstepsGoal goal;
726  goal.goal_footstep
727  = footstepArrayFromPosePair(goal_pose_pair, header, true);
728  goal.initial_footstep
730 
731  ac_planner_.sendGoal(goal, boost::bind(&FootstepMarker::planDoneCB, this, _1, _2));
733  }
734 
736  const PlanResult::ConstPtr &result)
737  {
738  boost::mutex::scoped_lock lock(planner_mutex_);
739  pub_plan_result_.publish(result->result);
741  plan_result_ = result->result;
742  }
743 
744  void FootstepMarker::planIfPossible(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
745  {
746  boost::mutex::scoped_lock lock(planner_mutex_);
747  if (planning_state_ != ON_GOING) {
748  ROS_INFO("start planning");
749  plan(feedback);
750  }
751  }
752 
755  }
756 
759  }
760 
762  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
763  {
764  if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP) {
765  ROS_INFO("mouse_up");
766  if(command_mode_ != STACK) {
767  ROS_INFO("cancel and planning");
768  cancelPlanning();
769  plan(feedback);
770  } else {
771  FootstepTrans current_marker_pose;
772  tf::poseMsgToEigen(feedback->pose, current_marker_pose);
773  lleg_goal_pose_ = current_marker_pose * current_lleg_offset_;
774  rleg_goal_pose_ = current_marker_pose * current_rleg_offset_;
775  }
776  }
777  else if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN) {
778 
779  }
780  else if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) {
781  ROS_INFO("pose_up");
782  // update position of goal footstep
783  FootstepTrans current_marker_pose;
784  tf::poseMsgToEigen(feedback->pose, current_marker_pose);
785  lleg_goal_pose_ = current_marker_pose * current_lleg_offset_;
786  rleg_goal_pose_ = current_marker_pose * current_rleg_offset_;
787  if(command_mode_ != STACK) {
788  planIfPossible(feedback);
789  } else { // stack mode
790  if (planning_state_ != ON_GOING) {
791  ROS_INFO("follow plan");
792  callFollowPathPlan(feedback);
793  }
794  }
795  }
796  updateMarkerArray(feedback->header, feedback->pose);
797  }
798 
799  visualization_msgs::Marker FootstepMarker::targetArrow(
800  const std_msgs::Header& header, const geometry_msgs::Pose& pose)
801  {
802  visualization_msgs::Marker marker;
803  marker.header = header;
804  marker.type = visualization_msgs::Marker::ARROW;
805  marker.scale.x = 0.1;
806  marker.scale.y = 0.01;
807  marker.scale.z = 0.01;
808  marker.color.a = 1.0;
809  marker.color.r = 1.0;
810  marker.pose = pose;
811  return marker;
812  }
813 
814  visualization_msgs::Marker FootstepMarker::distanceLineMarker(
815  const std_msgs::Header& header, const geometry_msgs::Pose& pose)
816  {
817  visualization_msgs::Marker marker;
818  marker.header = header;
819  marker.type = visualization_msgs::Marker::LINE_LIST;
820  marker.scale.x = 0.01;
821  marker.color.a = 1.0;
822  marker.color.r = 24 / 255.0;
823  marker.color.g = 240 / 255.0;
824  marker.color.b = 1.0;
825  FootstepTrans origin = original_foot_poses_->midcoords();
826  FootstepTrans posef;
827  tf::poseMsgToEigen(pose, posef);
828  FootstepVec direction(posef.translation() - origin.translation());
829  FootstepVec normalized_direction = direction.normalized();
830  FootstepVec original_x_direction = origin.rotation() * FootstepVec::UnitX();
831  FootstepVec rotate_axis = original_x_direction.cross(normalized_direction).normalized();
832  double pose_theta = acos(original_x_direction.dot(normalized_direction));
833  FootstepTrans transform;
834  if (pose_theta == 0.0) {
835  transform = origin * FootstepTrans::Identity();
836  }
837  else {
838  //transform = origin * FootstepTranslation(-origin.translation()) * Eigen::AngleAxisf(pose_theta, rotate_axis);
839  transform = origin * FootstepAngleAxis(pose_theta, rotate_axis);
840  }
841  double distance = (origin.inverse() * posef).translation().norm();
842  double r = distance / (2.0 * M_PI);
843  // (x, y) = r(theta - sin, 1 - cos)
844  const size_t resolustion = 100;
845  const double max_z = 1.0;
846  const double z_ratio = max_z / 2.0 / r;
847  for (size_t i = 0; i < resolustion - 1; i++) {
848  double theta = 2.0 * M_PI / resolustion * i;
849  double next_theta = 2.0 * M_PI / resolustion * (i + 1);
850  FootstepVec p = transform * FootstepVec(r * (theta - sin(theta)), 0, r * (1.0 - cos(theta)) * z_ratio);
851  FootstepVec q = transform * FootstepVec(r * (next_theta - sin(next_theta)), 0, r * (1.0 - cos(next_theta)) * z_ratio);
852  geometry_msgs::Point ros_p;
853  ros_p.x = p[0];
854  ros_p.y = p[1];
855  ros_p.z = p[2];
856  geometry_msgs::Point ros_q;
857  ros_q.x = q[0];
858  ros_q.y = q[1];
859  ros_q.z = q[2];
860  marker.colors.push_back(marker.color);
861  marker.points.push_back(ros_p);
862  marker.colors.push_back(marker.color);
863  marker.points.push_back(ros_q);
864  }
865  return marker;
866  }
867 
868  visualization_msgs::Marker FootstepMarker::originMarker(
869  const std_msgs::Header& header, const geometry_msgs::Pose& pose)
870  {
871  visualization_msgs::Marker marker;
872  marker.header = header;
873  marker.type = visualization_msgs::Marker::LINE_STRIP;
874  marker.scale.x = 0.01;
875  marker.color.a = 1.0;
876  marker.color.r = 24 / 255.0;
877  marker.color.g = 240 / 255.0;
878  marker.color.b = 1.0;
879  FootstepTrans origin = original_foot_poses_->midcoords();
880  tf::poseEigenToMsg(origin, marker.pose);
881  const size_t resolution = 100;
882  const double r = 0.5;
883  for (size_t i = 0; i < resolution + 1; i++) {
884  double theta = 2.0 * M_PI / resolution * i;
885  FootstepVec p(r * cos(theta), r * sin(theta), 0.0);
886  geometry_msgs::Point ros_p;
887  ros_p.x = p[0];
888  ros_p.y = p[1];
889  ros_p.z = p[2];
890  marker.points.push_back(ros_p);
891  marker.colors.push_back(marker.color);
892  }
893  return marker;
894  }
895 
896  visualization_msgs::Marker FootstepMarker::distanceTextMarker(
897  const std_msgs::Header& header, const geometry_msgs::Pose& pose)
898  {
899  visualization_msgs::Marker marker;
900  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
901  Eigen::Affine3f posef;
902  tf::poseMsgToEigen(pose, posef);
903  Eigen::Affine3f text_pose = posef * Eigen::Translation3f(-0.1, 0, 0.1);
904  Eigen::Affine3f midcoords;
905  jsk_recognition_utils::convertEigenAffine3(original_foot_poses_->midcoords().inverse(), midcoords);
906  Eigen::Affine3f transform = midcoords * posef;
907  Eigen::Vector3f pos(transform.translation());
908  float roll, pitch, yaw;
909  pcl::getEulerAngles(transform, roll, pitch, yaw);
910 
911  marker.text = (boost::format("pos[m] = (%.2f, %.2f, %.2f)\nrot[deg] = (%.2f, %.2f, %.2f)\n%.2f [m]\n%.0f [deg]")
912  % (pos[0]) % (pos[1]) % (pos[2])
913  % (angles::to_degrees(roll)) % (angles::to_degrees(pitch)) % (angles::to_degrees(yaw))
914  % (pos.norm()) % (Eigen::AngleAxisf(transform.rotation()).angle() / M_PI * 180)).str();
915  //marker.pose = pose;
916  tf::poseEigenToMsg(text_pose, marker.pose);
917  marker.header = header;
918  marker.scale.z = 0.1;
919  marker.color.a = 1.0;
920  marker.color.r = 1.0;
921  marker.color.g = 1.0;
922  marker.color.b = 1.0;
923  return marker;
924  }
925 
926  visualization_msgs::Marker FootstepMarker::originBoundingBoxMarker(
927  const std_msgs::Header& header, const geometry_msgs::Pose& pose)
928  {
929  visualization_msgs::Marker marker;
930  marker.header = header;
931  marker.type = visualization_msgs::Marker::CUBE;
932  marker.scale.x = collision_bbox_size_[0];
933  marker.scale.y = collision_bbox_size_[1];
934  marker.scale.z = collision_bbox_size_[2];
935  marker.color.a = 0.3;
936  marker.color.r = 1.0;
938  tf::poseEigenToMsg(box_pose, marker.pose);
939  return marker;
940  }
941 
942  visualization_msgs::Marker FootstepMarker::goalBoundingBoxMarker(
943  const std_msgs::Header& header, const geometry_msgs::Pose& pose)
944  {
945  visualization_msgs::Marker marker;
946  marker.header = header;
947  marker.type = visualization_msgs::Marker::CUBE;
948  marker.scale.x = collision_bbox_size_[0];
949  marker.scale.y = collision_bbox_size_[1];
950  marker.scale.z = collision_bbox_size_[2];
951  marker.color.a = 0.3;
952  marker.color.r = 1.0;
953  FootstepTrans input_pose;
954  tf::poseMsgToEigen(pose, input_pose);
955  FootstepTrans box_pose = input_pose * collision_bbox_offset_;
956  tf::poseEigenToMsg(box_pose, marker.pose);
957  return marker;
958  }
959 
960  visualization_msgs::Marker FootstepMarker::stackedPosesMarker(
961  const std_msgs::Header& header, const geometry_msgs::Pose& pose)
962  {
963  visualization_msgs::Marker marker;
964  marker.header = header;
965  if (command_mode_ != STACK) {
966  return marker;
967  }
968  marker.type = visualization_msgs::Marker::LINE_LIST;
969  marker.scale.x = 0.025; // line width
970  std_msgs::ColorRGBA col;
971  col.a = 1.0;
972  col.r = 1.0;
973  col.g = 240 / 255.0;
974  col.b = 24 / 255.0;
975  marker.color = col;
976 
977  for(int i=1; i < stacked_poses_.size(); i++) {
978  FootstepVec p = stacked_poses_[i-1].translation();
979  FootstepVec q = stacked_poses_[i].translation();
980  geometry_msgs::Point ros_p;
981  ros_p.x = p[0];
982  ros_p.y = p[1];
983  ros_p.z = p[2];
984  geometry_msgs::Point ros_q;
985  ros_q.x = q[0];
986  ros_q.y = q[1];
987  ros_q.z = q[2];
988  marker.colors.push_back(col);
989  marker.points.push_back(ros_p);
990  marker.colors.push_back(col);
991  marker.points.push_back(ros_q);
992  }
993  // add current
994  FootstepVec p = stacked_poses_[stacked_poses_.size() - 1].translation();
995  geometry_msgs::Point ros_p;
996  ros_p.x = p[0];
997  ros_p.y = p[1];
998  ros_p.z = p[2];
999  geometry_msgs::Point ros_q;
1000  ros_q.x = pose.position.x;
1001  ros_q.y = pose.position.y;
1002  ros_q.z = pose.position.z;
1003  col.r = 1.0;
1004  col.g = 80 / 255.0;
1005  col.b = 80 / 255.0;
1006  marker.colors.push_back(col);
1007  marker.points.push_back(ros_p);
1008  marker.colors.push_back(col);
1009  marker.points.push_back(ros_q);
1010 
1011  return marker;
1012  }
1013 
1014  void FootstepMarker::updateMarkerArray(const std_msgs::Header& header, const geometry_msgs::Pose& pose)
1015  {
1016  pub_marker_array_.insert("target arrow", targetArrow(header, pose));
1017  pub_marker_array_.insert("distance text", distanceTextMarker(header, pose));
1018  pub_marker_array_.insert("distance line", distanceLineMarker(header, pose));
1019  pub_marker_array_.insert("origin", originMarker(header, pose));
1020  pub_marker_array_.insert("origin_bbox", originBoundingBoxMarker(header, pose));
1021  pub_marker_array_.insert("goal_bbox", goalBoundingBoxMarker(header, pose));
1022  pub_marker_array_.insert("stacked_pose", stackedPosesMarker(header, pose));
1024  }
1025 
1027  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
1028  {
1029  }
1030 
1032  {
1033  FootstepTrans lleg_default_pose(FootstepTranslation(0, 0.1, 0));
1034  FootstepTrans rleg_default_pose(FootstepTranslation(0, -0.1, 0));
1035  return PosePair::Ptr(new PosePair(lleg_default_pose, lleg_end_coords_,
1036  rleg_default_pose, rleg_end_coords_));
1037  }
1038 
1040  {
1041  while (ros::ok())
1042  {
1043  try
1044  {
1046  }
1047  catch (tf2::TransformException& e)
1048  {
1049  ROS_WARN("tf error, retry: %s", e.what());
1050  }
1051  }
1052  }
1053 
1055  {
1056  // use tf2
1057  // odom -> lleg_end_coords
1058  geometry_msgs::TransformStamped lleg_transform
1059  = tf_client_->lookupTransform(odom_frame_id_, lleg_end_coords_, stamp);
1060  geometry_msgs::TransformStamped rleg_transform
1061  = tf_client_->lookupTransform(odom_frame_id_, rleg_end_coords_, stamp);
1062  FootstepTrans lleg_transform_eigen, rleg_transform_eigen;
1063  tf::transformMsgToEigen(lleg_transform.transform, lleg_transform_eigen);
1064  tf::transformMsgToEigen(rleg_transform.transform, rleg_transform_eigen);
1065  PosePair::Ptr ppair (new PosePair(lleg_transform_eigen, lleg_end_coords_,
1066  rleg_transform_eigen, rleg_end_coords_));
1067  if(use_default_goal_) {
1068  return PosePair::Ptr(new PosePair(ppair->midcoords() * getDefaultLeftLegOffset(), lleg_end_coords_,
1069  ppair->midcoords() * getDefaultRightLegOffset(), rleg_end_coords_));
1070  }
1071  return ppair;
1072  }
1073 
1074  void FootstepMarker::configCallback(Config &config, uint32_t level)
1075  {
1076  boost::mutex::scoped_lock lock(planner_mutex_);
1077  disable_tf_ = config.disable_tf;
1078  default_footstep_margin_ = config.default_footstep_margin;
1079  use_default_goal_ = config.use_default_step_as_goal;
1080  }
1081 
1082 
1084  const geometry_msgs::PoseStamped::ConstPtr& msg)
1085  {
1086  ROS_DEBUG("posestamped command is received");
1087  ROS_INFO("posestamped command is received");
1088  geometry_msgs::PoseStamped tmp_pose_stamped;
1089  if(msg->header.frame_id != odom_frame_id_ && !disable_tf_) {
1090  // apply target pose to goal marker
1091  // mutex should be limited in this range because planIfPossible also lock planner_mutex_
1092  boost::mutex::scoped_lock lock(planner_mutex_);
1093  try {
1094  geometry_msgs::TransformStamped offset = tf_client_->lookupTransform(odom_frame_id_, msg->header.frame_id,
1095  msg->header.stamp, ros::Duration(2.0));
1096  FootstepTrans msg_eigen;
1097  FootstepTrans offset_eigen;
1098  tf::poseMsgToEigen(msg->pose, msg_eigen);
1099  tf::transformMsgToEigen(offset.transform, offset_eigen);
1100  FootstepTrans pose = msg_eigen * offset_eigen;
1101 
1102  tf::poseEigenToMsg(pose, tmp_pose_stamped.pose);
1103  tmp_pose_stamped.header.stamp = msg->header.stamp;
1104  tmp_pose_stamped.header.frame_id = odom_frame_id_;
1105  } catch(tf2::TransformException ex) {
1106  ROS_ERROR("posestamped command transformation failed %s",ex.what());
1107  return;
1108  }
1109  } else {
1110  tmp_pose_stamped = *msg;
1111  }
1112  server_->setPose("movable_footstep_marker", tmp_pose_stamped.pose, tmp_pose_stamped.header);
1113  server_->applyChanges();
1114  // forcely call processFeedbackCB to execute planning
1115  visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
1116  dummy_feedback.header = tmp_pose_stamped.header;
1117  dummy_feedback.pose = tmp_pose_stamped.pose;
1118  dummy_feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
1119  const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
1120  = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
1121  processFeedbackCB(dummy_feedback_ptr);
1122  }
1123 
1125  std_srvs::Empty::Request& req,
1126  std_srvs::Empty::Response& res)
1127  {
1128  // forcely call resetMarkerCB to reset marker. feedback msg does not used in restMarkerCB.
1129  visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
1130  const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
1131  = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
1132  resetMarkerCB(dummy_feedback_ptr);
1133  return true;
1134  }
1135 
1137  std_srvs::Empty::Request& req,
1138  std_srvs::Empty::Response& res)
1139  {
1140  visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
1141  const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
1142  = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
1143 
1144  if (command_mode_ == SINGLE) {
1145  // single -> continuous
1146  enableContinuousCB(dummy_feedback_ptr);
1150  } else if (command_mode_ == CONTINUOUS) {
1151  // continuous -> stack
1152  enableStackCB(dummy_feedback_ptr);
1156  } else {
1157  // stack -> single
1158  enableSingleCB(dummy_feedback_ptr);
1162  }
1163  return true;
1164  }
1165 
1167  std_srvs::Empty::Request& req,
1168  std_srvs::Empty::Response& res)
1169  {
1170  // forcely call executeFootstepCB to execute footstep. feedback msg does not used in executeFootstepCB.
1171  visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
1172  const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
1173  = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
1174  executeFootstepCB(dummy_feedback_ptr);
1175  return true;
1176  // check footstep result
1178  return true;
1179  } else {
1180  return false;
1181  }
1182  }
1183 
1185  std_srvs::Empty::Request& req,
1186  std_srvs::Empty::Response& res)
1187  {
1188  bool result = true;
1190  if(!state.isDone()) {
1191  result = ac_exec_.waitForResult(ros::Duration(120.0));
1192  }
1193  return result;
1194  }
1195 
1197  std_srvs::Empty::Request& req,
1198  std_srvs::Empty::Response& res)
1199  {
1200  bool result = true;
1202  if(!state.isDone()) {
1203  result = ac_planner_.waitForResult(ros::Duration(120.0));
1204  }
1205  return result;
1206  }
1207 
1209  jsk_interactive_marker::GetTransformableMarkerPose::Request& req,
1210  jsk_interactive_marker::GetTransformableMarkerPose::Response& res)
1211  {
1212  boost::mutex::scoped_lock lock(planner_mutex_);
1213  std::string target_name = req.target_name;
1214  visualization_msgs::InteractiveMarker int_marker;
1215  if (server_->get(target_name, int_marker)) {
1216  geometry_msgs::PoseStamped ret_pose_stamped;
1217  ret_pose_stamped.header = int_marker.header;
1218  ret_pose_stamped.pose = int_marker.pose;
1219  res.pose_stamped = ret_pose_stamped;
1220  return true;
1221  } else {
1222  ROS_WARN("There is no marker named %s", target_name.c_str());
1223  return false;
1224  }
1225  }
1226 
1228  jsk_interactive_marker::SetPose::Request& req,
1229  jsk_interactive_marker::SetPose::Response& res)
1230  {
1231  ROS_INFO("stack marker service");
1232  if (command_mode_ != STACK) {
1233  return false;
1234  }
1235  visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
1236  dummy_feedback.header = req.pose.header;
1237  dummy_feedback.pose = req.pose.pose;
1238  const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
1239  = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
1240  stackFootstepCB(dummy_feedback_ptr);
1241  return true;
1242  }
1243 
1245  {
1246  std_msgs::ColorRGBA color;
1247  color.r = 0.3568627450980392;
1248  color.g = 0.7529411764705882;
1249  color.b = 0.8705882352941177;
1250  color.a = 1.0;
1251 
1252  std::string text;
1253  switch(command_mode_) {
1254  case SINGLE:
1255  text = "Single Mode";
1256  break;
1257  case CONTINUOUS:
1258  text = "Continuous Mode";
1259  break;
1260  case STACK:
1261  text = "Stack Mode";
1262  break;
1263  }
1264 
1265  jsk_rviz_plugins::OverlayText msg;
1266  msg.text = text;
1267  msg.width = 1000;
1268  msg.height = 1000;
1269  msg.top = 10;
1270  msg.left = 10;
1271  msg.bg_color.a = 0.0;
1272  msg.fg_color = color;
1273  msg.text_size = 24;
1275  }
1276 
1277  void FootstepMarker::callFollowPathPlan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
1278  {
1279  if(stacked_poses_.size() > 1) {
1280  boost::mutex::scoped_lock lock(planner_mutex_);
1281 
1282  jsk_footstep_planner::SetHeuristicPath srv_arg;
1283  for(int i = 0; i < stacked_poses_.size(); i++) {
1284  geometry_msgs::Point p;
1285  FootstepVec tl = stacked_poses_[i].translation();
1286  p.x = tl[0];
1287  p.y = tl[1];
1288  p.z = tl[2];
1289  srv_arg.request.segments.push_back(p);
1290  }
1291  { // add final pose
1292  geometry_msgs::Point p;
1295  FootstepVec tl = goal_pose_pair->midcoords().translation();
1296  p.x = tl[0];
1297  p.y = tl[1];
1298  p.z = tl[2];
1299  srv_arg.request.segments.push_back(p);
1300  }
1301  if (!ros::service::call("footstep_planner/set_heuristic_path", srv_arg)) {
1302  // ERROR
1303  ROS_ERROR("Service: failed to call footstep_planner/set_heuristic_path");
1304  return;
1305  }
1306  } else {
1307  return;
1308  }
1309  // plan
1310  plan(feedback);
1311  }
1312 }
1313 
1314 int main(int argc, char** argv)
1315 {
1316  ros::init(argc, argv, "footstep_marker");
1318  ros::spin();
1319 }
d
boost::shared_ptr< PosePair > Ptr
msg
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
virtual visualization_msgs::Marker distanceTextMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
virtual visualization_msgs::Marker goalBoundingBoxMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
virtual FootstepTrans getDefaultRightLegOffset()
virtual void planDoneCB(const actionlib::SimpleClientGoalState &state, const PlanResult::ConstPtr &result)
interactive_markers::MenuHandler::EntryHandle cont_mode_
#define ROS_FATAL(...)
jsk_footstep_msgs::Footstep last_steps_[2]
interactive_markers::MenuHandler::EntryHandle stack_mode_
virtual void configCallback(Config &config, uint32_t level)
virtual bool getFootstepMarkerPoseService(jsk_interactive_marker::GetTransformableMarkerPose::Request &req, jsk_interactive_marker::GetTransformableMarkerPose::Response &res)
pos
interactive_markers::MenuHandler::EntryHandle single_mode_
void publish(const boost::shared_ptr< M > &message) const
virtual void enableSingleCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void callFollowPathPlan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
bool getCheckState(EntryHandle handle, CheckState &check_state) const
virtual bool resetMarkerService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
double cos()
double sin()
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual void executeDoneCB(const actionlib::SimpleClientGoalState &state, const ExecResult::ConstPtr &result)
SINGLE
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
virtual void resetMarkerCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
bool call(const std::string &service_name, MReq &req, MRes &res)
pitch
bool sleep() const
virtual void plan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
virtual FootstepTrans getByName(const std::string &name)
virtual FootstepTrans getDefaultLeftLegOffset()
jsk_footstep_msgs::Footstep footstepFromEigenPose(Eigen::Affine3f pose)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
ROSCPP_DECL const std::string & getName()
virtual void updateMarkerArray(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
int main(int argc, char **argv)
virtual void stackFootstepCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
pose
#define ROS_WARN(...)
text
bool apply(InteractiveMarkerServer &server, const std::string &marker_name)
Eigen::Affine3d FootstepTrans
virtual bool waitForExecuteFootstepService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
virtual void enableContinuousCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Eigen::Quaterniond FootstepQuaternion
ROSCPP_DECL void spin(Spinner &spinner)
bool readVectorParameter(ros::NodeHandle &nh, const std::string &param_name, std::vector< double > &result)
virtual void setupInitialMarker(PosePair::Ptr leg_poses, visualization_msgs::InteractiveMarker &int_marker)
yaw
size
q
virtual PosePair::Ptr getLatestCurrentFootstepPoses()
c
result
bool setVisible(EntryHandle handle, bool visible)
GLfloat angle
bool setCheckState(EntryHandle handle, CheckState check_state)
#define M_PI
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
Eigen::Translation3d FootstepTranslation
virtual PosePair::Ptr getCurrentFootstepPoses(const ros::Time &stamp)
const Eigen::Vector3f resolution(0.05, 0.05, 0.08)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
interactive_markers::MenuHandler::EntryHandle entry_3d_mode_
header
virtual bool toggleFootstepMarkerModeService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
virtual void enableLineCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void add3Dof2DControl(visualization_msgs::InteractiveMarker &msg, bool fixed)
ROSCPP_DECL bool ok()
virtual bool waitForFootstepPlanService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
state
interactive_markers::MenuHandler::EntryHandle line_mode_
virtual void processMenuFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
static double to_degrees(double radians)
virtual FootstepTrans midcoords()
void convertEigenAffine3(const Eigen::Affine3d &from, Eigen::Affine3f &to)
jsk_footstep_msgs::FootstepArray plan_result_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
rot
virtual void processPoseUpdateCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
interactive_markers::MenuHandler::EntryHandle cube_mode_
virtual void insert(const std::string &name, visualization_msgs::Marker marker)
ex
virtual visualization_msgs::Marker originBoundingBoxMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
virtual void planIfPossible(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
mutex_t * lock
virtual void enableStackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
interactive_markers::MenuHandler::EntryHandle entry_2d_mode_
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
EntryHandle insert(const std::string &title, const FeedbackCallback &feedback_cb)
virtual visualization_msgs::Marker makeFootstepMarker(FootstepTrans pose, unsigned char leg)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
r
virtual void poseStampedCommandCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
virtual void enable2DCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Eigen::Vector3d FootstepVec
virtual bool stackMarkerPoseService(jsk_interactive_marker::SetPose::Request &req, jsk_interactive_marker::SetPose::Response &res)
void add6DofControl(visualization_msgs::InteractiveMarker &msg, bool fixed=false)
virtual jsk_footstep_msgs::FootstepArray footstepArrayFromPosePair(PosePair::Ptr pose_pair, const std_msgs::Header &header, bool is_lleg_first)
f
virtual void setupGoalMarker(FootstepTrans pose, visualization_msgs::InteractiveMarker &int_marker)
virtual PosePair::Ptr getDefaultFootstepPair()
static Time now()
Eigen::AngleAxisd FootstepAngleAxis
interactive_markers::MenuHandler::EntryHandle stack_btn_
std::string getService()
virtual visualization_msgs::Marker distanceLineMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
interactive_markers::MenuHandler menu_handler_
SimpleClientGoalState getState() const
std::vector< FootstepTrans > stacked_poses_
bool reApply(InteractiveMarkerServer &server)
virtual void processFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
virtual void executeFootstepCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
roll
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
virtual visualization_msgs::Marker targetArrow(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
#define ROS_ERROR(...)
virtual void enableCubeCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
double distance(const urdf::Pose &transform)
virtual visualization_msgs::Marker stackedPosesMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
virtual visualization_msgs::Marker originMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
char a[26]
virtual void enable3DCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
boost::shared_ptr< tf2_ros::BufferClient > tf_client_
#define ROS_DEBUG(...)
virtual bool executeFootstepService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri May 14 2021 02:51:52