moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
hybrid_planning_manager.h
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2020, PickNik Inc.
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 PickNik Inc. 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 OWNER 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/* Author: Sebastian Jahr
36 Description: The hybrid planning manager component node that serves as the control unit of the whole architecture.
37 */
38
39#pragma once
40
41#include <rclcpp/rclcpp.hpp>
42#include <rclcpp_action/rclcpp_action.hpp>
43
44#include <moveit_msgs/action/local_planner.hpp>
45#include <moveit_msgs/action/global_planner.hpp>
46#include <moveit_msgs/action/hybrid_planner.hpp>
47
49
50#include <pluginlib/class_loader.hpp>
51
53{
58{
59public:
61 HybridPlanningManager(const rclcpp::NodeOptions& options);
62
65 {
66 // Join the thread used for long-running callbacks
67 if (long_callback_thread_.joinable())
68 {
69 long_callback_thread_.join();
70 }
71 }
72
77 bool initialize();
78
79 // This function is required to make this class a valid NodeClass
80 // see https://docs.ros2.org/latest/api/rclcpp_components/register__node__macro_8hpp.html
81 rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT
82 {
83 return node_->get_node_base_interface(); // NOLINT
84 }
85
89 void cancelHybridManagerGoals() noexcept;
90
96 std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::HybridPlanner>> goal_handle);
97
103
109
114 void sendHybridPlanningResponse(bool success);
115
121 void processReactionResult(const ReactionResult& result);
122
123private:
124 std::shared_ptr<rclcpp::Node> node_;
125
126 // Planner logic plugin loader
127 std::unique_ptr<pluginlib::ClassLoader<PlannerLogicInterface>> planner_logic_plugin_loader_;
128
129 // Planner logic instance to implement reactive behavior
130 std::shared_ptr<PlannerLogicInterface> planner_logic_instance_;
131
132 // Flag that indicates hybrid planning has been canceled
133 std::atomic<bool> stop_hybrid_planning_;
134
135 // Shared hybrid planning goal handle
136 std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::HybridPlanner>> hybrid_planning_goal_handle_;
137
138 // Frequently updated feedback for the hybrid planning action requester
139 std::shared_ptr<moveit_msgs::action::HybridPlanner_Feedback> hybrid_planning_progess_;
140
141 // Planning request action clients
142 rclcpp_action::Client<moveit_msgs::action::LocalPlanner>::SharedPtr local_planner_action_client_;
143 rclcpp_action::Client<moveit_msgs::action::GlobalPlanner>::SharedPtr global_planner_action_client_;
144
145 // Hybrid planning request action server
146 rclcpp_action::Server<moveit_msgs::action::HybridPlanner>::SharedPtr hybrid_planning_request_server_;
147
148 // Global solution subscriber
149 rclcpp::Subscription<moveit_msgs::msg::MotionPlanResponse>::SharedPtr global_solution_sub_;
150
151 // This thread is used for long-running callbacks. It's a member so they do not go out of scope.
152 std::thread long_callback_thread_;
153
154 // A unique callback group, to avoid mixing callbacks with other action servers
155 rclcpp::CallbackGroup::SharedPtr cb_group_;
156};
157} // namespace moveit::hybrid_planning
void executeHybridPlannerGoal(std::shared_ptr< rclcpp_action::ServerGoalHandle< moveit_msgs::action::HybridPlanner > > goal_handle)
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface()
void processReactionResult(const ReactionResult &result)
Process the action result and do an action call if necessary.