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:
91  std::string first_name_;
92  std::string second_name_;
93  private:
94  };
95 
96  class FootstepMarker
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;
113  virtual ~FootstepMarker();
114  protected:
115  virtual void resetInteractiveMarker();
117  virtual PosePair::Ptr getCurrentFootstepPoses(const ros::Time& stamp);
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);
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_;
236 
237  jsk_footstep_msgs::FootstepArray plan_result_;
253 
257 
262 
264  jsk_footstep_msgs::Footstep last_steps_[2];
265  std::vector<FootstepTrans > stacked_poses_;
266  private:
267  };
268 }
269 
270 #endif
jsk_footstep_planner::FootstepMarker::enableContinuousCB
virtual void enableContinuousCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:595
jsk_footstep_planner::PosePair::midcoords
virtual FootstepTrans midcoords()
Definition: footstep_marker.cpp:129
jsk_footstep_planner::FootstepMarker::odom_frame_id_
std::string odom_frame_id_
Definition: footstep_marker.h:261
jsk_footstep_planner::FootstepTranslation
Eigen::Translation3d FootstepTranslation
Definition: footstep_marker.h:102
interactive_markers::MenuHandler::EntryHandle
uint32_t EntryHandle
jsk_footstep_planner::FootstepMarker::lleg_goal_pose_
FootstepTrans lleg_goal_pose_
Definition: footstep_marker.h:264
jsk_footstep_planner::FootstepMarker::lleg_end_coords_
std::string lleg_end_coords_
Definition: footstep_marker.h:262
jsk_footstep_planner::FootstepMarker::toggleFootstepMarkerModeService
virtual bool toggleFootstepMarkerModeService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: footstep_marker.cpp:1136
jsk_footstep_planner::FootstepMarker::getCurrentFootstepPoses
virtual PosePair::Ptr getCurrentFootstepPoses(const ros::Time &stamp)
Definition: footstep_marker.cpp:1054
ros::Publisher
jsk_footstep_planner::FootstepMarker::default_footstep_margin_
double default_footstep_margin_
Definition: footstep_marker.h:267
jsk_footstep_planner::FootstepMarker::enableLineCB
virtual void enableLineCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:522
jsk_footstep_planner::FootstepMarker::stackMarkerPoseService
virtual bool stackMarkerPoseService(jsk_interactive_marker::SetPose::Request &req, jsk_interactive_marker::SetPose::Response &res)
Definition: footstep_marker.cpp:1227
jsk_footstep_planner::FootstepMarker::enableStackCB
virtual void enableStackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:624
jsk_footstep_planner::FootstepMarker::enable2DCB
virtual void enable2DCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:536
boost::shared_ptr< PosePair >
jsk_footstep_planner::FootstepMarker::resetInteractiveMarker
virtual void resetInteractiveMarker()
Definition: footstep_marker.cpp:316
jsk_footstep_planner::FootstepMarker::originBoundingBoxMarker
virtual visualization_msgs::Marker originBoundingBoxMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
Definition: footstep_marker.cpp:926
jsk_footstep_planner::FootstepMarker::Config
FootstepMarkerConfig Config
Definition: footstep_marker.h:143
jsk_footstep_planner::FootstepMarker::plan_result_
jsk_footstep_msgs::FootstepArray plan_result_
Definition: footstep_marker.h:269
jsk_footstep_planner::FootstepMarker::foot_size_z_
double foot_size_z_
Definition: footstep_marker.h:286
jsk_footstep_planner::FootstepMarker::cancelPlanning
virtual void cancelPlanning()
Definition: footstep_marker.cpp:660
jsk_footstep_planner::FootstepMarker::nh_
ros::NodeHandle nh_
Definition: footstep_marker.h:244
jsk_footstep_planner::FootstepMarker::processMenuFeedbackCB
virtual void processMenuFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:655
jsk_footstep_planner::FootstepMarker::CommandMode
CommandMode
Definition: footstep_marker.h:142
jsk_footstep_planner::FootstepMarker::pnh_
ros::NodeHandle pnh_
Definition: footstep_marker.h:245
jsk_footstep_planner::FootstepMarker::is_cube_mode_
bool is_cube_mode_
Definition: footstep_marker.h:283
ros.h
jsk_footstep_planner::FootstepMarker::poseStampedCommandCallback
virtual void poseStampedCommandCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
Definition: footstep_marker.cpp:1083
jsk_footstep_planner::FootstepMarker::srv_wait_for_fs_plan_
ros::ServiceServer srv_wait_for_fs_plan_
Definition: footstep_marker.h:257
jsk_footstep_planner::FootstepAngleAxis
Eigen::AngleAxisd FootstepAngleAxis
Definition: footstep_marker.h:104
actionlib::SimpleClientGoalState
jsk_footstep_planner::FootstepMarker::planIfPossible
virtual void planIfPossible(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:744
jsk_footstep_planner::PosePair::second_
FootstepTrans second_
Definition: footstep_marker.h:122
jsk_footstep_planner::FootstepMarker::setupMenuHandler
virtual void setupMenuHandler()
Definition: footstep_marker.cpp:364
menu_handler.h
jsk_footstep_planner::FootstepMarker::setupGoalMarker
virtual void setupGoalMarker(FootstepTrans pose, visualization_msgs::InteractiveMarker &int_marker)
Definition: footstep_marker.cpp:292
jsk_footstep_planner::FootstepMarker::sub_pose_stamped_command_
ros::Subscriber sub_pose_stamped_command_
Definition: footstep_marker.h:252
jsk_footstep_planner::FootstepMarker::entry_2d_mode_
interactive_markers::MenuHandler::EntryHandle entry_2d_mode_
Definition: footstep_marker.h:275
jsk_footstep_planner::FootstepMarker::srv_execute_footstep_
ros::ServiceServer srv_execute_footstep_
Definition: footstep_marker.h:255
jsk_footstep_planner::FootstepQuaternion
Eigen::Quaterniond FootstepQuaternion
Definition: footstep_marker.h:103
jsk_footstep_planner::FootstepMarker::~FootstepMarker
virtual ~FootstepMarker()
Definition: footstep_marker.cpp:214
jsk_footstep_planner::FootstepMarker::cube_mode_
interactive_markers::MenuHandler::EntryHandle cube_mode_
Definition: footstep_marker.h:277
jsk_footstep_planner::FootstepMarker::planner_mutex_
boost::mutex planner_mutex_
Definition: footstep_marker.h:290
buffer_client.h
jsk_footstep_planner::PosePair::first_
FootstepTrans first_
Definition: footstep_marker.h:121
jsk_footstep_planner::FootstepMarker::getDefaultFootstepPair
virtual PosePair::Ptr getDefaultFootstepPair()
Definition: footstep_marker.cpp:1031
jsk_footstep_planner::FootstepMarker::stacked_poses_
std::vector< FootstepTrans > stacked_poses_
Definition: footstep_marker.h:297
jsk_footstep_planner::FootstepMarker::publishCurrentMarkerMode
virtual void publishCurrentMarkerMode()
Definition: footstep_marker.cpp:1244
jsk_footstep_planner::FootstepMarker::rleg_footstep_offset_
FootstepVec rleg_footstep_offset_
Definition: footstep_marker.h:266
jsk_footstep_planner::FootstepMarker::planning_state_
PlanningState planning_state_
Definition: footstep_marker.h:291
jsk_footstep_planner::FootstepMarker::executeFootstepCB
virtual void executeFootstepCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:449
mutex
boost::mutex mutex
Definition: pointcloud_model_generator_node.cpp:46
jsk_footstep_planner::FootstepMarker::makeFootstepMarker
virtual visualization_msgs::Marker makeFootstepMarker(FootstepTrans pose, unsigned char leg)
Definition: footstep_marker.cpp:221
jsk_footstep_planner::FootstepMarker::footstepArrayFromPosePair
virtual jsk_footstep_msgs::FootstepArray footstepArrayFromPosePair(PosePair::Ptr pose_pair, const std_msgs::Header &header, bool is_lleg_first)
Definition: footstep_marker.cpp:669
ros::ServiceServer
jsk_footstep_planner::FootstepMarker::ExecResult
jsk_footstep_msgs::ExecFootstepsResult ExecResult
Definition: footstep_marker.h:137
jsk_footstep_planner::FootstepMarker::enableCubeCB
virtual void enableCubeCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:508
jsk_footstep_planner::FootstepMarker::goalBoundingBoxMarker
virtual visualization_msgs::Marker goalBoundingBoxMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
Definition: footstep_marker.cpp:942
actionlib::SimpleActionClient< jsk_footstep_msgs::PlanFootstepsAction >
jsk_footstep_planner::FootstepMarker::PlanningActionClient
actionlib::SimpleActionClient< jsk_footstep_msgs::PlanFootstepsAction > PlanningActionClient
Definition: footstep_marker.h:133
marker_array_publisher.h
jsk_footstep_planner::FootstepMarker::PlanResult
jsk_footstep_msgs::PlanFootstepsResult PlanResult
Definition: footstep_marker.h:136
jsk_footstep_planner::FootstepMarker::SINGLE
@ SINGLE
Definition: footstep_marker.h:142
jsk_footstep_planner
Definition: ann_grid.h:50
interactive_markers::MenuHandler
jsk_footstep_planner::FootstepMarker::collision_bbox_size_
FootstepVec collision_bbox_size_
Definition: footstep_marker.h:292
jsk_footstep_planner::MarkerArrayPublisher
Definition: marker_array_publisher.h:73
jsk_footstep_planner::FootstepMarker::enable3DCB
virtual void enable3DCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:551
jsk_footstep_planner::FootstepMarker::lleg_footstep_offset_
FootstepVec lleg_footstep_offset_
Definition: footstep_marker.h:266
jsk_footstep_planner::FootstepMarker::pub_current_marker_mode_
ros::Publisher pub_current_marker_mode_
Definition: footstep_marker.h:251
jsk_footstep_planner::FootstepMarker::enableSingleCB
virtual void enableSingleCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:566
jsk_footstep_planner::FootstepMarker::menu_handler_
interactive_markers::MenuHandler menu_handler_
Definition: footstep_marker.h:273
simple_action_client.h
jsk_footstep_planner::FootstepMarker::stackedPosesMarker
virtual visualization_msgs::Marker stackedPosesMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
Definition: footstep_marker.cpp:960
jsk_footstep_planner::FootstepMarker::stack_mode_
interactive_markers::MenuHandler::EntryHandle stack_mode_
Definition: footstep_marker.h:281
jsk_footstep_planner::FootstepMarker::processFeedbackCB
virtual void processFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:761
jsk_footstep_planner::FootstepMarker::rleg_goal_pose_
FootstepTrans rleg_goal_pose_
Definition: footstep_marker.h:264
jsk_footstep_planner::FootstepMarker::distanceLineMarker
virtual visualization_msgs::Marker distanceLineMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
Definition: footstep_marker.cpp:814
jsk_footstep_planner::PosePair::Ptr
boost::shared_ptr< PosePair > Ptr
Definition: footstep_marker.h:115
jsk_footstep_planner::FootstepMarker::configCallback
virtual void configCallback(Config &config, uint32_t level)
Definition: footstep_marker.cpp:1074
jsk_footstep_planner::FootstepMarker::stack_btn_
interactive_markers::MenuHandler::EntryHandle stack_btn_
Definition: footstep_marker.h:274
jsk_footstep_planner::FootstepMarker::distanceTextMarker
virtual visualization_msgs::Marker distanceTextMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
Definition: footstep_marker.cpp:896
jsk_footstep_planner::FootstepMarker::Ptr
boost::shared_ptr< FootstepMarker > Ptr
Definition: footstep_marker.h:131
jsk_footstep_planner::FootstepMarker::server_
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
Definition: footstep_marker.h:271
jsk_footstep_planner::FootstepMarker::resetMarkerService
virtual bool resetMarkerService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: footstep_marker.cpp:1124
jsk_footstep_planner::FootstepMarker::originMarker
virtual visualization_msgs::Marker originMarker(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
Definition: footstep_marker.cpp:868
jsk_footstep_planner::FootstepMarker::targetArrow
virtual visualization_msgs::Marker targetArrow(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
Definition: footstep_marker.cpp:799
jsk_footstep_planner::FootstepMarker::callFollowPathPlan
virtual void callFollowPathPlan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:1277
jsk_footstep_planner::FootstepMarker::planDoneCB
virtual void planDoneCB(const actionlib::SimpleClientGoalState &state, const PlanResult::ConstPtr &result)
Definition: footstep_marker.cpp:735
jsk_footstep_planner::FootstepMarker::resetMarkerCB
virtual void resetMarkerCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:421
jsk_footstep_planner::FootstepMarker::original_foot_poses_
PosePair::Ptr original_foot_poses_
Definition: footstep_marker.h:263
jsk_footstep_planner::FootstepMarker::getFootstepMarkerPoseService
virtual bool getFootstepMarkerPoseService(jsk_interactive_marker::GetTransformableMarkerPose::Request &req, jsk_interactive_marker::GetTransformableMarkerPose::Response &res)
Definition: footstep_marker.cpp:1208
jsk_footstep_planner::FootstepMarker::getDefaultRightLegOffset
virtual FootstepTrans getDefaultRightLegOffset()
Definition: footstep_marker.cpp:757
jsk_footstep_planner::FootstepMarker::single_mode_
interactive_markers::MenuHandler::EntryHandle single_mode_
Definition: footstep_marker.h:279
jsk_footstep_planner::FootstepMarker::ac_planner_
PlanningActionClient ac_planner_
Definition: footstep_marker.h:248
jsk_footstep_planner::FootstepMarker::rleg_end_coords_
std::string rleg_end_coords_
Definition: footstep_marker.h:262
jsk_footstep_planner::FootstepMarker::foot_size_y_
double foot_size_y_
Definition: footstep_marker.h:286
jsk_footstep_planner::FootstepMarker::srv_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
Definition: footstep_marker.h:272
jsk_footstep_planner::FootstepMarker::command_mode_
CommandMode command_mode_
Definition: footstep_marker.h:284
jsk_footstep_planner::FootstepMarker::pub_plan_result_
ros::Publisher pub_plan_result_
Definition: footstep_marker.h:250
jsk_footstep_planner::FootstepMarker::pub_marker_array_
MarkerArrayPublisher pub_marker_array_
Definition: footstep_marker.h:247
jsk_footstep_planner::FootstepMarker::tf_client_
boost::shared_ptr< tf2_ros::BufferClient > tf_client_
Definition: footstep_marker.h:270
jsk_footstep_planner::FootstepMarker::cont_mode_
interactive_markers::MenuHandler::EntryHandle cont_mode_
Definition: footstep_marker.h:280
jsk_footstep_planner::FootstepMarker::last_steps_
jsk_footstep_msgs::Footstep last_steps_[2]
Definition: footstep_marker.h:296
jsk_footstep_planner::FootstepMarker::updateMarkerArray
virtual void updateMarkerArray(const std_msgs::Header &header, const geometry_msgs::Pose &pose)
Definition: footstep_marker.cpp:1014
jsk_footstep_planner::FootstepMarker::PlanningState
PlanningState
Definition: footstep_marker.h:141
jsk_footstep_planner::FootstepMarker::executeFootstepService
virtual bool executeFootstepService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: footstep_marker.cpp:1166
interactive_marker_server.h
jsk_footstep_planner::FootstepMarker::is_2d_mode_
bool is_2d_mode_
Definition: footstep_marker.h:282
ros::Time
jsk_footstep_planner::FootstepMarker::disable_tf_
bool disable_tf_
Definition: footstep_marker.h:287
jsk_footstep_planner::FootstepMarker::srv_get_fs_marker_pose_
ros::ServiceServer srv_get_fs_marker_pose_
Definition: footstep_marker.h:258
jsk_footstep_planner::FootstepMarker::STACK
@ STACK
Definition: footstep_marker.h:142
jsk_footstep_planner::FootstepMarker
Definition: footstep_marker.h:128
jsk_footstep_planner::PosePair::getByName
virtual FootstepTrans getByName(const std::string &name)
Definition: footstep_marker.cpp:116
jsk_footstep_planner::FootstepMarker::foot_size_x_
double foot_size_x_
Definition: footstep_marker.h:286
jsk_footstep_planner::FootstepVec
Eigen::Vector3d FootstepVec
Definition: footstep_marker.h:101
jsk_footstep_planner::FootstepMarker::getDefaultLeftLegOffset
virtual FootstepTrans getDefaultLeftLegOffset()
Definition: footstep_marker.cpp:753
jsk_footstep_planner::FootstepMarker::have_last_step_
bool have_last_step_
Definition: footstep_marker.h:295
jsk_footstep_planner::FootstepMarker::getLatestCurrentFootstepPoses
virtual PosePair::Ptr getLatestCurrentFootstepPoses()
Definition: footstep_marker.cpp:1039
jsk_footstep_planner::FootstepMarker::MenuCallbackFunction
boost::function< void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &)> MenuCallbackFunction
Definition: footstep_marker.h:139
jsk_footstep_planner::FootstepTrans
Eigen::Affine3d FootstepTrans
Definition: footstep_marker.h:100
jsk_footstep_planner::FootstepMarker::setupInitialMarker
virtual void setupInitialMarker(PosePair::Ptr leg_poses, visualization_msgs::InteractiveMarker &int_marker)
Definition: footstep_marker.cpp:267
jsk_footstep_planner::FootstepMarker::srv_stack_marker_pose_
ros::ServiceServer srv_stack_marker_pose_
Definition: footstep_marker.h:259
jsk_footstep_planner::FootstepMarker::srv_reset_fs_marker_
ros::ServiceServer srv_reset_fs_marker_
Definition: footstep_marker.h:253
jsk_footstep_planner::PosePair::PosePair
PosePair(const FootstepTrans &first, const std::string &first_name, const FootstepTrans &second, const std::string &second_name)
Definition: footstep_marker.cpp:108
jsk_footstep_planner::FootstepMarker::executeDoneCB
virtual void executeDoneCB(const actionlib::SimpleClientGoalState &state, const ExecResult::ConstPtr &result)
Definition: footstep_marker.cpp:431
jsk_footstep_planner::FootstepMarker::current_rleg_offset_
FootstepTrans current_rleg_offset_
Definition: footstep_marker.h:265
jsk_footstep_planner::FootstepMarker::entry_3d_mode_
interactive_markers::MenuHandler::EntryHandle entry_3d_mode_
Definition: footstep_marker.h:276
jsk_footstep_planner::FootstepMarker::srv_wait_for_exec_fs_
ros::ServiceServer srv_wait_for_exec_fs_
Definition: footstep_marker.h:256
jsk_footstep_planner::FootstepMarker::current_lleg_offset_
FootstepTrans current_lleg_offset_
Definition: footstep_marker.h:265
jsk_footstep_planner::FootstepMarker::waitForExecuteFootstepService
virtual bool waitForExecuteFootstepService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: footstep_marker.cpp:1184
jsk_footstep_planner::FootstepMarker::stackFootstepCB
virtual void stackFootstepCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:438
jsk_footstep_planner::FootstepMarker::srv_toggle_fs_com_mode_
ros::ServiceServer srv_toggle_fs_com_mode_
Definition: footstep_marker.h:254
jsk_footstep_planner::FootstepMarker::CONTINUOUS
@ CONTINUOUS
Definition: footstep_marker.h:142
jsk_footstep_planner::PosePair::second_name_
std::string second_name_
Definition: footstep_marker.h:124
jsk_footstep_planner::FootstepMarker::plan
virtual void plan(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:696
jsk_footstep_planner::FootstepMarker::ac_exec_
ExecuteActionClient ac_exec_
Definition: footstep_marker.h:249
jsk_footstep_planner::FootstepMarker::ON_GOING
@ ON_GOING
Definition: footstep_marker.h:141
jsk_footstep_planner::FootstepMarker::FINISHED
@ FINISHED
Definition: footstep_marker.h:141
jsk_footstep_planner::FootstepMarker::NOT_STARTED
@ NOT_STARTED
Definition: footstep_marker.h:141
jsk_footstep_planner::FootstepMarker::line_mode_
interactive_markers::MenuHandler::EntryHandle line_mode_
Definition: footstep_marker.h:278
jsk_footstep_planner::FootstepMarker::collision_bbox_offset_
FootstepTrans collision_bbox_offset_
Definition: footstep_marker.h:293
jsk_footstep_planner::FootstepMarker::waitForFootstepPlanService
virtual bool waitForFootstepPlanService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: footstep_marker.cpp:1196
jsk_footstep_planner::FootstepMarker::processPoseUpdateCB
virtual void processPoseUpdateCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:1026
ros::NodeHandle
ros::Subscriber
jsk_footstep_planner::PosePair::first_name_
std::string first_name_
Definition: footstep_marker.h:123
jsk_footstep_planner::FootstepMarker::ExecuteActionClient
actionlib::SimpleActionClient< jsk_footstep_msgs::ExecFootstepsAction > ExecuteActionClient
Definition: footstep_marker.h:135
jsk_footstep_planner::FootstepMarker::use_default_goal_
bool use_default_goal_
Definition: footstep_marker.h:288
jsk_footstep_planner::FootstepMarker::FootstepMarker
FootstepMarker()
Definition: footstep_marker.cpp:136


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jan 24 2024 04:05:29