jsk_footstep_planner: footstep_marker.h Source File

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 
00037 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_MARKER_H_
00038 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_MARKER_H_
00039 
00040 #include <ros/ros.h>
00041 #include <actionlib/client/simple_action_client.h>
00042 #include <jsk_footstep_msgs/PlanFootstepsAction.h>
00043 #include <jsk_footstep_msgs/ExecFootstepsAction.h>
00044 #include <jsk_interactive_marker/GetTransformableMarkerPose.h>
00045 #include <jsk_interactive_marker/SetPose.h>
00046 #include <interactive_markers/interactive_marker_server.h>
00047 #include <geometry_msgs/PointStamped.h>
00048 #include <geometry_msgs/PoseStamped.h>
00049 #include <std_srvs/Empty.h>
00050 #include <interactive_markers/menu_handler.h>
00051 #include <tf2_ros/buffer_client.h>
00052 #include <Eigen/Geometry>
00053 #include <visualization_msgs/MarkerArray.h>
00054 #include "jsk_footstep_planner/marker_array_publisher.h"
00055 #include "jsk_footstep_planner/FootstepMarkerConfig.h"
00056 #include "jsk_footstep_planner/SetHeuristicPath.h"
00057 #include <dynamic_reconfigure/server.h>
00058 #include <jsk_rviz_plugins/OverlayText.h>
00059 
00060 namespace jsk_footstep_planner
00061 {
00062 
00063   class UnknownPoseName: public std::exception
00064   {
00065 
00066   };
00067 #if 1
00068   typedef Eigen::Affine3d FootstepTrans;
00069   typedef Eigen::Vector3d FootstepVec;
00070   typedef Eigen::Translation3d FootstepTranslation;
00071   typedef Eigen::Quaterniond FootstepQuaternion;
00072   typedef Eigen::AngleAxisd FootstepAngleAxis;
00073 #else
00074   typedef Eigen::Affine3f FootstepTrans;
00075   typedef Eigen::Vector3f FootstepVec;
00076   typedef Eigen::Translation3f FootstepTranslation;
00077   typedef Eigen::Quaternionf FootstepQuaternion;
00078   typedef Eigen::AngleAxisf FootstepAngleAxis;
00079 #endif
00080   class PosePair
00081   {
00082   public:
00083     typedef boost::shared_ptr<PosePair> Ptr;
00084     PosePair(const FootstepTrans& first, const std::string& first_name,
00085              const FootstepTrans& second, const std::string& second_name);
00086     virtual FootstepTrans getByName(const std::string& name);
00087     virtual FootstepTrans midcoords();
00088   protected:
00089     FootstepTrans first_;
00090     FootstepTrans second_;
00091     std::string first_name_;
00092     std::string second_name_;
00093   private:
00094   };
00095 
00096   class FootstepMarker
00097   {
00098   public:
00099     typedef boost::shared_ptr<FootstepMarker> Ptr;
00100     typedef actionlib::SimpleActionClient<jsk_footstep_msgs::PlanFootstepsAction>
00101     PlanningActionClient;
00102     typedef actionlib::SimpleActionClient<jsk_footstep_msgs::ExecFootstepsAction>
00103     ExecuteActionClient;
00104     typedef jsk_footstep_msgs::PlanFootstepsResult PlanResult;
00105     typedef jsk_footstep_msgs::ExecFootstepsResult ExecResult;
00106     typedef boost::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&)>
00107     MenuCallbackFunction;
00108 
00109     typedef enum {NOT_STARTED, FINISHED, ON_GOING} PlanningState;
00110     typedef enum {SINGLE, CONTINUOUS, STACK} CommandMode;
00111     typedef FootstepMarkerConfig Config;
00112     FootstepMarker();
00113     virtual ~FootstepMarker();
00114   protected:
00115     virtual void resetInteractiveMarker();
00116     virtual PosePair::Ptr getLatestCurrentFootstepPoses();
00117     virtual PosePair::Ptr getCurrentFootstepPoses(const ros::Time& stamp);
00118     virtual PosePair::Ptr getDefaultFootstepPair();
00119     virtual visualization_msgs::Marker makeFootstepMarker(FootstepTrans pose,
00120                                                           unsigned char leg);
00121     virtual void processFeedbackCB(
00122       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00123     virtual void processMenuFeedbackCB(
00124       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00125     virtual void processPoseUpdateCB(
00126       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00127     virtual void resetMarkerCB(
00128       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00129     virtual void enable2DCB(
00130       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00131     virtual void enable3DCB(
00132       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00133     virtual void enableCubeCB(
00134       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00135     virtual void enableLineCB(
00136       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00137     virtual void enableSingleCB(
00138       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00139     virtual void enableContinuousCB(
00140       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00141     virtual void enableStackCB(
00142       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00143     virtual void executeFootstepCB(
00144       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00145     virtual void stackFootstepCB(
00146       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00147     virtual void executeDoneCB(const actionlib::SimpleClientGoalState &state,
00148                                const ExecResult::ConstPtr &result);
00149     virtual FootstepTrans getDefaultLeftLegOffset();
00150     virtual FootstepTrans getDefaultRightLegOffset();
00151     // planner interface
00152     virtual void cancelPlanning();
00153     virtual void plan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00154     virtual void planIfPossible(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00155     virtual void planDoneCB(const actionlib::SimpleClientGoalState &state,
00156                             const PlanResult::ConstPtr &result);
00157     virtual jsk_footstep_msgs::FootstepArray footstepArrayFromPosePair(PosePair::Ptr pose_pair,
00158                                                                        const std_msgs::Header& header,
00159                                                                        bool is_lleg_first);
00160     // marker methods
00161     virtual void setupInitialMarker(PosePair::Ptr leg_poses,
00162                                     visualization_msgs::InteractiveMarker& int_marker);
00163     virtual void setupGoalMarker(FootstepTrans pose,
00164                                  visualization_msgs::InteractiveMarker& int_marker);
00165     virtual void updateMarkerArray(const std_msgs::Header& header, const geometry_msgs::Pose& pose);
00166     virtual visualization_msgs::Marker originMarker(
00167       const std_msgs::Header& header, const geometry_msgs::Pose& pose);
00168     virtual visualization_msgs::Marker distanceTextMarker(
00169       const std_msgs::Header& header, const geometry_msgs::Pose& pose);
00170     virtual visualization_msgs::Marker distanceLineMarker(
00171       const std_msgs::Header& header, const geometry_msgs::Pose& pose);
00172     virtual visualization_msgs::Marker targetArrow(
00173       const std_msgs::Header& header, const geometry_msgs::Pose& pose);
00174     virtual visualization_msgs::Marker originBoundingBoxMarker(
00175       const std_msgs::Header& header, const geometry_msgs::Pose& pose);
00176     virtual visualization_msgs::Marker goalBoundingBoxMarker(
00177       const std_msgs::Header& header, const geometry_msgs::Pose& pose);
00178     virtual visualization_msgs::Marker stackedPosesMarker(
00179       const std_msgs::Header& header, const geometry_msgs::Pose& pose);
00180     virtual void setupMenuHandler();
00181 
00182     virtual void configCallback(Config& config, uint32_t level);
00183     virtual void poseStampedCommandCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
00184 
00185     virtual bool resetMarkerService(
00186       std_srvs::Empty::Request& req,
00187       std_srvs::Empty::Response& res);
00188     virtual bool toggleFootstepMarkerModeService(
00189       std_srvs::Empty::Request& req,
00190       std_srvs::Empty::Response& res);
00191     virtual bool executeFootstepService(
00192       std_srvs::Empty::Request& req,
00193       std_srvs::Empty::Response& res);
00194     virtual bool waitForExecuteFootstepService(
00195       std_srvs::Empty::Request& req,
00196       std_srvs::Empty::Response& res);
00197     virtual bool waitForFootstepPlanService(
00198       std_srvs::Empty::Request& req,
00199       std_srvs::Empty::Response& res);
00200     virtual bool getFootstepMarkerPoseService(
00201       jsk_interactive_marker::GetTransformableMarkerPose::Request& req,
00202       jsk_interactive_marker::GetTransformableMarkerPose::Response& res);
00203     virtual bool stackMarkerPoseService(
00204       jsk_interactive_marker::SetPose::Request& req,
00205       jsk_interactive_marker::SetPose::Response& res);
00206 
00207     virtual void publishCurrentMarkerMode();
00208 
00209     virtual void callFollowPathPlan(
00210       const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
00211 
00212     ros::NodeHandle nh_;
00213     ros::NodeHandle pnh_;
00214 
00215     MarkerArrayPublisher pub_marker_array_;
00216     PlanningActionClient ac_planner_;
00217     ExecuteActionClient ac_exec_;
00218     ros::Publisher pub_plan_result_;
00219     ros::Publisher pub_current_marker_mode_;
00220     ros::Subscriber sub_pose_stamped_command_;
00221     ros::ServiceServer srv_reset_fs_marker_;
00222     ros::ServiceServer srv_toggle_fs_com_mode_;
00223     ros::ServiceServer srv_execute_footstep_;
00224     ros::ServiceServer srv_wait_for_exec_fs_;
00225     ros::ServiceServer srv_wait_for_fs_plan_;
00226     ros::ServiceServer srv_get_fs_marker_pose_;
00227     ros::ServiceServer srv_stack_marker_pose_;
00228 
00229     std::string odom_frame_id_;
00230     std::string lleg_end_coords_, rleg_end_coords_;
00231     PosePair::Ptr original_foot_poses_;
00232     FootstepTrans lleg_goal_pose_, rleg_goal_pose_;
00233     FootstepTrans current_lleg_offset_, current_rleg_offset_;
00234     FootstepVec lleg_footstep_offset_, rleg_footstep_offset_;
00235     double default_footstep_margin_;
00236 
00237     jsk_footstep_msgs::FootstepArray plan_result_;
00238     boost::shared_ptr<tf2_ros::BufferClient> tf_client_;
00239     boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
00240     boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
00241     interactive_markers::MenuHandler menu_handler_;
00242     interactive_markers::MenuHandler::EntryHandle stack_btn_;
00243     interactive_markers::MenuHandler::EntryHandle entry_2d_mode_;
00244     interactive_markers::MenuHandler::EntryHandle entry_3d_mode_;
00245     interactive_markers::MenuHandler::EntryHandle cube_mode_;
00246     interactive_markers::MenuHandler::EntryHandle line_mode_;
00247     interactive_markers::MenuHandler::EntryHandle single_mode_;
00248     interactive_markers::MenuHandler::EntryHandle cont_mode_;
00249     interactive_markers::MenuHandler::EntryHandle stack_mode_;
00250     bool is_2d_mode_;
00251     bool is_cube_mode_;
00252     CommandMode command_mode_;
00253 
00254     double foot_size_x_, foot_size_y_, foot_size_z_;
00255     bool disable_tf_;
00256     bool use_default_goal_;
00257 
00258     boost::mutex planner_mutex_;
00259     PlanningState planning_state_;
00260     FootstepVec collision_bbox_size_;
00261     FootstepTrans collision_bbox_offset_;
00262 
00263     bool have_last_step_;
00264     jsk_footstep_msgs::Footstep last_steps_[2];
00265     std::vector<FootstepTrans > stacked_poses_;
00266   private:
00267   };
00268 }
00269 
00270 #endif