footstep_marker.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, 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/or 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 #include <ros/ros.h>
38 
39 #include <jsk_recognition_msgs/PolygonArray.h>
40 #include <jsk_recognition_msgs/ModelCoefficientsArray.h>
41 #include <jsk_interactive_marker/FootstepMarkerConfig.h>
42 
44 #include <jsk_interactive_marker/SetPose.h>
45 #include <jsk_interactive_marker/MarkerSetPose.h>
47 
48 #include <geometry_msgs/PointStamped.h>
52 
53 #include <tf/transform_listener.h>
55 #include <jsk_footstep_msgs/PlanFootstepsAction.h>
56 #include <jsk_footstep_msgs/ExecFootstepsAction.h>
57 #include <geometry_msgs/Polygon.h>
58 #include <std_msgs/UInt8.h>
59 #include <std_msgs/Empty.h>
60 #include <std_srvs/Empty.h>
61 #include <jsk_recognition_msgs/SimpleOccupancyGridArray.h>
62 #include <dynamic_reconfigure/server.h>
63 
65 public:
66  typedef jsk_interactive_marker::FootstepMarkerConfig Config;
68  virtual ~FootstepMarker();
69  void updateInitialFootstep();
74  typedef jsk_footstep_msgs::PlanFootstepsResult PlanResult;
75 protected:
77  void processFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
78  void menuFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
79  void moveMarkerCB(const geometry_msgs::PoseStamped::ConstPtr& msg);
80  void menuCommandCB(const std_msgs::UInt8::ConstPtr& msg);
81  void executeCB(const std_msgs::Empty::ConstPtr& msg);
82  void resumeCB(const std_msgs::Empty::ConstPtr& msg);
84  const PlanResult::ConstPtr &result);
85  void processMenuFeedback(uint8_t id);
86  geometry_msgs::Polygon computePolygon(uint8_t leg);
87  void snapLegs();
88  geometry_msgs::Pose computeLegTransformation(uint8_t leg);
89  geometry_msgs::Pose getFootstepPose(bool leftp);
90  void changePlannerHeuristic(const std::string& heuristic);
91  void callEstimateOcclusion();
92  void cancelWalk();
93  void planIfPossible();
94  void resetLegPoses();
95  void lookGround();
96  void configCallback(Config& config, uint32_t level);
97  bool forceToReplan(std_srvs::Empty::Request& req, std_srvs::Empty::Request& res);
100  std::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
101  // projection to the planes
102  bool projectMarkerToPlane();
103 
104  jsk_recognition_msgs::SimpleOccupancyGridArray::ConstPtr latest_grids_;
105  // read a geometry_msgs/pose from the parameter specified.
106  // the format of the parameter is [x, y, z, xx, yy, zz, ww].
107  // where x, y and z means position and xx, yy, zz and ww means
108  // orientation.
109  void readPoseParam(ros::NodeHandle& pnh, const std::string param,
110  tf::Transform& offset);
111 
112  // execute footstep
113  // sending action goal to footstep controller
114  void executeFootstep();
115  void resumeFootstep();
116 
117  void projectionCallback(const geometry_msgs::PoseStamped& pose);
118 
119  visualization_msgs::Marker makeFootstepMarker(geometry_msgs::Pose pose);
120 
121  std::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
123  double foot_size_x_;
124  double foot_size_y_;
125  double foot_size_z_;
126  double footstep_margin_;
127  std::string marker_frame_id_;
128  geometry_msgs::PoseStamped marker_pose_;
141  std::shared_ptr<tf::TransformListener> tf_listener_;
146  bool show_6dof_control_;
149  bool plan_run_;
150  bool use_plane_snap_;
151  bool wait_snapit_server_;
156  bool use_2d_;
158  geometry_msgs::Pose lleg_pose_;
159  geometry_msgs::Pose rleg_pose_;
160  geometry_msgs::Pose lleg_initial_pose_;
161  geometry_msgs::Pose rleg_initial_pose_;
164  std::string lfoot_frame_id_;
165  std::string rfoot_frame_id_;
166 
167  // footstep plannner result
168  PlanResult::ConstPtr plan_result_;
169 };
FootstepMarker::initial_reference_frame_
std::string initial_reference_frame_
Definition: footstep_marker.h:189
FootstepMarker::wait_snapit_server_
bool wait_snapit_server_
Definition: footstep_marker.h:183
result
def result
FootstepMarker::projectMarkerToPlane
bool projectMarkerToPlane()
Definition: footstep_marker.cpp:492
FootstepMarker::use_initial_reference_
bool use_initial_reference_
Definition: footstep_marker.h:185
FootstepMarker::tf_listener_
std::shared_ptr< tf::TransformListener > tf_listener_
Definition: footstep_marker.h:173
FootstepMarker::rleg_initial_pose_
geometry_msgs::Pose rleg_initial_pose_
Definition: footstep_marker.h:193
FootstepMarker::lleg_offset_
tf::Transform lleg_offset_
Definition: footstep_marker.h:194
FootstepMarker::resumeCB
void resumeCB(const std_msgs::Empty::ConstPtr &msg)
Definition: footstep_marker.cpp:371
FootstepMarker::rfoot_frame_id_
std::string rfoot_frame_id_
Definition: footstep_marker.h:197
FootstepMarker::rleg_offset_
tf::Transform rleg_offset_
Definition: footstep_marker.h:195
msg
msg
ros::Publisher
FootstepMarker::updateInitialFootstep
void updateInitialFootstep()
Definition: footstep_marker.cpp:379
FootstepMarker::forceToReplan
bool forceToReplan(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)
Definition: footstep_marker.cpp:416
FootstepMarker::resume_sub_
ros::Subscriber resume_sub_
Definition: footstep_marker.h:164
FootstepMarker::move_marker_sub_
ros::Subscriber move_marker_sub_
Definition: footstep_marker.h:161
FootstepMarker::processFeedbackCB
void processFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:544
FootstepMarker::projection_sub_
ros::Subscriber projection_sub_
Definition: footstep_marker.h:165
FootstepMarker::getFootstepPose
geometry_msgs::Pose getFootstepPose(bool leftp)
Definition: footstep_marker.cpp:692
ros.h
FootstepMarker::processMenuFeedback
void processMenuFeedback(uint8_t id)
Definition: footstep_marker.cpp:422
FootstepMarker::menuCommandCB
void menuCommandCB(const std_msgs::UInt8::ConstPtr &msg)
Definition: footstep_marker.cpp:375
FootstepMarker::foot_size_x_
double foot_size_x_
Definition: footstep_marker.h:155
actionlib::SimpleClientGoalState
FootstepMarker::foot_size_z_
double foot_size_z_
Definition: footstep_marker.h:157
menu_handler.h
FootstepMarker::plan_if_possible_srv_
ros::ServiceServer plan_if_possible_srv_
Definition: footstep_marker.h:172
time_synchronizer.h
FootstepMarker::snapit_client_
ros::ServiceClient snapit_client_
Definition: footstep_marker.h:170
FootstepMarker::lleg_initial_pose_
geometry_msgs::Pose lleg_initial_pose_
Definition: footstep_marker.h:192
FootstepMarker::srv_
std::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
Definition: footstep_marker.h:132
FootstepMarker::readPoseParam
void readPoseParam(ros::NodeHandle &pnh, const std::string param, tf::Transform &offset)
Definition: footstep_marker.cpp:247
FootstepMarker::current_pose_pub_
ros::Publisher current_pose_pub_
Definition: footstep_marker.h:168
FootstepMarker
Definition: footstep_marker.h:64
FootstepMarker::latest_grids_
jsk_recognition_msgs::SimpleOccupancyGridArray::ConstPtr latest_grids_
Definition: footstep_marker.h:136
ros::ServiceServer
FootstepMarker::menu_handler_
interactive_markers::MenuHandler menu_handler_
Definition: footstep_marker.h:154
FootstepMarker::project_footprint_pub_
ros::Publisher project_footprint_pub_
Definition: footstep_marker.h:166
actionlib::SimpleActionClient< jsk_footstep_msgs::PlanFootstepsAction >
interactive_markers::MenuHandler
mutex
boost::mutex mutex
Definition: bounding_box_marker.cpp:44
simple_action_client.h
FootstepMarker::use_plane_snap_
bool use_plane_snap_
Definition: footstep_marker.h:182
FootstepMarker::server_
std::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
Definition: footstep_marker.h:153
subscriber.h
FootstepMarker::resetLegPoses
void resetLegPoses()
Definition: footstep_marker.cpp:279
FootstepMarker::marker_frame_id_
std::string marker_frame_id_
Definition: footstep_marker.h:159
FootstepMarker::FootstepMarker
FootstepMarker()
Definition: footstep_marker.cpp:57
FootstepMarker::use_projection_service_
bool use_projection_service_
Definition: footstep_marker.h:176
FootstepMarker::ExecuteActionClient
actionlib::SimpleActionClient< jsk_footstep_msgs::ExecFootstepsAction > ExecuteActionClient
Definition: footstep_marker.h:105
FootstepMarker::Config
jsk_interactive_marker::FootstepMarkerConfig Config
Definition: footstep_marker.h:98
ros::ServiceClient
FootstepMarker::rleg_pose_
geometry_msgs::Pose rleg_pose_
Definition: footstep_marker.h:191
FootstepMarker::computeLegTransformation
geometry_msgs::Pose computeLegTransformation(uint8_t leg)
Definition: footstep_marker.cpp:297
tf::Transform
FootstepMarker::use_footstep_planner_
bool use_footstep_planner_
Definition: footstep_marker.h:179
FootstepMarker::PlanResult
jsk_footstep_msgs::PlanFootstepsResult PlanResult
Definition: footstep_marker.h:106
FootstepMarker::footstep_pub_
ros::Publisher footstep_pub_
Definition: footstep_marker.h:169
FootstepMarker::executeFootstep
void executeFootstep()
Definition: footstep_marker.cpp:599
FootstepMarker::exec_sub_
ros::Subscriber exec_sub_
Definition: footstep_marker.h:163
state
state
FootstepMarker::projectionCallback
void projectionCallback(const geometry_msgs::PoseStamped &pose)
Definition: footstep_marker.cpp:582
FootstepMarker::resumeFootstep
void resumeFootstep()
Definition: footstep_marker.cpp:568
FootstepMarker::snapLegs
void snapLegs()
Definition: footstep_marker.cpp:325
FootstepMarker::menu_command_sub_
ros::Subscriber menu_command_sub_
Definition: footstep_marker.h:162
FootstepMarker::~FootstepMarker
virtual ~FootstepMarker()
Definition: footstep_marker.cpp:834
FootstepMarker::plan_result_
PlanResult::ConstPtr plan_result_
Definition: footstep_marker.h:200
FootstepMarker::ac_
PlanningActionClient ac_
Definition: footstep_marker.h:174
transform_listener.h
FootstepMarker::plane_mutex_
boost::mutex plane_mutex_
Definition: footstep_marker.h:130
FootstepMarker::planDoneCB
void planDoneCB(const actionlib::SimpleClientGoalState &state, const PlanResult::ConstPtr &result)
Definition: footstep_marker.cpp:681
FootstepMarker::planIfPossible
void planIfPossible()
Definition: footstep_marker.cpp:623
FootstepMarker::executeCB
void executeCB(const std_msgs::Empty::ConstPtr &msg)
Definition: footstep_marker.cpp:367
FootstepMarker::makeFootstepMarker
visualization_msgs::Marker makeFootstepMarker(geometry_msgs::Pose pose)
Definition: footstep_marker.cpp:748
interactive_marker_server.h
FootstepMarker::cancelWalk
void cancelWalk()
Definition: footstep_marker.cpp:479
FootstepMarker::lleg_pose_
geometry_msgs::Pose lleg_pose_
Definition: footstep_marker.h:190
FootstepMarker::plan_run_mutex_
boost::mutex plan_run_mutex_
Definition: footstep_marker.h:131
FootstepMarker::use_initial_footstep_tf_
bool use_initial_footstep_tf_
Definition: footstep_marker.h:184
FootstepMarker::plan_run_
bool plan_run_
Definition: footstep_marker.h:181
FootstepMarker::lfoot_frame_id_
std::string lfoot_frame_id_
Definition: footstep_marker.h:196
FootstepMarker::computePolygon
geometry_msgs::Polygon computePolygon(uint8_t leg)
Definition: footstep_marker.cpp:334
FootstepMarker::foot_size_y_
double foot_size_y_
Definition: footstep_marker.h:156
FootstepMarker::use_projection_topic_
bool use_projection_topic_
Definition: footstep_marker.h:177
synchronizer.h
FootstepMarker::callEstimateOcclusion
void callEstimateOcclusion()
Definition: footstep_marker.cpp:486
FootstepMarker::marker_pose_
geometry_msgs::PoseStamped marker_pose_
Definition: footstep_marker.h:160
FootstepMarker::lookGround
void lookGround()
Definition: footstep_marker.cpp:405
FootstepMarker::ac_exec_
ExecuteActionClient ac_exec_
Definition: footstep_marker.h:175
FootstepMarker::use_footstep_controller_
bool use_footstep_controller_
Definition: footstep_marker.h:180
FootstepMarker::footstep_margin_
double footstep_margin_
Definition: footstep_marker.h:158
FootstepMarker::lleg_first_
bool lleg_first_
Definition: footstep_marker.h:187
FootstepMarker::moveMarkerCB
void moveMarkerCB(const geometry_msgs::PoseStamped::ConstPtr &msg)
Definition: footstep_marker.cpp:724
FootstepMarker::changePlannerHeuristic
void changePlannerHeuristic(const std::string &heuristic)
Definition: footstep_marker.cpp:467
FootstepMarker::estimate_occlusion_client_
ros::ServiceClient estimate_occlusion_client_
Definition: footstep_marker.h:171
FootstepMarker::menuFeedbackCB
void menuFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: footstep_marker.cpp:540
FootstepMarker::always_planning_
bool always_planning_
Definition: footstep_marker.h:186
FootstepMarker::PlanningActionClient
actionlib::SimpleActionClient< jsk_footstep_msgs::PlanFootstepsAction > PlanningActionClient
Definition: footstep_marker.h:103
config
config
FootstepMarker::initializeInteractiveMarker
void initializeInteractiveMarker()
Definition: footstep_marker.cpp:759
FootstepMarker::show_6dof_control_
bool show_6dof_control_
Definition: footstep_marker.h:178
ros::NodeHandle
ros::Subscriber
FootstepMarker::use_2d_
bool use_2d_
Definition: footstep_marker.h:188
FootstepMarker::snapped_pose_pub_
ros::Publisher snapped_pose_pub_
Definition: footstep_marker.h:167
FootstepMarker::configCallback
void configCallback(Config &config, uint32_t level)
Definition: footstep_marker.cpp:224


jsk_interactive_marker
Author(s): furuta
autogenerated on Fri Aug 2 2024 08:50:24