single_thread_locomotor.cpp
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 #include <locomotor/locomotor.h>
37 #include <string>
38 
39 namespace locomotor
40 {
42 
54 {
55 public:
56  explicit SingleThreadLocomotor(const ros::NodeHandle& private_nh)
57  : private_nh_(private_nh), locomotor_(private_nh_), main_ex_(private_nh_, false),
58  as_(private_nh_, std::bind(&SingleThreadLocomotor::setGoal, this, std::placeholders::_1))
59  {
64  this, false, false); // one_shot=false(default), auto_start=false
69  }
70 
71  void setGoal(nav_2d_msgs::Pose2DStamped goal)
72  {
73  locomotor_.setGoal(goal);
75  std::bind(&SingleThreadLocomotor::onGlobalCostmapUpdate, this, std::placeholders::_1),
76  std::bind(&SingleThreadLocomotor::onGlobalCostmapException, this, std::placeholders::_1, std::placeholders::_2));
77  }
78 
79 protected:
81  {
83  std::bind(&SingleThreadLocomotor::onGlobalCostmapUpdate, this, std::placeholders::_1),
84  std::bind(&SingleThreadLocomotor::onGlobalCostmapException, this, std::placeholders::_1, std::placeholders::_2));
85  }
86 
87  void requestNavigationFailure(const locomotor_msgs::ResultCode& result)
88  {
90  std::bind(&SingleThreadLocomotor::onNavigationFailure, this, std::placeholders::_1));
91  }
92 
93  // Locomotor Callbacks
94  void onGlobalCostmapUpdate(const ros::Duration& planning_time)
95  {
97  std::bind(&SingleThreadLocomotor::onNewGlobalPlan, this, std::placeholders::_1, std::placeholders::_2),
98  std::bind(&SingleThreadLocomotor::onGlobalPlanningException, this, std::placeholders::_1, std::placeholders::_2));
99  }
100 
102  {
103  requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_COSTMAP, getResultCode(e_ptr),
104  "Global Costmap failure."));
105  }
106 
107  void onNewGlobalPlan(nav_2d_msgs::Path2D new_global_plan, const ros::Duration& planning_time)
108  {
109  locomotor_.publishPath(new_global_plan);
110  locomotor_.getCurrentLocalPlanner().setPlan(new_global_plan);
113  }
114 
116  {
117  ROS_ERROR_NAMED("Locomotor", "Global planning error. Giving up.");
118  requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_PLANNER, getResultCode(e_ptr),
119  "Global Planning Failure."));
120  }
121 
123  {
125  std::bind(&SingleThreadLocomotor::onLocalCostmapUpdate, this, std::placeholders::_1),
126  std::bind(&SingleThreadLocomotor::onLocalCostmapException, this, std::placeholders::_1, std::placeholders::_2));
127  }
128 
129  void onLocalCostmapUpdate(const ros::Duration& planning_time)
130  {
132  std::bind(&SingleThreadLocomotor::onNewLocalPlan, this, std::placeholders::_1, std::placeholders::_2),
133  std::bind(&SingleThreadLocomotor::onLocalPlanningException, this, std::placeholders::_1, std::placeholders::_2),
135  }
136 
138  {
139  requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::LOCAL_COSTMAP, getResultCode(e_ptr),
140  "Local Costmap failure."));
141  }
142 
143  void onNewLocalPlan(nav_2d_msgs::Twist2DStamped new_command, const ros::Duration& planning_time)
144  {
145  locomotor_.publishTwist(new_command);
146  if (planning_time > desired_control_duration_)
147  {
148  ROS_WARN_NAMED("locomotor", "Control loop missed its desired rate of %.4fHz... "
149  "the loop actually took %.4f seconds (>%.4f).",
151  }
153  }
154 
156  {
157  ROS_WARN_NAMED("Locomotor", "Local planning error. Creating new global plan.");
160  }
161 
163  {
164  ROS_INFO_NAMED("Locomotor", "Plan completed! Stopping.");
167  }
168 
169  void onNavigationFailure(const locomotor_msgs::ResultCode result)
170  {
172  as_.failNavigation(result);
173  }
174 
176  // Locomotor Object
178 
179  // Timer Stuff
180  double controller_frequency_ { 20.0 };
183 
184  // Main Executor using Global CallbackQueue
186 
187  // Action Server
189 };
190 }; // namespace locomotor
191 
192 int main(int argc, char** argv)
193 {
194  ros::init(argc, argv, "locomotor_node");
195  ros::NodeHandle nh("~");
197  ros::spin();
198  return 0;
199 }
void onNewGlobalPlan(nav_2d_msgs::Path2D new_global_plan, const ros::Duration &planning_time)
virtual void setPlan(const nav_2d_msgs::Path2D &path)=0
void onNavigationFailure(const locomotor_msgs::ResultCode result)
void onLocalCostmapUpdate(const ros::Duration &planning_time)
void publishTwist(const nav_2d_msgs::Twist2DStamped &command)
Definition: locomotor.h:190
void failNavigation(const locomotor_msgs::ResultCode &result_code)
nav_core2::LocalPlanner & getCurrentLocalPlanner()
Definition: locomotor.h:176
#define ROS_INFO_NAMED(name,...)
Collection of objects used in ROS CallbackQueue threading.
Definition: executor.h:72
#define ROS_WARN_NAMED(name,...)
void onGlobalCostmapUpdate(const ros::Duration &planning_time)
locomotor_msgs::ResultCode makeResultCode(int component=-1, int result_code=-1, const std::string &message="")
Definition: locomotor.h:66
void stop()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void requestLocalCostmapUpdate(Executor &work_ex, Executor &result_ex, CostmapUpdateCallback cb=nullptr, CostmapUpdateExceptionCallback fail_cb=nullptr)
Request the local costmap get updated as a new callback.
Definition: locomotor.cpp:132
void start()
SingleThreadLocomotor(const ros::NodeHandle &private_nh)
void initializeGlobalPlanners(Executor &ex)
Definition: locomotor.cpp:93
virtual void setGoal(nav_2d_msgs::Pose2DStamped goal)
Starts a new navigation to the given goal.
Definition: locomotor.cpp:111
std::exception_ptr NavCore2ExceptionPtr
ROSCPP_DECL void spin(Spinner &spinner)
void publishFeedback(const locomotor_msgs::NavigationState &nav_state)
void onLocalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
void publishPath(const nav_2d_msgs::Path2D &global_plan)
Definition: locomotor.h:189
an extensible path planning coordination engine
Definition: locomotor.h:80
bool param(const std::string &param_name, T &param_val, const T &default_val) const
const locomotor_msgs::NavigationState & getNavigationState() const
Definition: locomotor.h:166
void initializeLocalCostmap(Executor &ex)
Definition: locomotor.cpp:83
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void initializeGlobalCostmap(Executor &ex)
Definition: locomotor.cpp:73
Connect the callbacks in Locomotor to do global planning once and then local planning on a timer...
void controlLoopCallback(const ros::TimerEvent &event)
void initializeLocalPlanners(Executor &ex)
Definition: locomotor.cpp:102
void onNewLocalPlan(nav_2d_msgs::Twist2DStamped new_command, const ros::Duration &planning_time)
void requestNavigationFailure(Executor &result_ex, const locomotor_msgs::ResultCode &result, NavigationFailureCallback cb=nullptr)
Request that a onNavigationFailure event be added as a new callback.
Definition: locomotor.cpp:152
void setGoal(nav_2d_msgs::Pose2DStamped goal)
#define ROS_ERROR_NAMED(name,...)
int main(int argc, char **argv)
void requestGlobalCostmapUpdate(Executor &work_ex, Executor &result_ex, CostmapUpdateCallback cb=nullptr, CostmapUpdateExceptionCallback fail_cb=nullptr)
Request the global costmap get updated as a new callback.
Definition: locomotor.cpp:125
void onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
void onLocalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
void requestNavigationFailure(const locomotor_msgs::ResultCode &result)
void onGlobalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
void requestGlobalPlan(Executor &work_ex, Executor &result_ex, GlobalPlanCallback cb=nullptr, PlannerExceptionCallback fail_cb=nullptr)
Request the global planner get run as a new callback.
Definition: locomotor.cpp:139
int getResultCode(const NavCore2ExceptionPtr &e_ptr)
void requestLocalPlan(Executor &work_ex, Executor &result_ex, LocalPlanCallback cb=nullptr, PlannerExceptionCallback fail_cb=nullptr, NavigationCompleteCallback complete_cb=nullptr)
Request the local planner get run as a new callback.
Definition: locomotor.cpp:145


locomotor
Author(s):
autogenerated on Sun Jan 10 2021 04:08:39