footstep_marker.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_footstep_planner/footstep_marker.h"
00037 #include <jsk_topic_tools/log_utils.h>
00038 #include <jsk_recognition_utils/pcl_conversion_util.h>
00039 #include <jsk_interactive_marker/interactive_marker_helpers.h>
00040 #include <boost/format.hpp>
00041 #include <pcl/common/eigen.h>
00042 #include <angles/angles.h>
00043 #include "jsk_footstep_planner/footstep_conversions.h"
00044 #include <jsk_topic_tools/rosparam_utils.h>
00045 #include <jsk_interactive_marker/SnapFootPrint.h>
00046 #include <jsk_footstep_planner/CollisionBoundingBoxInfo.h>
00047 #include <dynamic_reconfigure/Reconfigure.h>
00048 
00049 #define printAffine(af) { \
00050   geometry_msgs::Pose __geom_pose;\
00051   tf::poseEigenToMsg(af, __geom_pose);\
00052   std::cerr << __geom_pose.position.x << ", ";\
00053   std::cerr << __geom_pose.position.y << ", ";\
00054   std::cerr << __geom_pose.position.z << " / ";\
00055   std::cerr << __geom_pose.orientation.w << ", ";\
00056   std::cerr << __geom_pose.orientation.x << ", ";\
00057   std::cerr << __geom_pose.orientation.y << ", ";\
00058   std::cerr << __geom_pose.orientation.z << std::endl; }
00059 
00060 namespace jsk_recognition_utils
00061 {
00062   void convertEigenAffine3(const Eigen::Affine3f& from,
00063                            Eigen::Affine3f& to)
00064   {
00065     to = from;
00066   }
00067 }
00068 
00069 namespace jsk_footstep_planner
00070 {
00071 
00072   void add3Dof2DControl( visualization_msgs::InteractiveMarker &msg, bool fixed)
00073   {
00074     visualization_msgs::InteractiveMarkerControl control;
00075 
00076     if(fixed)
00077       control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED;
00078 
00079     control.orientation.w = 1;
00080     control.orientation.x = 1;
00081     control.orientation.y = 0;
00082     control.orientation.z = 0;
00083     // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00084     // msg.controls.push_back(control);
00085     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00086     msg.controls.push_back(control);
00087 
00088     control.orientation.w = 1;
00089     control.orientation.x = 0;
00090     control.orientation.y = 1;
00091     control.orientation.z = 0;
00092     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00093     msg.controls.push_back(control);
00094     // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00095     // msg.controls.push_back(control);
00096 
00097     control.orientation.w = 1;
00098     control.orientation.x = 0;
00099     control.orientation.y = 0;
00100     control.orientation.z = 1;
00101     // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00102     // msg.controls.push_back(control);
00103     control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00104     msg.controls.push_back(control);
00105 
00106   }
00107 
00108   PosePair::PosePair(const FootstepTrans& first, const std::string& first_name,
00109                      const FootstepTrans& second, const std::string& second_name):
00110     first_(first), first_name_(first_name),
00111     second_(second), second_name_(second_name)
00112   {
00113 
00114   }
00115 
00116   FootstepTrans PosePair::getByName(const std::string& name)
00117   {
00118     if (first_name_ == name) {
00119       return first_;
00120     }
00121     else if (second_name_ == name) {
00122       return second_;
00123     }
00124     else {
00125       throw UnknownPoseName();
00126     }
00127   }
00128 
00129   FootstepTrans PosePair::midcoords()
00130   {
00131     FootstepTranslation pos((FootstepVec(first_.translation()) + FootstepVec(second_.translation())) / 2.0);
00132     FootstepQuaternion rot = FootstepQuaternion(first_.rotation()).slerp(0.5, FootstepQuaternion(second_.rotation()));
00133     return pos * rot;
00134   }
00135 
00136   FootstepMarker::FootstepMarker():
00137     pnh_("~"), ac_planner_("footstep_planner", true), ac_exec_("footstep_controller", true),
00138     pub_marker_array_(pnh_, "marker_array"),
00139     is_2d_mode_(true), is_cube_mode_(false), command_mode_(SINGLE), have_last_step_(false),
00140     planning_state_(NOT_STARTED)
00141   {
00142     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (pnh_);
00143     typename dynamic_reconfigure::Server<Config>::CallbackType f =
00144       boost::bind (&FootstepMarker::configCallback, this, _1, _2);
00145     srv_->setCallback (f);
00146 
00147     tf_client_.reset(new tf2_ros::BufferClient("tf2_buffer_server"));
00148     server_.reset(new interactive_markers::InteractiveMarkerServer(ros::this_node::getName()));
00149     pnh_.param("frame_id", odom_frame_id_, std::string("odom"));
00150     pnh_.param("lleg_end_coords", lleg_end_coords_, std::string("lleg_end_coords"));
00151     pnh_.param("rleg_end_coords", rleg_end_coords_, std::string("rleg_end_coords"));
00152     pnh_.param("footstep_size_x", foot_size_x_, 0.24);
00153     pnh_.param("footstep_size_y", foot_size_y_, 0.14);
00154     pnh_.param("footstep_size_z", foot_size_z_, 0.01);
00155     std::vector<double> lleg_footstep_offset, rleg_footstep_offset;
00156     if (jsk_topic_tools::readVectorParameter(pnh_, "lleg_footstep_offset", lleg_footstep_offset)) {
00157       lleg_footstep_offset_[0] = lleg_footstep_offset[0];
00158       lleg_footstep_offset_[1] = lleg_footstep_offset[1];
00159       lleg_footstep_offset_[2] = lleg_footstep_offset[2];
00160     }
00161     if (jsk_topic_tools::readVectorParameter(pnh_, "rleg_footstep_offset", rleg_footstep_offset)) {
00162       rleg_footstep_offset_[0] = rleg_footstep_offset[0];
00163       rleg_footstep_offset_[1] = rleg_footstep_offset[1];
00164       rleg_footstep_offset_[2] = rleg_footstep_offset[2];
00165     }
00166 
00167     // query bbox size and offset
00168     ros::NodeHandle nh;
00169     ros::ServiceClient bbox_client = nh.serviceClient<jsk_footstep_planner::CollisionBoundingBoxInfo>("footstep_planner/collision_bounding_box_info", false);
00170     ROS_INFO("waiting for %s", bbox_client.getService().c_str());
00171     bbox_client.waitForExistence();
00172     jsk_footstep_planner::CollisionBoundingBoxInfo bbox_info;
00173     if (bbox_client.call(bbox_info)) {
00174       collision_bbox_size_[0] = bbox_info.response.box_dimensions.x;
00175       collision_bbox_size_[1] = bbox_info.response.box_dimensions.y;
00176       collision_bbox_size_[2] = bbox_info.response.box_dimensions.z;
00177       tf::poseMsgToEigen(bbox_info.response.box_offset, collision_bbox_offset_);
00178     }
00179 
00180     // pose stamped command interface
00181     sub_pose_stamped_command_ = pnh_.subscribe("pose_stamped_command", 1, &FootstepMarker::poseStampedCommandCallback, this);
00182 
00183     // service servers
00184     srv_reset_fs_marker_    = pnh_.advertiseService("reset_marker",
00185                                                     &FootstepMarker::resetMarkerService, this);
00186     srv_toggle_fs_com_mode_ = pnh_.advertiseService("toggle_footstep_marker_mode",
00187                                                     &FootstepMarker::toggleFootstepMarkerModeService, this);
00188     srv_execute_footstep_   = pnh_.advertiseService("execute_footstep",
00189                                                     &FootstepMarker::executeFootstepService, this);
00190     srv_wait_for_exec_fs_   = pnh_.advertiseService("wait_for_execute",
00191                                                     &FootstepMarker::waitForExecuteFootstepService, this);
00192     srv_wait_for_fs_plan_   = pnh_.advertiseService("wait_for_plan",
00193                                                     &FootstepMarker::waitForFootstepPlanService, this);
00194     srv_get_fs_marker_pose_ = pnh_.advertiseService("get_footstep_marker_pose",
00195                                                     &FootstepMarker::getFootstepMarkerPoseService, this);
00196     srv_stack_marker_pose_  = pnh_.advertiseService("stack_marker_pose",
00197                                                     &FootstepMarker::stackMarkerPoseService, this);
00198 
00199     pub_plan_result_ = pnh_.advertise<jsk_footstep_msgs::FootstepArray>("output/plan_result", 1);
00200     pub_current_marker_mode_ = pnh_.advertise<jsk_rviz_plugins::OverlayText>("marker_mode", 1, true);
00201 
00202     //ROS_INFO("waiting for footstep_planner");
00203     //ac_planner_.waitForServer();
00204     //ROS_INFO("waiting for footstep_controller");
00205     //ac_exec_.waitForServer();
00206     // initialize interactive marker
00207     // build menu handler
00208     setupMenuHandler();
00209     resetInteractiveMarker();
00210     publishCurrentMarkerMode();
00211     ROS_INFO("initialization done");
00212   }
00213 
00214   FootstepMarker::~FootstepMarker()
00215   {
00216     pub_marker_array_.clear();
00217     pub_marker_array_.publish();
00218     ros::Duration(1.0).sleep();
00219   }
00220 
00221   visualization_msgs::Marker FootstepMarker::makeFootstepMarker(FootstepTrans pose, unsigned char leg)
00222   {
00223     FootstepTrans footpose = pose;
00224     if (leg == jsk_footstep_msgs::Footstep::LLEG) {
00225       FootstepTranslation ltrans(lleg_footstep_offset_[0], lleg_footstep_offset_[1], lleg_footstep_offset_[2]);
00226       footpose = pose * ltrans;
00227     } else if (leg == jsk_footstep_msgs::Footstep::RLEG) {
00228       FootstepTranslation rtrans(rleg_footstep_offset_[0], rleg_footstep_offset_[1], rleg_footstep_offset_[2]);
00229       footpose = pose * rtrans;
00230     } else {
00231       ROS_ERROR ("makeFootstepMarker not implemented leg (%d)", leg);
00232     }
00233     visualization_msgs::Marker marker;
00234     if (is_cube_mode_) {
00235       marker.type = visualization_msgs::Marker::CUBE;
00236       marker.scale.x = foot_size_x_;
00237       marker.scale.y = foot_size_y_;
00238       marker.scale.z = foot_size_z_;
00239       tf::poseEigenToMsg(footpose, marker.pose);
00240     }
00241     else {
00242       marker.type = visualization_msgs::Marker::LINE_STRIP;
00243       marker.scale.x = 0.01;
00244       FootstepVec local_a( foot_size_x_ / 2.0,  foot_size_y_ / 2.0, 0.0);
00245       FootstepVec local_b(-foot_size_x_ / 2.0,  foot_size_y_ / 2.0, 0.0);
00246       FootstepVec local_c(-foot_size_x_ / 2.0, -foot_size_y_ / 2.0, 0.0);
00247       FootstepVec local_d( foot_size_x_ / 2.0, -foot_size_y_ / 2.0, 0.0);
00248       FootstepVec a = footpose * local_a;
00249       FootstepVec b = footpose * local_b;
00250       FootstepVec c = footpose * local_c;
00251       FootstepVec d = footpose * local_d;
00252       geometry_msgs::Point ros_a, ros_b, ros_c, ros_d;
00253       ros_a.x = a[0]; ros_a.y = a[1]; ros_a.z = a[2];
00254       ros_b.x = b[0]; ros_b.y = b[1]; ros_b.z = b[2];
00255       ros_c.x = c[0]; ros_c.y = c[1]; ros_c.z = c[2];
00256       ros_d.x = d[0]; ros_d.y = d[1]; ros_d.z = d[2];
00257       marker.points.push_back(ros_a);
00258       marker.points.push_back(ros_b);
00259       marker.points.push_back(ros_c);
00260       marker.points.push_back(ros_d);
00261       marker.points.push_back(ros_a);
00262     }
00263     marker.color.a = 1.0;
00264     return marker;
00265   }
00266 
00267   void FootstepMarker::setupInitialMarker(PosePair::Ptr leg_poses,
00268                                           visualization_msgs::InteractiveMarker& int_marker)
00269   {
00270     FootstepTrans midcoords = leg_poses->midcoords();
00271     tf::poseEigenToMsg(midcoords, int_marker.pose);
00272     visualization_msgs::Marker left_box_marker = makeFootstepMarker(midcoords.inverse() * leg_poses->getByName(lleg_end_coords_),
00273                                                                     jsk_footstep_msgs::Footstep::LLEG);
00274     left_box_marker.color.g = 1.0;
00275     visualization_msgs::Marker right_box_marker = makeFootstepMarker(midcoords.inverse() * leg_poses->getByName(rleg_end_coords_),
00276                                                                      jsk_footstep_msgs::Footstep::RLEG);
00277     right_box_marker.color.r = 1.0;
00278     visualization_msgs::InteractiveMarkerControl left_box_control;
00279     left_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00280     left_box_control.always_visible = true;
00281     left_box_control.markers.push_back(left_box_marker);
00282     int_marker.controls.push_back(left_box_control);
00283     visualization_msgs::InteractiveMarkerControl right_box_control;
00284     right_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00285     right_box_control.always_visible = true;
00286     right_box_control.markers.push_back(right_box_marker);
00287     int_marker.controls.push_back(right_box_control);
00288     int_marker.name = "initial_footstep_marker";
00289     int_marker.description = "Initial Footsteps";
00290   }
00291 
00292   void FootstepMarker::setupGoalMarker(FootstepTrans pose,
00293                                        visualization_msgs::InteractiveMarker& int_goal_marker)
00294   {
00295     int_goal_marker.name = "movable_footstep_marker";
00296     int_goal_marker.description = "Goal Footsteps";
00297     tf::poseEigenToMsg(pose, int_goal_marker.pose);
00298     current_lleg_offset_ = pose.inverse() * lleg_goal_pose_;
00299     current_rleg_offset_ = pose.inverse() * rleg_goal_pose_;
00300     visualization_msgs::Marker left_box_marker = makeFootstepMarker(current_lleg_offset_, jsk_footstep_msgs::Footstep::LLEG);
00301     left_box_marker.color.g = 1.0;
00302     visualization_msgs::Marker right_box_marker = makeFootstepMarker(current_rleg_offset_, jsk_footstep_msgs::Footstep::RLEG);
00303     right_box_marker.color.r = 1.0;
00304     visualization_msgs::InteractiveMarkerControl left_box_control;
00305     left_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00306     left_box_control.always_visible = true;
00307     left_box_control.markers.push_back(left_box_marker);
00308     int_goal_marker.controls.push_back(left_box_control);
00309     visualization_msgs::InteractiveMarkerControl right_box_control;
00310     right_box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00311     right_box_control.always_visible = true;
00312     right_box_control.markers.push_back(right_box_marker);
00313     int_goal_marker.controls.push_back(right_box_control);
00314   }
00315 
00316   void FootstepMarker::resetInteractiveMarker()
00317   {
00318     ROS_INFO("reset marker");
00319     PosePair::Ptr leg_poses;
00320     if (disable_tf_) {
00321       leg_poses = getDefaultFootstepPair();
00322     }
00323     else {
00324       if(command_mode_ == SINGLE || !have_last_step_) {
00325         leg_poses = getLatestCurrentFootstepPoses();
00326       } else { // !single_mode && have_last_step_
00327         FootstepTrans lleg_pose;
00328         FootstepTrans rleg_pose;
00329         tf::poseMsgToEigen(last_steps_[0].pose, lleg_pose);
00330         tf::poseMsgToEigen(last_steps_[1].pose, rleg_pose);
00331         leg_poses =
00332           PosePair::Ptr(new PosePair(lleg_pose, lleg_end_coords_,
00333                                      rleg_pose, rleg_end_coords_));
00334       }
00335     }
00336     original_foot_poses_ = leg_poses;
00337     visualization_msgs::InteractiveMarker int_marker;
00338     int_marker.header.frame_id = odom_frame_id_;
00339 
00340     setupInitialMarker(leg_poses, int_marker);
00341     server_->insert(int_marker,
00342                     boost::bind(&FootstepMarker::processFeedbackCB, this, _1));
00343 
00344     visualization_msgs::InteractiveMarker int_goal_marker;
00345     int_goal_marker.header.frame_id = odom_frame_id_;
00346     lleg_goal_pose_ = leg_poses->getByName(lleg_end_coords_);
00347     rleg_goal_pose_ = leg_poses->getByName(rleg_end_coords_);
00348     setupGoalMarker(leg_poses->midcoords(), int_goal_marker);
00349     if (is_2d_mode_) {
00350       add3Dof2DControl(int_goal_marker, false);
00351     }
00352     else {
00353       im_helpers::add6DofControl(int_goal_marker, false);
00354     }
00355 
00356     server_->insert(int_goal_marker,
00357                     boost::bind(&FootstepMarker::processFeedbackCB, this, _1));
00358     menu_handler_.apply(*server_, "movable_footstep_marker");
00359     menu_handler_.apply(*server_, "initial_footstep_marker");
00360     server_->applyChanges();
00361     updateMarkerArray(int_marker.header, int_goal_marker.pose);
00362   }
00363 
00364   void FootstepMarker::setupMenuHandler()
00365   {
00366     menu_handler_.insert("Reset Marker", boost::bind(&FootstepMarker::resetMarkerCB, this, _1));
00367     menu_handler_.insert("Execute Footstep", boost::bind(&FootstepMarker::executeFootstepCB, this, _1));
00368     stack_btn_ = menu_handler_.insert("Stack Footstep", boost::bind(&FootstepMarker::stackFootstepCB, this, _1));
00369     menu_handler_.setVisible(stack_btn_, false);
00370     interactive_markers::MenuHandler::EntryHandle mode_handle = menu_handler_.insert("2D/3D Mode");
00371     entry_2d_mode_ = menu_handler_.insert(mode_handle, "2D mode", boost::bind(&FootstepMarker::enable2DCB, this, _1));
00372     entry_3d_mode_ = menu_handler_.insert(mode_handle, "3D mode", boost::bind(&FootstepMarker::enable3DCB, this, _1));
00373     if (is_2d_mode_) {
00374       menu_handler_.setCheckState(entry_2d_mode_,
00375                                   interactive_markers::MenuHandler::CHECKED);
00376       menu_handler_.setCheckState(entry_3d_mode_,
00377                                 interactive_markers::MenuHandler::UNCHECKED);
00378     }
00379     else {
00380       menu_handler_.setCheckState(entry_2d_mode_,
00381                                   interactive_markers::MenuHandler::UNCHECKED);
00382       menu_handler_.setCheckState(entry_3d_mode_,
00383                                 interactive_markers::MenuHandler::CHECKED);
00384     }
00385 
00386     interactive_markers::MenuHandler::EntryHandle vis_mode_handle = menu_handler_.insert("Visualization");
00387     cube_mode_ = menu_handler_.insert(vis_mode_handle, "Cube", boost::bind(&FootstepMarker::enableCubeCB, this, _1));
00388     line_mode_ = menu_handler_.insert(vis_mode_handle, "Line", boost::bind(&FootstepMarker::enableLineCB, this, _1));
00389     if (is_cube_mode_) {
00390       menu_handler_.setCheckState(cube_mode_, interactive_markers::MenuHandler::CHECKED);
00391       menu_handler_.setCheckState(line_mode_, interactive_markers::MenuHandler::UNCHECKED);
00392     }
00393     else {
00394       menu_handler_.setCheckState(cube_mode_, interactive_markers::MenuHandler::UNCHECKED);
00395       menu_handler_.setCheckState(line_mode_, interactive_markers::MenuHandler::CHECKED);
00396     }
00397 
00398     interactive_markers::MenuHandler::EntryHandle p_mode_handle = menu_handler_.insert("Plan Mode");
00399     single_mode_ = menu_handler_.insert(p_mode_handle, "Single", boost::bind(&FootstepMarker::enableSingleCB, this, _1));
00400     cont_mode_ = menu_handler_.insert(p_mode_handle, "Continuous", boost::bind(&FootstepMarker::enableContinuousCB, this, _1));
00401     stack_mode_ = menu_handler_.insert(p_mode_handle, "Stack", boost::bind(&FootstepMarker::enableStackCB, this, _1)); // TODO
00402     menu_handler_.setCheckState(single_mode_, interactive_markers::MenuHandler::UNCHECKED);
00403     menu_handler_.setCheckState(cont_mode_,   interactive_markers::MenuHandler::UNCHECKED);
00404     menu_handler_.setCheckState(stack_mode_,  interactive_markers::MenuHandler::UNCHECKED);
00405     switch(command_mode_) {
00406     case SINGLE:
00407       menu_handler_.setCheckState(single_mode_, interactive_markers::MenuHandler::CHECKED);
00408       break;
00409     case CONTINUOUS:
00410       menu_handler_.setCheckState(cont_mode_,   interactive_markers::MenuHandler::CHECKED);
00411       break;
00412     case STACK:
00413       stacked_poses_.resize(0);
00414       stacked_poses_.push_back(original_foot_poses_->midcoords());
00415       menu_handler_.setVisible(stack_btn_, true);
00416       menu_handler_.setCheckState(stack_mode_,  interactive_markers::MenuHandler::CHECKED);
00417       break;
00418     }
00419   }
00420 
00421   void FootstepMarker::resetMarkerCB(
00422     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00423   {
00424     actionlib::SimpleClientGoalState state = ac_exec_.getState();
00425     if (state.isDone()) { // Do not reset last step while footsteps are executed
00426       have_last_step_ = false;
00427     }
00428     resetInteractiveMarker();
00429   }
00430 
00431   void FootstepMarker::executeDoneCB(const actionlib::SimpleClientGoalState &state,
00432                                      const ExecResult::ConstPtr &result)
00433   {
00434     ROS_INFO("Done footsteps");
00435     resetInteractiveMarker();
00436   }
00437 
00438   void FootstepMarker::stackFootstepCB(
00439     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00440   {
00441     PosePair::Ptr goal_pose_pair(new PosePair(lleg_goal_pose_, lleg_end_coords_,
00442                                               rleg_goal_pose_, rleg_end_coords_));
00443     FootstepTrans midcoords = goal_pose_pair->midcoords();
00444     stacked_poses_.push_back(midcoords);
00445 
00446     updateMarkerArray(feedback->header, feedback->pose);
00447   }
00448 
00449   void FootstepMarker::executeFootstepCB(
00450     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00451   {
00452     // Lock footstep planner
00453     boost::mutex::scoped_lock lock(planner_mutex_);
00454     if (planning_state_ == FINISHED) {
00455       jsk_footstep_msgs::ExecFootstepsGoal goal;
00456       planning_state_ = NOT_STARTED;
00457       goal.footstep = plan_result_;
00458       if (command_mode_ == SINGLE) {
00459         //if(ac_exec_.getState() == actionlib::SimpleClientGoalState::ACTIVE) {
00460         //  ac_exec_.waitForResult(ros::Duration(120.0));
00461         //  return;
00462         //}
00463         goal.strategy = jsk_footstep_msgs::ExecFootstepsGoal::NEW_TARGET;
00464         have_last_step_ = false;
00465 
00466         ROS_INFO("Execute footsteps single");
00467         ac_exec_.sendGoal(goal, boost::bind(&FootstepMarker::executeDoneCB, this, _1, _2));
00468       } else { // continuous mode
00469         if (have_last_step_ ) {
00470           goal.strategy = jsk_footstep_msgs::ExecFootstepsGoal::RESUME;
00471         } else {
00472           goal.strategy = jsk_footstep_msgs::ExecFootstepsGoal::NEW_TARGET;
00473         }
00474 
00475         int size = plan_result_.footsteps.size();
00476         {
00477           if(plan_result_.footsteps[size-1].leg == jsk_footstep_msgs::Footstep::LEFT) {
00478             last_steps_[0] = plan_result_.footsteps[size-1]; // left
00479             last_steps_[1] = plan_result_.footsteps[size-2]; // right
00480           } else {
00481             last_steps_[0] = plan_result_.footsteps[size-2]; // left
00482             last_steps_[1] = plan_result_.footsteps[size-1]; // right
00483           }
00484         }
00485         have_last_step_ = true;
00486         if (goal.strategy == jsk_footstep_msgs::ExecFootstepsGoal::NEW_TARGET) {
00487           ROS_INFO("Execute footsteps continuous(new)");
00488         } else {
00489           ROS_INFO("Execute footsteps continuous(added)");
00490         }
00491         // wait result or ...
00492         if (ac_exec_.isServerConnected()) {
00493           ac_exec_.sendGoal(goal, boost::bind(&FootstepMarker::executeDoneCB, this, _1, _2));
00494         } else {
00495           ROS_FATAL("actionlib server is not connected");
00496         }
00497         resetInteractiveMarker();
00498       }
00499     }
00500     else if (planning_state_ == ON_GOING) {
00501       ROS_FATAL("cannot execute footstep because planning state is ON_GOING");
00502     }
00503     else if (planning_state_ == NOT_STARTED) {
00504       ROS_FATAL("cannot execute footstep because planning state is NOT_STARTED");
00505     }
00506   }
00507 
00508   void FootstepMarker::enableCubeCB(
00509     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00510   {
00511     interactive_markers::MenuHandler::CheckState check_state;
00512     menu_handler_.getCheckState(cube_mode_, check_state);
00513     if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
00514       menu_handler_.setCheckState(cube_mode_, interactive_markers::MenuHandler::CHECKED);
00515       menu_handler_.setCheckState(line_mode_, interactive_markers::MenuHandler::UNCHECKED);
00516       menu_handler_.reApply(*server_);
00517       is_cube_mode_ = true;
00518       resetInteractiveMarker();
00519     }
00520   }
00521 
00522   void FootstepMarker::enableLineCB(
00523     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00524   {
00525     interactive_markers::MenuHandler::CheckState check_state;
00526     menu_handler_.getCheckState(line_mode_, check_state);
00527     if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
00528       menu_handler_.setCheckState(cube_mode_, interactive_markers::MenuHandler::UNCHECKED);
00529       menu_handler_.setCheckState(line_mode_, interactive_markers::MenuHandler::CHECKED);
00530       menu_handler_.reApply(*server_);
00531       is_cube_mode_ = false;
00532       resetInteractiveMarker();
00533     }
00534   }
00535 
00536   void FootstepMarker::enable2DCB(
00537     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00538   {
00539     interactive_markers::MenuHandler::CheckState check_state;
00540     menu_handler_.getCheckState(entry_2d_mode_, check_state);
00541     if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
00542       menu_handler_.setCheckState(entry_2d_mode_, interactive_markers::MenuHandler::CHECKED);
00543       menu_handler_.setCheckState(entry_3d_mode_, interactive_markers::MenuHandler::UNCHECKED);
00544       menu_handler_.reApply(*server_);
00545       // remove 6dof marker and use 3dof marker
00546       is_2d_mode_ = true;
00547       resetInteractiveMarker();
00548     }
00549   }
00550 
00551   void FootstepMarker::enable3DCB(
00552       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00553   {
00554     interactive_markers::MenuHandler::CheckState check_state;
00555     menu_handler_.getCheckState(entry_3d_mode_, check_state);
00556     if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
00557       menu_handler_.setCheckState(entry_3d_mode_, interactive_markers::MenuHandler::CHECKED);
00558       menu_handler_.setCheckState(entry_2d_mode_, interactive_markers::MenuHandler::UNCHECKED);
00559       menu_handler_.reApply(*server_);
00560       // remove 3dof marker and use 6dof marker
00561       is_2d_mode_ = false;
00562       resetInteractiveMarker();
00563     }
00564   }
00565 
00566   void FootstepMarker::enableSingleCB(
00567     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00568   {
00569     interactive_markers::MenuHandler::CheckState check_state;
00570     menu_handler_.getCheckState(single_mode_, check_state);
00571     if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
00572       menu_handler_.setVisible(stack_btn_, false);
00573       menu_handler_.setCheckState(single_mode_, interactive_markers::MenuHandler::CHECKED);
00574       menu_handler_.setCheckState(cont_mode_,   interactive_markers::MenuHandler::UNCHECKED);
00575       menu_handler_.setCheckState(stack_mode_,  interactive_markers::MenuHandler::UNCHECKED);
00576       menu_handler_.reApply(*server_);
00577       command_mode_ = SINGLE;
00578       resetInteractiveMarker();
00579       publishCurrentMarkerMode();
00580       { // change heuristic
00581         dynamic_reconfigure::Reconfigure rconf;
00582         dynamic_reconfigure::StrParameter spara;
00583         spara.name = "heuristic";
00584         spara.value = "path_cost";
00585         rconf.request.config.strs.push_back(spara);
00586         if (!ros::service::call("footstep_planner/set_parameters", rconf)) {
00587           // ERROR
00588           ROS_ERROR("Dynamic reconfigure: set parameters failed");
00589           return;
00590         }
00591       }
00592     }
00593   }
00594 
00595   void FootstepMarker::enableContinuousCB(
00596     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00597   {
00598     interactive_markers::MenuHandler::CheckState check_state;
00599     menu_handler_.getCheckState(cont_mode_, check_state);
00600     if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
00601       menu_handler_.setVisible(stack_btn_, false);
00602       menu_handler_.setCheckState(single_mode_, interactive_markers::MenuHandler::UNCHECKED);
00603       menu_handler_.setCheckState(cont_mode_,   interactive_markers::MenuHandler::CHECKED);
00604       menu_handler_.setCheckState(stack_mode_,  interactive_markers::MenuHandler::UNCHECKED);
00605       menu_handler_.reApply(*server_);
00606       command_mode_ = CONTINUOUS;
00607       resetInteractiveMarker();
00608       publishCurrentMarkerMode();
00609       { // change heuristic
00610         dynamic_reconfigure::Reconfigure rconf;
00611         dynamic_reconfigure::StrParameter spara;
00612         spara.name = "heuristic";
00613         spara.value = "path_cost";
00614         rconf.request.config.strs.push_back(spara);
00615         if (!ros::service::call("footstep_planner/set_parameters", rconf)) {
00616           // ERROR
00617           ROS_ERROR("Dynamic reconfigure: set parameters failed");
00618           return;
00619         }
00620       }
00621     }
00622   }
00623 
00624   void FootstepMarker::enableStackCB(
00625     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00626   {
00627     interactive_markers::MenuHandler::CheckState check_state;
00628     menu_handler_.getCheckState(stack_mode_, check_state);
00629     if (check_state == interactive_markers::MenuHandler::UNCHECKED) {
00630       menu_handler_.setVisible(stack_btn_, true);
00631       menu_handler_.setCheckState(single_mode_, interactive_markers::MenuHandler::UNCHECKED);
00632       menu_handler_.setCheckState(cont_mode_,   interactive_markers::MenuHandler::UNCHECKED);
00633       menu_handler_.setCheckState(stack_mode_,  interactive_markers::MenuHandler::CHECKED);
00634       menu_handler_.reApply(*server_);
00635       command_mode_ = STACK;
00636       stacked_poses_.resize(0);
00637       stacked_poses_.push_back(original_foot_poses_->midcoords());
00638       resetInteractiveMarker();
00639       publishCurrentMarkerMode();
00640       { // change heuristic
00641         dynamic_reconfigure::Reconfigure rconf;
00642         dynamic_reconfigure::StrParameter spara;
00643         spara.name = "heuristic";
00644         spara.value = "follow_path";
00645         rconf.request.config.strs.push_back(spara);
00646         if (!ros::service::call("footstep_planner/set_parameters", rconf)) {
00647           // ERROR
00648           ROS_ERROR("Dynamic reconfigure: set parameters failed");
00649           return;
00650         }
00651       }
00652     }
00653   }
00654 
00655   void FootstepMarker::processMenuFeedbackCB(
00656     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00657   {
00658   }
00659 
00660   void FootstepMarker::cancelPlanning()
00661   {
00662     boost::mutex::scoped_lock lock(planner_mutex_);
00663     if (planning_state_ == ON_GOING) {
00664       ac_planner_.cancelGoal();
00665       planning_state_ = FINISHED;
00666     }
00667   }
00668 
00669   jsk_footstep_msgs::FootstepArray FootstepMarker::footstepArrayFromPosePair(PosePair::Ptr pose_pair,
00670                                                                              const std_msgs::Header& header,
00671                                                                              bool is_lleg_first)
00672   {
00673     jsk_footstep_msgs::FootstepArray footstep_array;
00674     footstep_array.header = header;
00675     jsk_footstep_msgs::Footstep lleg = footstepFromEigenPose(pose_pair->getByName(lleg_end_coords_));
00676     jsk_footstep_msgs::Footstep rleg = footstepFromEigenPose(pose_pair->getByName(rleg_end_coords_));
00677     lleg.leg = jsk_footstep_msgs::Footstep::LEFT;
00678     rleg.leg = jsk_footstep_msgs::Footstep::RIGHT;
00679     lleg.dimensions.x = foot_size_x_;
00680     lleg.dimensions.y = foot_size_y_;
00681     lleg.dimensions.z = foot_size_z_;
00682     rleg.dimensions.x = foot_size_x_;
00683     rleg.dimensions.y = foot_size_y_;
00684     rleg.dimensions.z = foot_size_z_;
00685     if (is_lleg_first) {
00686       footstep_array.footsteps.push_back(lleg);
00687       footstep_array.footsteps.push_back(rleg);
00688     }
00689     else {
00690       footstep_array.footsteps.push_back(rleg);
00691       footstep_array.footsteps.push_back(lleg);
00692     }
00693     return footstep_array;
00694   }
00695 
00696   void FootstepMarker::plan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00697   {
00698     std_msgs::Header header;
00699     header.frame_id = odom_frame_id_;
00700     header.stamp = ros::Time::now();
00701     // first, project footstep using footstep_planner/project_footprint_with_local_search API.
00702     jsk_interactive_marker::SnapFootPrint srv_arg;
00703 
00704     PosePair::Ptr goal_pose_pair(new PosePair(lleg_goal_pose_, lleg_end_coords_,
00705                                               rleg_goal_pose_, rleg_end_coords_));
00706     srv_arg.request.input_pose.header = header;
00707     FootstepTrans midcoords = goal_pose_pair->midcoords();
00708     FootstepTrans lleg_trans = midcoords.inverse() * lleg_goal_pose_;
00709     FootstepTrans rleg_trans = midcoords.inverse() * rleg_goal_pose_;
00710     tf::poseEigenToMsg(midcoords, srv_arg.request.input_pose.pose);
00711     tf::poseEigenToMsg(lleg_trans, srv_arg.request.lleg_pose);
00712     tf::poseEigenToMsg(rleg_trans, srv_arg.request.rleg_pose);
00713     if (ros::service::call("footstep_planner/project_footprint_with_local_search", srv_arg)) {
00714       if (srv_arg.response.success) {
00715         FootstepTrans new_center_pose;
00716         tf::poseMsgToEigen(srv_arg.response.snapped_pose.pose, new_center_pose);
00717         goal_pose_pair.reset(new PosePair(new_center_pose * lleg_trans, lleg_end_coords_,
00718                                           new_center_pose * rleg_trans, rleg_end_coords_));
00719       }
00720       else {
00721         ROS_ERROR("Failed to project goal"); // should display message
00722         return;
00723       }
00724     }
00725     jsk_footstep_msgs::PlanFootstepsGoal goal;
00726     goal.goal_footstep
00727       = footstepArrayFromPosePair(goal_pose_pair, header, true);
00728     goal.initial_footstep
00729       = footstepArrayFromPosePair(original_foot_poses_, header, true);
00730 
00731     ac_planner_.sendGoal(goal, boost::bind(&FootstepMarker::planDoneCB, this, _1, _2));
00732     planning_state_ = ON_GOING;
00733   }
00734 
00735   void FootstepMarker::planDoneCB(const actionlib::SimpleClientGoalState &state,
00736                                   const PlanResult::ConstPtr &result)
00737   {
00738     boost::mutex::scoped_lock lock(planner_mutex_);
00739     pub_plan_result_.publish(result->result);
00740     planning_state_ = FINISHED;
00741     plan_result_ = result->result;
00742   }
00743 
00744   void FootstepMarker::planIfPossible(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00745   {
00746     boost::mutex::scoped_lock lock(planner_mutex_);
00747     if (planning_state_ != ON_GOING) {
00748       ROS_INFO("start planning");
00749       plan(feedback);
00750     }
00751   }
00752 
00753   FootstepTrans FootstepMarker::getDefaultLeftLegOffset() {
00754     return FootstepTrans(FootstepTranslation(0, default_footstep_margin_ / 2.0, 0));
00755   }
00756 
00757   FootstepTrans FootstepMarker::getDefaultRightLegOffset() {
00758     return FootstepTrans(FootstepTranslation(0, - default_footstep_margin_ / 2.0, 0));
00759   }
00760 
00761   void FootstepMarker::processFeedbackCB(
00762     const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
00763   {
00764     if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP) {
00765       ROS_INFO("mouse_up");
00766       if(command_mode_ != STACK) {
00767         ROS_INFO("cancel and planning");
00768         cancelPlanning();
00769         plan(feedback);
00770       } else {
00771         FootstepTrans current_marker_pose;
00772         tf::poseMsgToEigen(feedback->pose, current_marker_pose);
00773         lleg_goal_pose_ = current_marker_pose * current_lleg_offset_;
00774         rleg_goal_pose_ = current_marker_pose * current_rleg_offset_;
00775       }
00776     }
00777     else if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN) {
00778 
00779     }
00780     else if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) {
00781       ROS_INFO("pose_up");
00782       // update position of goal footstep
00783       FootstepTrans current_marker_pose;
00784       tf::poseMsgToEigen(feedback->pose, current_marker_pose);
00785       lleg_goal_pose_ = current_marker_pose * current_lleg_offset_;
00786       rleg_goal_pose_ = current_marker_pose * current_rleg_offset_;
00787       if(command_mode_ != STACK) {
00788         planIfPossible(feedback);
00789       } else { // stack mode
00790         if (planning_state_ != ON_GOING) {
00791           ROS_INFO("follow plan");
00792           callFollowPathPlan(feedback);
00793         }
00794       }
00795     }
00796     updateMarkerArray(feedback->header, feedback->pose);
00797   }
00798 
00799   visualization_msgs::Marker FootstepMarker::targetArrow(
00800     const std_msgs::Header& header, const geometry_msgs::Pose& pose)
00801   {
00802     visualization_msgs::Marker marker;
00803     marker.header = header;
00804     marker.type = visualization_msgs::Marker::ARROW;
00805     marker.scale.x = 0.1;
00806     marker.scale.y = 0.01;
00807     marker.scale.z = 0.01;
00808     marker.color.a = 1.0;
00809     marker.color.r = 1.0;
00810     marker.pose = pose;
00811     return marker;
00812   }
00813 
00814   visualization_msgs::Marker FootstepMarker::distanceLineMarker(
00815     const std_msgs::Header& header, const geometry_msgs::Pose& pose)
00816   {
00817     visualization_msgs::Marker marker;
00818     marker.header = header;
00819     marker.type = visualization_msgs::Marker::LINE_LIST;
00820     marker.scale.x = 0.01;
00821     marker.color.a = 1.0;
00822     marker.color.r = 24 / 255.0;
00823     marker.color.g = 240 / 255.0;
00824     marker.color.b = 1.0;
00825     FootstepTrans origin = original_foot_poses_->midcoords();
00826     FootstepTrans posef;
00827     tf::poseMsgToEigen(pose, posef);
00828     FootstepVec direction(posef.translation() - origin.translation());
00829     FootstepVec normalized_direction = direction.normalized();
00830     FootstepVec original_x_direction = origin.rotation() * FootstepVec::UnitX();
00831     FootstepVec rotate_axis = original_x_direction.cross(normalized_direction).normalized();
00832     double pose_theta = acos(original_x_direction.dot(normalized_direction));
00833     FootstepTrans transform;
00834     if (pose_theta == 0.0) {
00835       transform = origin * FootstepTrans::Identity();
00836     }
00837     else {
00838       //transform = origin * FootstepTranslation(-origin.translation()) *  Eigen::AngleAxisf(pose_theta, rotate_axis);
00839       transform = origin * FootstepAngleAxis(pose_theta, rotate_axis);
00840     }
00841     double distance = (origin.inverse() * posef).translation().norm();
00842     double r = distance / (2.0 * M_PI);
00843     // (x, y) = r(theta - sin,  1 - cos)
00844     const size_t resolustion = 100;
00845     const double max_z = 1.0;
00846     const double z_ratio = max_z / 2.0 / r;
00847     for (size_t i = 0; i < resolustion - 1; i++) {
00848       double theta = 2.0 * M_PI / resolustion * i;
00849       double next_theta = 2.0 * M_PI / resolustion * (i + 1);
00850       FootstepVec p = transform * FootstepVec(r * (theta - sin(theta)), 0, r * (1.0 - cos(theta)) * z_ratio);
00851       FootstepVec q = transform * FootstepVec(r * (next_theta - sin(next_theta)), 0, r * (1.0 - cos(next_theta)) * z_ratio);
00852       geometry_msgs::Point ros_p;
00853       ros_p.x = p[0];
00854       ros_p.y = p[1];
00855       ros_p.z = p[2];
00856       geometry_msgs::Point ros_q;
00857       ros_q.x = q[0];
00858       ros_q.y = q[1];
00859       ros_q.z = q[2];
00860       marker.colors.push_back(marker.color);
00861       marker.points.push_back(ros_p);
00862       marker.colors.push_back(marker.color);
00863       marker.points.push_back(ros_q);
00864     }
00865     return marker;
00866   }
00867 
00868   visualization_msgs::Marker FootstepMarker::originMarker(
00869     const std_msgs::Header& header, const geometry_msgs::Pose& pose)
00870   {
00871     visualization_msgs::Marker marker;
00872     marker.header = header;
00873     marker.type = visualization_msgs::Marker::LINE_STRIP;
00874     marker.scale.x = 0.01;
00875     marker.color.a = 1.0;
00876     marker.color.r = 24 / 255.0;
00877     marker.color.g = 240 / 255.0;
00878     marker.color.b = 1.0;
00879     FootstepTrans origin = original_foot_poses_->midcoords();
00880     tf::poseEigenToMsg(origin, marker.pose);
00881     const size_t resolution = 100;
00882     const double r = 0.5;
00883     for (size_t i = 0; i < resolution + 1; i++) {
00884       double theta = 2.0 * M_PI / resolution * i;
00885       FootstepVec p(r * cos(theta), r * sin(theta), 0.0);
00886       geometry_msgs::Point ros_p;
00887       ros_p.x = p[0];
00888       ros_p.y = p[1];
00889       ros_p.z = p[2];
00890       marker.points.push_back(ros_p);
00891       marker.colors.push_back(marker.color);
00892     }
00893     return marker;
00894   }
00895 
00896   visualization_msgs::Marker FootstepMarker::distanceTextMarker(
00897     const std_msgs::Header& header, const geometry_msgs::Pose& pose)
00898   {
00899     visualization_msgs::Marker marker;
00900     marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00901     Eigen::Affine3f posef;
00902     tf::poseMsgToEigen(pose, posef);
00903     Eigen::Affine3f text_pose = posef * Eigen::Translation3f(-0.1, 0, 0.1);
00904     Eigen::Affine3f midcoords;
00905     jsk_recognition_utils::convertEigenAffine3(original_foot_poses_->midcoords().inverse(), midcoords);
00906     Eigen::Affine3f transform = midcoords * posef;
00907     Eigen::Vector3f pos(transform.translation());
00908     float roll, pitch, yaw;
00909     pcl::getEulerAngles(transform, roll, pitch, yaw);
00910 
00911     marker.text = (boost::format("pos[m] = (%.2f, %.2f, %.2f)\nrot[deg] = (%.2f, %.2f, %.2f)\n%.2f [m]\n%.0f [deg]")
00912                    % (pos[0]) % (pos[1]) % (pos[2])
00913                    % (angles::to_degrees(roll)) % (angles::to_degrees(pitch)) % (angles::to_degrees(yaw))
00914                    % (pos.norm()) % (Eigen::AngleAxisf(transform.rotation()).angle() / M_PI * 180)).str();
00915     //marker.pose = pose;
00916     tf::poseEigenToMsg(text_pose, marker.pose);
00917     marker.header = header;
00918     marker.scale.z = 0.1;
00919     marker.color.a = 1.0;
00920     marker.color.r = 1.0;
00921     marker.color.g = 1.0;
00922     marker.color.b = 1.0;
00923     return marker;
00924   }
00925 
00926   visualization_msgs::Marker FootstepMarker::originBoundingBoxMarker(
00927       const std_msgs::Header& header, const geometry_msgs::Pose& pose)
00928   {
00929     visualization_msgs::Marker marker;
00930     marker.header = header;
00931     marker.type = visualization_msgs::Marker::CUBE;
00932     marker.scale.x = collision_bbox_size_[0];
00933     marker.scale.y = collision_bbox_size_[1];
00934     marker.scale.z = collision_bbox_size_[2];
00935     marker.color.a = 0.3;
00936     marker.color.r = 1.0;
00937     FootstepTrans box_pose = original_foot_poses_->midcoords() * collision_bbox_offset_;
00938     tf::poseEigenToMsg(box_pose, marker.pose);
00939     return marker;
00940   }
00941 
00942   visualization_msgs::Marker FootstepMarker::goalBoundingBoxMarker(
00943       const std_msgs::Header& header, const geometry_msgs::Pose& pose)
00944   {
00945     visualization_msgs::Marker marker;
00946     marker.header = header;
00947     marker.type = visualization_msgs::Marker::CUBE;
00948     marker.scale.x = collision_bbox_size_[0];
00949     marker.scale.y = collision_bbox_size_[1];
00950     marker.scale.z = collision_bbox_size_[2];
00951     marker.color.a = 0.3;
00952     marker.color.r = 1.0;
00953     FootstepTrans input_pose;
00954     tf::poseMsgToEigen(pose, input_pose);
00955     FootstepTrans box_pose = input_pose * collision_bbox_offset_;
00956     tf::poseEigenToMsg(box_pose, marker.pose);
00957     return marker;
00958   }
00959 
00960   visualization_msgs::Marker FootstepMarker::stackedPosesMarker(
00961     const std_msgs::Header& header, const geometry_msgs::Pose& pose)
00962   {
00963     visualization_msgs::Marker marker;
00964     marker.header = header;
00965     if (command_mode_ != STACK) {
00966       return marker;
00967     }
00968     marker.type = visualization_msgs::Marker::LINE_LIST;
00969     marker.scale.x = 0.025; // line width
00970     std_msgs::ColorRGBA col;
00971     col.a = 1.0;
00972     col.r = 1.0;
00973     col.g = 240 / 255.0;
00974     col.b = 24 / 255.0;
00975     marker.color = col;
00976 
00977     for(int i=1; i < stacked_poses_.size(); i++) {
00978       FootstepVec p = stacked_poses_[i-1].translation();
00979       FootstepVec q = stacked_poses_[i].translation();
00980       geometry_msgs::Point ros_p;
00981       ros_p.x = p[0];
00982       ros_p.y = p[1];
00983       ros_p.z = p[2];
00984       geometry_msgs::Point ros_q;
00985       ros_q.x = q[0];
00986       ros_q.y = q[1];
00987       ros_q.z = q[2];
00988       marker.colors.push_back(col);
00989       marker.points.push_back(ros_p);
00990       marker.colors.push_back(col);
00991       marker.points.push_back(ros_q);
00992     }
00993     // add current
00994     FootstepVec p = stacked_poses_[stacked_poses_.size() - 1].translation();
00995     geometry_msgs::Point ros_p;
00996     ros_p.x = p[0];
00997     ros_p.y = p[1];
00998     ros_p.z = p[2];
00999     geometry_msgs::Point ros_q;
01000     ros_q.x = pose.position.x;
01001     ros_q.y = pose.position.y;
01002     ros_q.z = pose.position.z;
01003     col.r = 1.0;
01004     col.g = 80 / 255.0;
01005     col.b = 80 / 255.0;
01006     marker.colors.push_back(col);
01007     marker.points.push_back(ros_p);
01008     marker.colors.push_back(col);
01009     marker.points.push_back(ros_q);
01010 
01011     return marker;
01012   }
01013 
01014   void FootstepMarker::updateMarkerArray(const std_msgs::Header& header, const geometry_msgs::Pose& pose)
01015   {
01016     pub_marker_array_.insert("target arrow", targetArrow(header, pose));
01017     pub_marker_array_.insert("distance text", distanceTextMarker(header, pose));
01018     pub_marker_array_.insert("distance line", distanceLineMarker(header, pose));
01019     pub_marker_array_.insert("origin", originMarker(header, pose));
01020     pub_marker_array_.insert("origin_bbox", originBoundingBoxMarker(header, pose));
01021     pub_marker_array_.insert("goal_bbox", goalBoundingBoxMarker(header, pose));
01022     pub_marker_array_.insert("stacked_pose", stackedPosesMarker(header, pose));
01023     pub_marker_array_.publish();
01024   }
01025 
01026   void FootstepMarker::processPoseUpdateCB(
01027     const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
01028   {
01029   }
01030 
01031   PosePair::Ptr FootstepMarker::getDefaultFootstepPair()
01032   {
01033     FootstepTrans lleg_default_pose(FootstepTranslation(0, 0.1, 0));
01034     FootstepTrans rleg_default_pose(FootstepTranslation(0, -0.1, 0));
01035     return PosePair::Ptr(new PosePair(lleg_default_pose, lleg_end_coords_,
01036                                       rleg_default_pose, rleg_end_coords_));
01037   }
01038 
01039   PosePair::Ptr FootstepMarker::getLatestCurrentFootstepPoses()
01040   {
01041     while (ros::ok())
01042     {
01043       try
01044       {
01045         return getCurrentFootstepPoses(ros::Time(0));
01046       }
01047       catch (tf2::TransformException& e)
01048       {
01049         ROS_WARN("tf error, retry: %s", e.what());
01050       }
01051     }
01052   }
01053 
01054   PosePair::Ptr FootstepMarker::getCurrentFootstepPoses(const ros::Time& stamp)
01055   {
01056     // use tf2
01057     // odom -> lleg_end_coords
01058     geometry_msgs::TransformStamped lleg_transform
01059       = tf_client_->lookupTransform(odom_frame_id_, lleg_end_coords_, stamp);
01060     geometry_msgs::TransformStamped rleg_transform
01061       = tf_client_->lookupTransform(odom_frame_id_, rleg_end_coords_, stamp);
01062     FootstepTrans lleg_transform_eigen, rleg_transform_eigen;
01063     tf::transformMsgToEigen(lleg_transform.transform, lleg_transform_eigen);
01064     tf::transformMsgToEigen(rleg_transform.transform, rleg_transform_eigen);
01065     PosePair::Ptr ppair (new PosePair(lleg_transform_eigen, lleg_end_coords_,
01066                                       rleg_transform_eigen, rleg_end_coords_));
01067     if(use_default_goal_) {
01068       return PosePair::Ptr(new PosePair(ppair->midcoords() * getDefaultLeftLegOffset(),  lleg_end_coords_,
01069                                         ppair->midcoords() * getDefaultRightLegOffset(), rleg_end_coords_));
01070     }
01071     return ppair;
01072   }
01073 
01074   void FootstepMarker::configCallback(Config &config, uint32_t level)
01075   {
01076     boost::mutex::scoped_lock lock(planner_mutex_);
01077     disable_tf_ = config.disable_tf;
01078     default_footstep_margin_ = config.default_footstep_margin;
01079     use_default_goal_ =  config.use_default_step_as_goal;
01080   }
01081 
01082 
01083   void FootstepMarker::poseStampedCommandCallback(
01084     const geometry_msgs::PoseStamped::ConstPtr& msg)
01085   {
01086     ROS_DEBUG("posestamped command is received");
01087     ROS_INFO("posestamped command is received");
01088     geometry_msgs::PoseStamped tmp_pose_stamped;
01089     if(msg->header.frame_id != odom_frame_id_ && !disable_tf_) {
01090       // apply target pose to goal marker
01091       // mutex should be limited in this range because planIfPossible also lock planner_mutex_
01092       boost::mutex::scoped_lock lock(planner_mutex_);
01093       try {
01094         geometry_msgs::TransformStamped offset = tf_client_->lookupTransform(odom_frame_id_, msg->header.frame_id,
01095                                                                              msg->header.stamp, ros::Duration(2.0));
01096         FootstepTrans msg_eigen;
01097         FootstepTrans offset_eigen;
01098         tf::poseMsgToEigen(msg->pose, msg_eigen);
01099         tf::transformMsgToEigen(offset.transform, offset_eigen);
01100         FootstepTrans pose = msg_eigen * offset_eigen;
01101 
01102         tf::poseEigenToMsg(pose, tmp_pose_stamped.pose);
01103         tmp_pose_stamped.header.stamp = msg->header.stamp;
01104         tmp_pose_stamped.header.frame_id = odom_frame_id_;
01105       } catch(tf2::TransformException ex) {
01106         ROS_ERROR("posestamped command transformation failed %s",ex.what());
01107         return;
01108       }
01109     } else {
01110       tmp_pose_stamped = *msg;
01111     }
01112     server_->setPose("movable_footstep_marker", tmp_pose_stamped.pose, tmp_pose_stamped.header);
01113     server_->applyChanges();
01114     // forcely call processFeedbackCB to execute planning
01115     visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
01116     dummy_feedback.header = tmp_pose_stamped.header;
01117     dummy_feedback.pose = tmp_pose_stamped.pose;
01118     dummy_feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
01119     const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
01120       = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
01121     processFeedbackCB(dummy_feedback_ptr);
01122   }
01123 
01124   bool FootstepMarker::resetMarkerService(
01125     std_srvs::Empty::Request& req,
01126     std_srvs::Empty::Response& res)
01127   {
01128     // forcely call resetMarkerCB to reset marker. feedback msg does not used in restMarkerCB.
01129     visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
01130     const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
01131       = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
01132     resetMarkerCB(dummy_feedback_ptr);
01133     return true;
01134   }
01135 
01136   bool FootstepMarker::toggleFootstepMarkerModeService(
01137     std_srvs::Empty::Request& req,
01138     std_srvs::Empty::Response& res)
01139   {
01140     visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
01141     const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
01142       = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
01143 
01144     if (command_mode_ == SINGLE) {
01145       // single -> continuous
01146       enableContinuousCB(dummy_feedback_ptr);
01147       menu_handler_.setCheckState(single_mode_, interactive_markers::MenuHandler::UNCHECKED);
01148       menu_handler_.setCheckState(cont_mode_, interactive_markers::MenuHandler::CHECKED);
01149       menu_handler_.setCheckState(stack_mode_, interactive_markers::MenuHandler::UNCHECKED);
01150     } else if (command_mode_ == CONTINUOUS) {
01151       // continuous -> stack
01152       enableStackCB(dummy_feedback_ptr);
01153       menu_handler_.setCheckState(single_mode_, interactive_markers::MenuHandler::UNCHECKED);
01154       menu_handler_.setCheckState(cont_mode_, interactive_markers::MenuHandler::UNCHECKED);
01155       menu_handler_.setCheckState(stack_mode_, interactive_markers::MenuHandler::CHECKED);
01156     } else {
01157       // stack -> single
01158       enableSingleCB(dummy_feedback_ptr);
01159       menu_handler_.setCheckState(single_mode_, interactive_markers::MenuHandler::CHECKED);
01160       menu_handler_.setCheckState(cont_mode_, interactive_markers::MenuHandler::UNCHECKED);
01161       menu_handler_.setCheckState(stack_mode_, interactive_markers::MenuHandler::UNCHECKED);
01162     }
01163     return true;
01164   }
01165 
01166   bool FootstepMarker::executeFootstepService(
01167     std_srvs::Empty::Request& req,
01168     std_srvs::Empty::Response& res)
01169   {
01170     // forcely call executeFootstepCB to execute footstep. feedback msg does not used in executeFootstepCB.
01171     visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
01172     const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
01173       = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
01174     executeFootstepCB(dummy_feedback_ptr);
01175     return true;
01176     // check footstep result
01177     if (ac_exec_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
01178       return true;
01179     } else {
01180       return false;
01181     }
01182   }
01183 
01184   bool FootstepMarker::waitForExecuteFootstepService(
01185     std_srvs::Empty::Request& req,
01186     std_srvs::Empty::Response& res)
01187   {
01188     bool result = true;
01189     actionlib::SimpleClientGoalState state = ac_exec_.getState();
01190     if(!state.isDone()) {
01191       result = ac_exec_.waitForResult(ros::Duration(120.0));
01192     }
01193     return result;
01194   }
01195 
01196   bool FootstepMarker::waitForFootstepPlanService(
01197     std_srvs::Empty::Request& req,
01198     std_srvs::Empty::Response& res)
01199   {
01200     bool result = true;
01201     actionlib::SimpleClientGoalState state = ac_planner_.getState();
01202     if(!state.isDone()) {
01203       result = ac_planner_.waitForResult(ros::Duration(120.0));
01204     }
01205     return result;
01206   }
01207 
01208   bool FootstepMarker::getFootstepMarkerPoseService(
01209     jsk_interactive_marker::GetTransformableMarkerPose::Request& req,
01210     jsk_interactive_marker::GetTransformableMarkerPose::Response& res)
01211   {
01212     boost::mutex::scoped_lock lock(planner_mutex_);
01213     std::string target_name = req.target_name;
01214     visualization_msgs::InteractiveMarker int_marker;
01215     if (server_->get(target_name, int_marker)) {
01216       geometry_msgs::PoseStamped ret_pose_stamped;
01217       ret_pose_stamped.header = int_marker.header;
01218       ret_pose_stamped.pose = int_marker.pose;
01219       res.pose_stamped = ret_pose_stamped;
01220       return true;
01221     } else {
01222       ROS_WARN("There is no marker named %s", target_name.c_str());
01223       return false;
01224     }
01225   }
01226 
01227   bool FootstepMarker::stackMarkerPoseService(
01228       jsk_interactive_marker::SetPose::Request& req,
01229       jsk_interactive_marker::SetPose::Response& res)
01230   {
01231     ROS_INFO("stack marker service");
01232     if (command_mode_ != STACK) {
01233       return false;
01234     }
01235     visualization_msgs::InteractiveMarkerFeedback dummy_feedback;
01236     dummy_feedback.header = req.pose.header;
01237     dummy_feedback.pose   = req.pose.pose;
01238     const visualization_msgs::InteractiveMarkerFeedbackConstPtr dummy_feedback_ptr
01239       = boost::make_shared<const visualization_msgs::InteractiveMarkerFeedback>(dummy_feedback);
01240     stackFootstepCB(dummy_feedback_ptr);
01241     return true;
01242   }
01243 
01244   void FootstepMarker::publishCurrentMarkerMode()
01245   {
01246     std_msgs::ColorRGBA color;
01247     color.r = 0.3568627450980392;
01248     color.g = 0.7529411764705882;
01249     color.b = 0.8705882352941177;
01250     color.a = 1.0;
01251 
01252     std::string text;
01253     switch(command_mode_) {
01254     case SINGLE:
01255       text = "Single Mode";
01256       break;
01257     case CONTINUOUS:
01258       text = "Continuous Mode";
01259       break;
01260     case STACK:
01261       text = "Stack Mode";
01262       break;
01263     }
01264 
01265     jsk_rviz_plugins::OverlayText msg;
01266     msg.text = text;
01267     msg.width = 1000;
01268     msg.height = 1000;
01269     msg.top = 10;
01270     msg.left = 10;
01271     msg.bg_color.a = 0.0;
01272     msg.fg_color = color;
01273     msg.text_size = 24;
01274     pub_current_marker_mode_.publish(msg);
01275   }
01276 
01277   void FootstepMarker::callFollowPathPlan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
01278   {
01279     if(stacked_poses_.size() > 1) {
01280       boost::mutex::scoped_lock lock(planner_mutex_);
01281 
01282       jsk_footstep_planner::SetHeuristicPath srv_arg;
01283       for(int i = 0; i < stacked_poses_.size(); i++) {
01284         geometry_msgs::Point p;
01285         FootstepVec tl = stacked_poses_[i].translation();
01286         p.x = tl[0];
01287         p.y = tl[1];
01288         p.z = tl[2];
01289         srv_arg.request.segments.push_back(p);
01290       }
01291       { // add final pose
01292         geometry_msgs::Point p;
01293         PosePair::Ptr goal_pose_pair(new PosePair(lleg_goal_pose_, lleg_end_coords_,
01294                                                   rleg_goal_pose_, rleg_end_coords_));
01295         FootstepVec tl = goal_pose_pair->midcoords().translation();
01296         p.x = tl[0];
01297         p.y = tl[1];
01298         p.z = tl[2];
01299         srv_arg.request.segments.push_back(p);
01300       }
01301       if (!ros::service::call("footstep_planner/set_heuristic_path", srv_arg)) {
01302         // ERROR
01303         ROS_ERROR("Service: failed to call footstep_planner/set_heuristic_path");
01304         return;
01305       }
01306     } else {
01307       return;
01308     }
01309     // plan
01310     plan(feedback);
01311   }
01312 }
01313 
01314 int main(int argc, char** argv)
01315 {
01316   ros::init(argc, argv, "footstep_marker");
01317   jsk_footstep_planner::FootstepMarker marker;
01318   ros::spin();
01319 }


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:28