double_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 
56 {
57 public:
58  explicit DoubleThreadLocomotor(const ros::NodeHandle& private_nh)
59  : private_nh_(private_nh), locomotor_(private_nh_),
61  as_(private_nh_, std::bind(&DoubleThreadLocomotor::setGoal, this, std::placeholders::_1))
62  {
66  this, false, false); // one_shot=false(default), auto_start=false
71  this, false, false); // one_shot=false(default), auto_start=false
76  }
77 
78  void setGoal(nav_2d_msgs::Pose2DStamped goal)
79  {
80  locomotor_.setGoal(goal);
82  }
83 
84 protected:
85  void planLoopCallback(const ros::TimerEvent& event)
86  {
88  }
89 
91  {
93  std::bind(&DoubleThreadLocomotor::onGlobalCostmapUpdate, this, std::placeholders::_1),
94  std::bind(&DoubleThreadLocomotor::onGlobalCostmapException, this, std::placeholders::_1, std::placeholders::_2));
95  }
96 
97  void requestNavigationFailure(const locomotor_msgs::ResultCode& result)
98  {
100  std::bind(&DoubleThreadLocomotor::onNavigationFailure, this, std::placeholders::_1));
101  }
102 
103  // Locomotor Callbacks
104  void onGlobalCostmapUpdate(const ros::Duration& planning_time)
105  {
106  // Run the global planning on the separate executor, but put the result on the main executor
108  std::bind(&DoubleThreadLocomotor::onNewGlobalPlan, this, std::placeholders::_1, std::placeholders::_2),
109  std::bind(&DoubleThreadLocomotor::onGlobalPlanningException, this, std::placeholders::_1, std::placeholders::_2));
110  }
111 
113  {
114  requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_COSTMAP, getResultCode(e_ptr),
115  "Global Costmap failure."));
116  }
117 
118  void onNewGlobalPlan(nav_2d_msgs::Path2D new_global_plan, const ros::Duration& planning_time)
119  {
120  locomotor_.publishPath(new_global_plan);
121  locomotor_.getCurrentLocalPlanner().setPlan(new_global_plan);
122 
123  if (planning_time > desired_plan_duration_)
124  {
125  ROS_WARN_NAMED("locomotor", "Global planning missed its desired rate of %.4fHz... "
126  "the loop actually took %.4f seconds (>%.4f).",
128  }
131  }
132 
134  {
135  ROS_ERROR_NAMED("Locomotor", "Global planning error. Giving up.");
136  requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::GLOBAL_PLANNER, getResultCode(e_ptr),
137  "Global Planning Failure."));
138  }
139 
141  {
143  std::bind(&DoubleThreadLocomotor::onLocalCostmapUpdate, this, std::placeholders::_1),
144  std::bind(&DoubleThreadLocomotor::onLocalCostmapException, this, std::placeholders::_1, std::placeholders::_2));
145  }
146 
147  void onLocalCostmapUpdate(const ros::Duration& planning_time)
148  {
150  std::bind(&DoubleThreadLocomotor::onNewLocalPlan, this, std::placeholders::_1, std::placeholders::_2),
151  std::bind(&DoubleThreadLocomotor::onLocalPlanningException, this, std::placeholders::_1, std::placeholders::_2),
153  }
154 
156  {
157  requestNavigationFailure(makeResultCode(locomotor_msgs::ResultCode::LOCAL_COSTMAP, getResultCode(e_ptr),
158  "Local Costmap failure."));
159  }
160 
161  void onNewLocalPlan(nav_2d_msgs::Twist2DStamped new_command, const ros::Duration& planning_time)
162  {
163  locomotor_.publishTwist(new_command);
164  if (planning_time > desired_control_duration_)
165  {
166  ROS_WARN_NAMED("locomotor", "Control loop missed its desired rate of %.4fHz... "
167  "the loop actually took %.4f seconds (>%.4f).",
169  }
171  }
172 
174  {
175  ROS_WARN_NAMED("Locomotor", "Local planning error. Creating new global plan.");
178  }
179 
181  {
182  ROS_INFO_NAMED("Locomotor", "Plan completed! Stopping.");
186  }
187 
188  void onNavigationFailure(const locomotor_msgs::ResultCode result)
189  {
192  as_.failNavigation(result);
193  }
194 
196  // Locomotor Object
198 
199  // Timer Stuff
200  double planner_frequency_ { 10.0 }, controller_frequency_ { 20.0 };
203 
204  // The Two Executors
206 
207  // Action Server
209 };
210 }; // namespace locomotor
211 
212 int main(int argc, char** argv)
213 {
214  ros::init(argc, argv, "locomotor_node");
215  ros::NodeHandle nh("~");
217  ros::spin();
218  return 0;
219 }
virtual void setPlan(const nav_2d_msgs::Path2D &path)=0
void publishTwist(const nav_2d_msgs::Twist2DStamped &command)
Definition: locomotor.h:189
void failNavigation(const locomotor_msgs::ResultCode &result_code)
void onLocalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
nav_core2::LocalPlanner & getCurrentLocalPlanner()
Definition: locomotor.h:175
#define ROS_INFO_NAMED(name,...)
Collection of objects used in ROS CallbackQueue threading.
Definition: executor.h:71
void controlLoopCallback(const ros::TimerEvent &event)
#define ROS_WARN_NAMED(name,...)
locomotor_msgs::ResultCode makeResultCode(int component=-1, int result_code=-1, const std::string &message="")
Definition: locomotor.h:65
void onNewLocalPlan(nav_2d_msgs::Twist2DStamped new_command, const ros::Duration &planning_time)
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:131
void onGlobalPlanningException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
void start()
void initializeGlobalPlanners(Executor &ex)
Definition: locomotor.cpp:92
virtual void setGoal(nav_2d_msgs::Pose2DStamped goal)
Starts a new navigation to the given goal.
Definition: locomotor.cpp:110
std::exception_ptr NavCore2ExceptionPtr
ROSCPP_DECL void spin(Spinner &spinner)
void publishFeedback(const locomotor_msgs::NavigationState &nav_state)
void planLoopCallback(const ros::TimerEvent &event)
void onLocalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
void publishPath(const nav_2d_msgs::Path2D &global_plan)
Definition: locomotor.h:188
an extensible path planning coordination engine
Definition: locomotor.h:79
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void requestNavigationFailure(const locomotor_msgs::ResultCode &result)
void onGlobalCostmapException(nav_core2::NavCore2ExceptionPtr e_ptr, const ros::Duration &planning_time)
const locomotor_msgs::NavigationState & getNavigationState() const
Definition: locomotor.h:165
DoubleThreadLocomotor(const ros::NodeHandle &private_nh)
void initializeLocalCostmap(Executor &ex)
Definition: locomotor.cpp:82
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void initializeGlobalCostmap(Executor &ex)
Definition: locomotor.cpp:72
void setGoal(nav_2d_msgs::Pose2DStamped goal)
void initializeLocalPlanners(Executor &ex)
Definition: locomotor.cpp:101
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:151
void onGlobalCostmapUpdate(const ros::Duration &planning_time)
int main(int argc, char **argv)
void onNewGlobalPlan(nav_2d_msgs::Path2D new_global_plan, const ros::Duration &planning_time)
#define ROS_ERROR_NAMED(name,...)
Connect the callbacks in Locomotor to do global and local planning on two separate timers...
void onLocalCostmapUpdate(const ros::Duration &planning_time)
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:124
void onNavigationFailure(const locomotor_msgs::ResultCode result)
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:138
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:144


locomotor
Author(s):
autogenerated on Wed Jun 26 2019 20:06:18