locomove_base.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef LOCOMOVE_BASE_LOCOMOVE_BASE_H
36 #define LOCOMOVE_BASE_LOCOMOVE_BASE_H
37 
39 #include <locomotor/locomotor.h>
40 #include <move_base_msgs/MoveBaseAction.h>
42 #include <string>
43 #include <vector>
44 
45 namespace locomove_base
46 {
48 {
50 };
51 
53 {
54 public:
55  explicit LocoMoveBase(const ros::NodeHandle& nh);
56  void setGoal(nav_2d_msgs::Pose2DStamped goal);
57  void resetState();
58 protected:
59  void requestNavigationFailure(const locomotor_msgs::ResultCode& result);
60 
61  void planLoopCallback(const ros::TimerEvent& event);
63 
64  void onGlobalCostmapUpdate(const ros::Duration& planning_time);
66 
67  void onNewGlobalPlan(nav_2d_msgs::Path2D new_global_plan, const ros::Duration& planning_time);
69 
70  void controlLoopCallback(const ros::TimerEvent& event);
71 
72  void onLocalCostmapUpdate(const ros::Duration& planning_time);
74 
75  void onNewLocalPlan(nav_2d_msgs::Twist2DStamped new_command, const ros::Duration& planning_time);
77 
78  void onNavigationCompleted();
79  void onNavigationFailure(const locomotor_msgs::ResultCode result);
80 
81  void publishZeroVelocity();
82 
89 
94 
95  void recovery();
96 
99 
100  // Simple Goal Handling
101  void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
103 
104  // MoveBaseAction
105  void executeCB();
107 
108  // Recoveries
111  std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;
112  unsigned int recovery_index_;
114 
115  // Timer Stuff
116  double planner_frequency_ { 10.0 }, controller_frequency_ { 20.0 };
119 
120  // The Two Executors
122 
123  // Patience
125  double planner_patience_ { 5.0 }, controller_patience_ { 15.0 };
127  int max_planning_retries_ {-1}, // disabled by default
129 
130  // Oscillation variables
133  geometry_msgs::Pose2D oscillation_pose_;
134 
135  // Costmap Pointer Copies
139 
140  // Debug Publishing
142 };
143 } // namespace locomove_base
144 
145 #endif // LOCOMOVE_BASE_LOCOMOVE_BASE_H
ros::NodeHandle private_nh_
Definition: locomove_base.h:97
locomotor::Locomotor locomotor_
Definition: locomove_base.h:98
void controlLoopCallback(const ros::TimerEvent &event)
costmap_2d::Costmap2DROS * planner_costmap_ros_
locomotor::Executor local_planning_ex_
void onLocalCostmapUpdate(const ros::Duration &planning_time)
void onGlobalCostmapUpdate(const ros::Duration &planning_time)
geometry_msgs::Pose2D oscillation_pose_
bool loadRecoveryBehaviors(ros::NodeHandle node)
Load the recovery behaviors for the navigation stack from the parameter server.
void onNewLocalPlan(nav_2d_msgs::Twist2DStamped new_command, const ros::Duration &planning_time)
actionlib::SimpleActionServer< move_base_msgs::MoveBaseAction > server_
std::exception_ptr NavCore2ExceptionPtr
void loadDefaultRecoveryBehaviors()
Loads the default recovery behaviors for the navigation stack.
void onNavigationFailure(const locomotor_msgs::ResultCode result)
costmap_2d::Costmap2DROS * getCostmapPointer(const nav_core2::Costmap::Ptr &costmap)
void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal)
void onLocalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
void setGoal(nav_2d_msgs::Pose2DStamped goal)
ros::Duration desired_plan_duration_
void requestNavigationFailure(const locomotor_msgs::ResultCode &result)
LocoMoveBase(const ros::NodeHandle &nh)
ros::Duration desired_control_duration_
pluginlib::ClassLoader< nav_core::RecoveryBehavior > recovery_loader_
void planLoopCallback(const ros::TimerEvent &event)
std::vector< boost::shared_ptr< nav_core::RecoveryBehavior > > recovery_behaviors_
void onLocalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
std::shared_ptr< Costmap > Ptr
void onNewGlobalPlan(nav_2d_msgs::Path2D new_global_plan, const ros::Duration &planning_time)
void onGlobalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
costmap_2d::Costmap2DROS * controller_costmap_ros_
RecoveryTrigger recovery_trigger_
void onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
goal
locomotor::Executor global_planning_ex_


locomove_base
Author(s):
autogenerated on Wed Jun 26 2019 20:06:27