footstep_marker.h
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 
36 
37 #ifndef JSK_FOOTSTEP_PLANNER_FOOTSTEP_MARKER_H_
38 #define JSK_FOOTSTEP_PLANNER_FOOTSTEP_MARKER_H_
39 
40 #include <ros/ros.h>
42 #include <jsk_footstep_msgs/PlanFootstepsAction.h>
43 #include <jsk_footstep_msgs/ExecFootstepsAction.h>
44 #include <jsk_interactive_marker/GetTransformableMarkerPose.h>
45 #include <jsk_interactive_marker/SetPose.h>
47 #include <geometry_msgs/PointStamped.h>
48 #include <geometry_msgs/PoseStamped.h>
49 #include <std_srvs/Empty.h>
51 #include <tf2_ros/buffer_client.h>
52 #include <Eigen/Geometry>
53 #include <visualization_msgs/MarkerArray.h>
55 #include "jsk_footstep_planner/FootstepMarkerConfig.h"
56 #include "jsk_footstep_planner/SetHeuristicPath.h"
57 #include <dynamic_reconfigure/server.h>
58 #include <jsk_rviz_plugins/OverlayText.h>
59 
60 namespace jsk_footstep_planner
61 {
62 
63  class UnknownPoseName: public std::exception
64  {
65 
66  };
67 #if 1
68  typedef Eigen::Affine3d FootstepTrans;
69  typedef Eigen::Vector3d FootstepVec;
70  typedef Eigen::Translation3d FootstepTranslation;
71  typedef Eigen::Quaterniond FootstepQuaternion;
72  typedef Eigen::AngleAxisd FootstepAngleAxis;
73 #else
74  typedef Eigen::Affine3f FootstepTrans;
75  typedef Eigen::Vector3f FootstepVec;
76  typedef Eigen::Translation3f FootstepTranslation;
77  typedef Eigen::Quaternionf FootstepQuaternion;
78  typedef Eigen::AngleAxisf FootstepAngleAxis;
79 #endif
80  class PosePair
81  {
82  public:
84  PosePair(const FootstepTrans& first, const std::string& first_name,
85  const FootstepTrans& second, const std::string& second_name);
86  virtual FootstepTrans getByName(const std::string& name);
87  virtual FootstepTrans midcoords();
88  protected:
89  FootstepTrans first_;
90  FootstepTrans second_;
91  std::string first_name_;
92  std::string second_name_;
93  private:
94  };
95 
97  {
98  public:
104  typedef jsk_footstep_msgs::PlanFootstepsResult PlanResult;
105  typedef jsk_footstep_msgs::ExecFootstepsResult ExecResult;
106  typedef boost::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&)>
108 
109  typedef enum {NOT_STARTED, FINISHED, ON_GOING} PlanningState;
110  typedef enum {SINGLE, CONTINUOUS, STACK} CommandMode;
111  typedef FootstepMarkerConfig Config;
112  FootstepMarker();
113  virtual ~FootstepMarker();
114  protected:
115  virtual void resetInteractiveMarker();
116  virtual PosePair::Ptr getLatestCurrentFootstepPoses();
117  virtual PosePair::Ptr getCurrentFootstepPoses(const ros::Time& stamp);
118  virtual PosePair::Ptr getDefaultFootstepPair();
119  virtual visualization_msgs::Marker makeFootstepMarker(FootstepTrans pose,
120  unsigned char leg);
121  virtual void processFeedbackCB(
122  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
123  virtual void processMenuFeedbackCB(
124  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
125  virtual void processPoseUpdateCB(
126  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
127  virtual void resetMarkerCB(
128  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
129  virtual void enable2DCB(
130  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
131  virtual void enable3DCB(
132  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
133  virtual void enableCubeCB(
134  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
135  virtual void enableLineCB(
136  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
137  virtual void enableSingleCB(
138  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
139  virtual void enableContinuousCB(
140  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
141  virtual void enableStackCB(
142  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
143  virtual void executeFootstepCB(
144  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
145  virtual void stackFootstepCB(
146  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
147  virtual void executeDoneCB(const actionlib::SimpleClientGoalState &state,
148  const ExecResult::ConstPtr &result);
149  virtual FootstepTrans getDefaultLeftLegOffset();
150  virtual FootstepTrans getDefaultRightLegOffset();
151  // planner interface
152  virtual void cancelPlanning();
153  virtual void plan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
154  virtual void planIfPossible(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
155  virtual void planDoneCB(const actionlib::SimpleClientGoalState &state,
156  const PlanResult::ConstPtr &result);
157  virtual jsk_footstep_msgs::FootstepArray footstepArrayFromPosePair(PosePair::Ptr pose_pair,
158  const std_msgs::Header& header,
159  bool is_lleg_first);
160  // marker methods
161  virtual void setupInitialMarker(PosePair::Ptr leg_poses,
162  visualization_msgs::InteractiveMarker& int_marker);
163  virtual void setupGoalMarker(FootstepTrans pose,
164  visualization_msgs::InteractiveMarker& int_marker);
165  virtual void updateMarkerArray(const std_msgs::Header& header, const geometry_msgs::Pose& pose);
166  virtual visualization_msgs::Marker originMarker(
167  const std_msgs::Header& header, const geometry_msgs::Pose& pose);
168  virtual visualization_msgs::Marker distanceTextMarker(
169  const std_msgs::Header& header, const geometry_msgs::Pose& pose);
170  virtual visualization_msgs::Marker distanceLineMarker(
171  const std_msgs::Header& header, const geometry_msgs::Pose& pose);
172  virtual visualization_msgs::Marker targetArrow(
173  const std_msgs::Header& header, const geometry_msgs::Pose& pose);
174  virtual visualization_msgs::Marker originBoundingBoxMarker(
175  const std_msgs::Header& header, const geometry_msgs::Pose& pose);
176  virtual visualization_msgs::Marker goalBoundingBoxMarker(
177  const std_msgs::Header& header, const geometry_msgs::Pose& pose);
178  virtual visualization_msgs::Marker stackedPosesMarker(
179  const std_msgs::Header& header, const geometry_msgs::Pose& pose);
180  virtual void setupMenuHandler();
181 
182  virtual void configCallback(Config& config, uint32_t level);
183  virtual void poseStampedCommandCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
184 
185  virtual bool resetMarkerService(
186  std_srvs::Empty::Request& req,
187  std_srvs::Empty::Response& res);
188  virtual bool toggleFootstepMarkerModeService(
189  std_srvs::Empty::Request& req,
190  std_srvs::Empty::Response& res);
191  virtual bool executeFootstepService(
192  std_srvs::Empty::Request& req,
193  std_srvs::Empty::Response& res);
194  virtual bool waitForExecuteFootstepService(
195  std_srvs::Empty::Request& req,
196  std_srvs::Empty::Response& res);
197  virtual bool waitForFootstepPlanService(
198  std_srvs::Empty::Request& req,
199  std_srvs::Empty::Response& res);
200  virtual bool getFootstepMarkerPoseService(
201  jsk_interactive_marker::GetTransformableMarkerPose::Request& req,
202  jsk_interactive_marker::GetTransformableMarkerPose::Response& res);
203  virtual bool stackMarkerPoseService(
204  jsk_interactive_marker::SetPose::Request& req,
205  jsk_interactive_marker::SetPose::Response& res);
206 
207  virtual void publishCurrentMarkerMode();
208 
209  virtual void callFollowPathPlan(
210  const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
211 
214 
228 
229  std::string odom_frame_id_;
230  std::string lleg_end_coords_, rleg_end_coords_;
232  FootstepTrans lleg_goal_pose_, rleg_goal_pose_;
233  FootstepTrans current_lleg_offset_, current_rleg_offset_;
234  FootstepVec lleg_footstep_offset_, rleg_footstep_offset_;
236 
237  jsk_footstep_msgs::FootstepArray plan_result_;
252  CommandMode command_mode_;
253 
254  double foot_size_x_, foot_size_y_, foot_size_z_;
257 
259  PlanningState planning_state_;
260  FootstepVec collision_bbox_size_;
261  FootstepTrans collision_bbox_offset_;
262 
264  jsk_footstep_msgs::Footstep last_steps_[2];
265  std::vector<FootstepTrans > stacked_poses_;
266  private:
267  };
268 }
269 
270 #endif
boost::shared_ptr< PosePair > Ptr
jsk_footstep_msgs::PlanFootstepsResult PlanResult
actionlib::SimpleActionClient< jsk_footstep_msgs::PlanFootstepsAction > PlanningActionClient
interactive_markers::MenuHandler::EntryHandle cont_mode_
interactive_markers::MenuHandler::EntryHandle stack_mode_
interactive_markers::MenuHandler::EntryHandle single_mode_
boost::shared_ptr< FootstepMarker > Ptr
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
SINGLE
void processFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
actionlib::SimpleActionClient< jsk_footstep_msgs::ExecFootstepsAction > ExecuteActionClient
Eigen::Affine3d FootstepTrans
Eigen::Quaterniond FootstepQuaternion
boost::function< void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &)> MenuCallbackFunction
Eigen::Translation3d FootstepTranslation
interactive_markers::MenuHandler::EntryHandle entry_3d_mode_
interactive_markers::MenuHandler::EntryHandle line_mode_
jsk_footstep_msgs::FootstepArray plan_result_
interactive_markers::MenuHandler::EntryHandle cube_mode_
interactive_markers::MenuHandler::EntryHandle entry_2d_mode_
Eigen::Vector3d FootstepVec
Eigen::AngleAxisd FootstepAngleAxis
interactive_markers::MenuHandler::EntryHandle stack_btn_
boost::mutex mutex
interactive_markers::MenuHandler menu_handler_
std::vector< FootstepTrans > stacked_poses_
void plan(double x, double y, double yaw, FootstepGraph::Ptr graph, ros::Publisher &pub_path, ros::Publisher &pub_goal, Eigen::Vector3f footstep_size)
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
jsk_footstep_msgs::ExecFootstepsResult ExecResult
boost::shared_ptr< tf2_ros::BufferClient > tf_client_


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