diff --git a/moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp b/moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp index dff5468674..812c6fb1fa 100644 --- a/moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp +++ b/moveit_ros/hybrid_planning/global_planner/src/global_planner_component.cpp @@ -80,6 +80,7 @@ GlobalPlannerComponent::GlobalPlannerComponent(const rclcpp::NodeOptions& option global_trajectory_pub_ = this->create_publisher("global_trajectory", 1); // Initialize global planner after construction + // TODO(sjahr) Remove once life cycle component nodes are available timer_ = this->create_wall_timer(1ms, [this]() { if (initialized_) { @@ -163,7 +164,6 @@ void GlobalPlannerComponent::globalPlanningRequestCallback( const auto goal = goal_handle->get_goal(); // Plan global trajectory - RCLCPP_INFO(LOGGER, "Start global planning"); moveit_msgs::msg::MotionPlanResponse planning_solution = plan(goal->request); // Publish global planning solution to the local planner diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h index 2a3c116f57..449ca6c888 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h @@ -45,9 +45,10 @@ #include #include -#include #include +#include + namespace moveit_hybrid_planning { /** diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h index d471951f91..3b9caab234 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h @@ -39,11 +39,76 @@ #pragma once #include - +#include #include namespace moveit_hybrid_planning { +// TODO(sjahr): Move this into utility package +class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes +{ +public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int code) const + { + return val == code; + } + bool operator!=(const int code) const + { + return val != code; + } +}; + +// Describes the outcome of a reaction to an event in the hybrid planning architecture +struct ReactionResult +{ + ReactionResult(const BasicHybridPlanningEvent& planning_event, const std::string& error_msg, const int& error_code) + : error_message(error_msg), error_code(error_code) + { + switch (planning_event) + { + case moveit_hybrid_planning::BasicHybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED: + event = "Hybrid planning request received"; + break; + case moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_PLANNING_ACTION_FINISHED: + event = "Global planning action finished"; + break; + case moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE: + event = "Global solution available"; + break; + case moveit_hybrid_planning::BasicHybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED: + event = "Local planning action finished"; + break; + } + }; + ReactionResult(const std::string& event, const std::string& error_msg, const int& error_code) + : event(event), error_message(error_msg), error_code(error_code){}; + + // Event that triggered the reaction + std::string event; + + // Additional error description + std::string error_message; + + // Error code + MoveItErrorCode error_code; +}; + class HybridPlanningManager; // Forward declaration /** @@ -65,16 +130,16 @@ class PlannerLogicInterface /** * React to event defined in BasicHybridPlanningEvent enum * @param event Basic hybrid planning event - * @return true if reaction was successful + * @return Reaction result that summarizes the outcome of the reaction */ - virtual bool react(BasicHybridPlanningEvent event) = 0; + virtual ReactionResult react(const BasicHybridPlanningEvent& event) = 0; /** * React to custom event * @param event Encoded as string - * @return true if reaction was successful + * @return Reaction result that summarizes the outcome of the reaction */ - virtual bool react(const std::string& event) = 0; + virtual ReactionResult react(const std::string& event) = 0; virtual ~PlannerLogicInterface(){}; protected: diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp index 7c3d470d02..fa1b787f31 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp @@ -48,6 +48,7 @@ HybridPlanningManager::HybridPlanningManager(const rclcpp::NodeOptions& options) { initialized_ = false; // Initialize hybrid planning component after after construction + // TODO(sjahr) Remove once life cycle component nodes are available timer_ = this->create_wall_timer(1ms, [this]() { if (initialized_) { @@ -77,7 +78,7 @@ bool HybridPlanningManager::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL(LOGGER, "Exception while creating planner logic plugin loader %s", ex.what()); + RCLCPP_FATAL(LOGGER, "Exception while creating planner logic plugin loader '%s'", ex.what()); } // TODO(sjahr) Refactor parameter declaration and use repository wide solution std::string logic_plugin_name = ""; @@ -101,7 +102,7 @@ bool HybridPlanningManager::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while loading planner logic '%s': %s", logic_plugin_name.c_str(), ex.what()); + RCLCPP_ERROR(LOGGER, "Exception while loading planner logic '%s': '%s'", logic_plugin_name.c_str(), ex.what()); } // Initialize local planning action client @@ -142,14 +143,23 @@ bool HybridPlanningManager::initialize() // Initialize global solution subscriber global_solution_sub_ = create_subscription( "global_trajectory", 1, [this](const moveit_msgs::msg::MotionPlanResponse::SharedPtr msg) { - planner_logic_instance_->react(moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE); + // Reaction result from the latest event + ReactionResult reaction_result = + planner_logic_instance_->react(moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE); + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } }); return true; } bool HybridPlanningManager::planGlobalTrajectory() { - RCLCPP_INFO(LOGGER, "START PLANNING"); auto global_goal_options = rclcpp_action::Client::SendGoalOptions(); // Add goal response callback @@ -170,17 +180,28 @@ bool HybridPlanningManager::planGlobalTrajectory() }; // Add result callback global_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - planner_logic_instance_->react( + [this](const rclcpp_action::ClientGoalHandle::WrappedResult& global_result) { + // Reaction result from the latest event + ReactionResult reaction_result = planner_logic_instance_->react( moveit_hybrid_planning::BasicHybridPlanningEvent::GLOBAL_PLANNING_ACTION_FINISHED); + auto result = std::make_shared(); + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } }; + // Forward global trajectory goal from hybrid planning request TODO(sjahr) pass goal as function argument auto global_goal_msg = moveit_msgs::action::GlobalPlanner::Goal(); global_goal_msg.request = latest_hybrid_planning_goal_; // Send global planning goal and wait until it's accepted auto goal_handle_future = global_planner_action_client_->async_send_goal(global_goal_msg, global_goal_options); return true; // return always success TODO(sjahr) add more error checking -} +}; bool HybridPlanningManager::runLocalPlanner() { @@ -210,13 +231,32 @@ bool HybridPlanningManager::runLocalPlanner() local_goal_options.feedback_callback = [this](rclcpp_action::ClientGoalHandle::SharedPtr /*unused*/, const std::shared_ptr local_planner_feedback) { - planner_logic_instance_->react(local_planner_feedback->feedback); + // Reaction result from the latest event + ReactionResult reaction_result = planner_logic_instance_->react(local_planner_feedback->feedback); + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } }; // Add result callback to print the result local_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - planner_logic_instance_->react(moveit_hybrid_planning::BasicHybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED); + // Reaction result from the latest event + ReactionResult reaction_result = planner_logic_instance_->react( + moveit_hybrid_planning::BasicHybridPlanningEvent::LOCAL_PLANNING_ACTION_FINISHED); + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } }; // Send global planning goal @@ -232,7 +272,17 @@ void HybridPlanningManager::hybridPlanningRequestCallback( latest_hybrid_planning_goal_ = (hybrid_planning_goal_handle_->get_goal())->request; // React on incoming planning request - planner_logic_instance_->react(moveit_hybrid_planning::BasicHybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED); + // Reaction result from the latest event + ReactionResult reaction_result = planner_logic_instance_->react( + moveit_hybrid_planning::BasicHybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED); + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + } } void HybridPlanningManager::sendHybridPlanningResponse(bool success) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h index dc9bf0a615..0ba575e95f 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/replan_invalidated_trajectory.h @@ -48,6 +48,6 @@ class ReplanInvalidatedTrajectory : public SinglePlanExecution // Inherit from public: ReplanInvalidatedTrajectory(){}; ~ReplanInvalidatedTrajectory() override{}; - bool react(const std::string& event) override; + ReactionResult react(const std::string& event) override; }; } // namespace moveit_hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h index 4db3831f51..0bc7079210 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h @@ -49,7 +49,7 @@ class SinglePlanExecution : public PlannerLogicInterface ~SinglePlanExecution() override{}; bool initialize(const std::shared_ptr& hybrid_planning_manager) override; - bool react(BasicHybridPlanningEvent event) override; - bool react(const std::string& event) override; + ReactionResult react(const BasicHybridPlanningEvent& event) override; + ReactionResult react(const std::string& event) override; }; } // namespace moveit_hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp index 1f45fdcad5..4b748d1eb6 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp @@ -41,7 +41,7 @@ namespace moveit_hybrid_planning { const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); -bool ReplanInvalidatedTrajectory::react(const std::string& event) +ReactionResult ReplanInvalidatedTrajectory::react(const std::string& event) { if (event == "collision_ahead") { @@ -49,12 +49,13 @@ bool ReplanInvalidatedTrajectory::react(const std::string& event) { hybrid_planning_manager_->sendHybridPlanningResponse(false); } + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } - else if (!event.empty()) + else { - RCLCPP_WARN(LOGGER, "Received unkown event: %s", event.c_str()); + return ReactionResult(event, "'ReplanInvalidatedTrajectory' plugin cannot handle this event.", + moveit_msgs::msg::MoveItErrorCodes::FAILURE); } - return true; }; } // namespace moveit_hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp index b2d1da1a9f..067e08a4a4 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp @@ -49,7 +49,7 @@ bool SinglePlanExecution::initialize( return true; } -bool SinglePlanExecution::react(BasicHybridPlanningEvent event) +ReactionResult SinglePlanExecution::react(const BasicHybridPlanningEvent& event) { switch (event) { @@ -71,16 +71,16 @@ bool SinglePlanExecution::react(BasicHybridPlanningEvent event) hybrid_planning_manager_->sendHybridPlanningResponse(true); break; default: - // Do nothing break; + // Do nothing } - return true; + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); } -bool SinglePlanExecution::react(const std::string& event) + +ReactionResult SinglePlanExecution::react(const std::string& event) { - auto& clock = *hybrid_planning_manager_->get_clock(); - RCLCPP_INFO_THROTTLE(LOGGER, clock, 1000, event.c_str()); - return true; + return ReactionResult(event, "'Single-Plan-Execution' plugin cannot handle events given as string.", + moveit_msgs::msg::MoveItErrorCodes::FAILURE); }; } // namespace moveit_hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h index 5451d83801..cf2afbc186 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h @@ -54,9 +54,10 @@ class ForwardTrajectory : public LocalConstraintSolverInterface const std::string& group_name) override; bool reset() override; - moveit_msgs::action::LocalPlanner::Feedback solve(const robot_trajectory::RobotTrajectory& local_trajectory, - const std::vector& local_constraints, - trajectory_msgs::msg::JointTrajectory& local_solution) override; + moveit_msgs::action::LocalPlanner::Feedback + solve(const robot_trajectory::RobotTrajectory& local_trajectory, + const std::shared_ptr local_goal, + trajectory_msgs::msg::JointTrajectory& local_solution) override; private: rclcpp::Node::SharedPtr node_handle_; diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp index 32643cb28a..fa36a82432 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp @@ -60,12 +60,13 @@ bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node, bool ForwardTrajectory::reset() { + path_invalidation_event_send_ = false; return true; }; moveit_msgs::action::LocalPlanner::Feedback ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajectory, - const std::vector& local_constraints, + const std::shared_ptr local_goal, trajectory_msgs::msg::JointTrajectory& local_solution) { // Create controller command trajectory diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h index 422b677caf..f2760a4b35 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_constraint_solver_interface.h @@ -70,13 +70,13 @@ class LocalConstraintSolverInterface /** * Solve local planning problem for the current loop run * @param local_trajectory The local trajectory to pursue - * @param local_constraints Local goal constraints + * @param local_goal Local goal constraints * @param local_solution solution plan in joint space * @return Feedback event from the current solver call i.e. "Collision detected" */ virtual moveit_msgs::action::LocalPlanner::Feedback solve(const robot_trajectory::RobotTrajectory& local_trajectory, - const std::vector& local_constraints, + const std::shared_ptr local_goal, trajectory_msgs::msg::JointTrajectory& local_solution) = 0; /** diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h index 3ee7cbb313..f72c26f515 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h @@ -76,7 +76,7 @@ void declareOrGetParam(const std::string& param_name, T& output_value, const T& catch (const rclcpp::exceptions::InvalidParameterTypeException& e) { RCLCPP_ERROR_STREAM(node->get_logger(), - "Error getting parameter \'" << param_name << "\', check parameter type in YAML file"); + "Error getting parameter '" << param_name << "', check parameter type in YAML file"); throw e; } } @@ -105,6 +105,7 @@ class LocalPlannerComponent : public rclcpp::Node void load(const rclcpp::Node::SharedPtr& node) { std::string undefined = ""; + declareOrGetParam("group_name", group_name, undefined, node); declareOrGetParam("trajectory_operator_plugin_name", trajectory_operator_plugin_name, undefined, node); declareOrGetParam("local_constraint_solver_plugin_name", local_constraint_solver_plugin_name, @@ -115,6 +116,7 @@ class LocalPlannerComponent : public rclcpp::Node declareOrGetParam("local_solution_topic_type", local_solution_topic_type, undefined, node); } + std::string group_name; std::string robot_description; std::string robot_description_semantic; std::string publish_planning_scene_topic; @@ -152,9 +154,12 @@ class LocalPlannerComponent : public rclcpp::Node // Timer to periodically call executePlanningLoopRun() rclcpp::TimerBase::SharedPtr timer_; - // Current action goal handle + // Latest action goal handle std::shared_ptr> local_planning_goal_handle_; + // Local planner feedback + std::shared_ptr local_planner_feedback_; + // Planning scene monitor to get the current planning scene planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h index 74d077b5b7..674a4350be 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/trajectory_operator_interface.h @@ -43,6 +43,9 @@ #include #include + +#include + #include #include @@ -72,14 +75,17 @@ class TrajectoryOperatorInterface * @param new_trajectory New reference trajectory segment to add * @return True if segment was successfully added */ - virtual bool addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) = 0; + virtual moveit_msgs::action::LocalPlanner::Feedback + addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) = 0; /** * Return the current local constraints based on the newest robot state * @param current_state Current RobotState * @return Current local constraints that define the local planning goal */ - virtual robot_trajectory::RobotTrajectory getLocalTrajectory(const moveit::core::RobotState& current_state) = 0; + virtual moveit_msgs::action::LocalPlanner::Feedback + getLocalTrajectory(const moveit::core::RobotState& current_state, + robot_trajectory::RobotTrajectory& local_trajectory) = 0; /** * Return the processing status of the reference trajectory's execution based on a user defined diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp index 7b58fa7899..589d66b8bc 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -52,8 +52,10 @@ LocalPlannerComponent::LocalPlannerComponent(const rclcpp::NodeOptions& options) : Node("local_planner_component", options) { state_ = moveit_hybrid_planning::LocalPlannerState::UNCONFIGURED; + local_planner_feedback_ = std::make_shared(); // Initialize local planner after construction + // TODO(sjahr) Remove once life cycle component nodes are available timer_ = this->create_wall_timer(1ms, [this]() { switch (state_) { @@ -110,20 +112,20 @@ bool LocalPlannerComponent::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL(LOGGER, "Exception while creating trajectory operator plugin loader %s", ex.what()); + RCLCPP_FATAL(LOGGER, "Exception while creating trajectory operator plugin loader: '%s'", ex.what()); } try { trajectory_operator_instance_ = trajectory_operator_loader_->createUniqueInstance(config_.trajectory_operator_plugin_name); if (!trajectory_operator_instance_->initialize(node_ptr, planning_scene_monitor_->getRobotModel(), - "panda_arm")) // TODO(sjahr) add default group param + config_.group_name)) // TODO(sjahr) add default group param throw std::runtime_error("Unable to initialize trajectory operator plugin"); RCLCPP_INFO(LOGGER, "Using trajectory operator interface '%s'", config_.trajectory_operator_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while loading trajectory operator '%s': %s", + RCLCPP_ERROR(LOGGER, "Exception while loading trajectory operator '%s': '%s'", config_.trajectory_operator_plugin_name.c_str(), ex.what()); } @@ -136,19 +138,19 @@ bool LocalPlannerComponent::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_FATAL(LOGGER, "Exception while creating constraint solver plugin loader %s", ex.what()); + RCLCPP_FATAL(LOGGER, "Exception while creating constraint solver plugin loader '%s'", ex.what()); } try { local_constraint_solver_instance_ = local_constraint_solver_plugin_loader_->createUniqueInstance(config_.local_constraint_solver_plugin_name); - if (!local_constraint_solver_instance_->initialize(node_ptr, planning_scene_monitor_, "panda_arm")) + if (!local_constraint_solver_instance_->initialize(node_ptr, planning_scene_monitor_, config_.group_name)) throw std::runtime_error("Unable to initialize constraint solver plugin"); RCLCPP_INFO(LOGGER, "Using constraint solver interface '%s'", config_.local_constraint_solver_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(LOGGER, "Exception while loading constraint solver '%s': %s", + RCLCPP_ERROR(LOGGER, "Exception while loading constraint solver '%s': '%s'", config_.local_constraint_solver_plugin_name.c_str(), ex.what()); } @@ -181,13 +183,21 @@ bool LocalPlannerComponent::initialize() moveit::core::RobotState start_state(planning_scene_monitor_->getRobotModel()); moveit::core::robotStateMsgToRobotState(msg->trajectory_start, start_state); new_trajectory.setRobotTrajectoryMsg(start_state, msg->trajectory); - trajectory_operator_instance_->addTrajectorySegment(new_trajectory); + *local_planner_feedback_ = trajectory_operator_instance_->addTrajectorySegment(new_trajectory); + + // Feedback is only send when the hybrid planning architecture should react to a discrete event that occurred + // when the reference trajectory is updated + if (!local_planner_feedback_->feedback.empty()) + { + local_planning_goal_handle_->publish_feedback(local_planner_feedback_); + } // Update local planner state state_ = moveit_hybrid_planning::LocalPlannerState::LOCAL_PLANNING_ACTIVE; }); // Initialize local solution publisher + RCLCPP_INFO(LOGGER, "Using '%s' as local solution topic type", config_.local_solution_topic_type.c_str()); if (config_.local_solution_topic_type == "trajectory_msgs/JointTrajectory") { local_trajectory_publisher_ = @@ -261,21 +271,32 @@ void LocalPlannerComponent::executePlanningLoopRun() // Get local goal trajectory to follow robot_trajectory::RobotTrajectory local_trajectory = - trajectory_operator_instance_->getLocalTrajectory(current_robot_state); - const auto goal = local_planning_goal_handle_->get_goal(); + robot_trajectory::RobotTrajectory(planning_scene_monitor_->getRobotModel(), config_.group_name); + *local_planner_feedback_ = + trajectory_operator_instance_->getLocalTrajectory(current_robot_state, local_trajectory); + + // Feedback is only send when the hybrid planning architecture should react to a discrete event that occurred + // during the identification of the local planning problem + if (!local_planner_feedback_->feedback.empty()) + { + local_planning_goal_handle_->publish_feedback(local_planner_feedback_); + } // Solve local planning problem trajectory_msgs::msg::JointTrajectory local_solution; - auto local_feedback = std::make_shared(); - *local_feedback = - local_constraint_solver_instance_->solve(local_trajectory, goal->local_constraints, local_solution); - if (!local_feedback->feedback.empty()) + // Feedback is only send when the hybrid planning architecture should react to a discrete event that occurred + // while computing a local solution + *local_planner_feedback_ = local_constraint_solver_instance_->solve( + local_trajectory, local_planning_goal_handle_->get_goal(), local_solution); + + // Feedback is only send when the hybrid planning architecture should react to a discrete event + if (!local_planner_feedback_->feedback.empty()) { - local_planning_goal_handle_->publish_feedback(local_feedback); + local_planning_goal_handle_->publish_feedback(local_planner_feedback_); } - // Use the same message interface to controllers like MoveIt servo to use application dependend controller + // Use a configurable message interface like MoveIt serve // (See https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/src/servo_calcs.cpp) // Format outgoing msg in the right format // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h index 7516d11655..6a9f0b0c46 100644 --- a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/include/moveit/trajectory_operator_plugins/simple_sampler.h @@ -51,8 +51,11 @@ class SimpleSampler : public TrajectoryOperatorInterface bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name) override; - bool addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) override; - robot_trajectory::RobotTrajectory getLocalTrajectory(const moveit::core::RobotState& current_state) override; + moveit_msgs::action::LocalPlanner::Feedback + addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) override; + moveit_msgs::action::LocalPlanner::Feedback + getLocalTrajectory(const moveit::core::RobotState& current_state, + robot_trajectory::RobotTrajectory& local_trajectory) override; double getTrajectoryProgress(const moveit::core::RobotState& current_state) override; bool reset() override; @@ -60,6 +63,7 @@ class SimpleSampler : public TrajectoryOperatorInterface std::size_t next_waypoint_index_; // Indicates which reference trajectory waypoint is the current local goal constrained bool pass_through_; // If true, the reference_trajectory is simply forwarded each time the getLocalTrajectory() function is called + moveit_msgs::action::LocalPlanner::Feedback feedback_; // Empty feedback trajectory_processing::TimeOptimalTrajectoryGeneration time_parametrization_; }; } // namespace moveit_hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp index 913a4e0920..255882b240 100644 --- a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/src/simple_sampler.cpp @@ -48,16 +48,20 @@ bool SimpleSampler::initialize(const rclcpp::Node::SharedPtr& node, const moveit { // Load parameter & initialize member variables if (node->has_parameter("pass_through")) + { node->get_parameter("pass_through", pass_through_); + } else + { pass_through_ = node->declare_parameter("pass_through", false); - + } reference_trajectory_ = std::make_shared(robot_model, group_name); next_waypoint_index_ = 0; return true; } -bool SimpleSampler::addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) +moveit_msgs::action::LocalPlanner::Feedback +SimpleSampler::addTrajectorySegment(const robot_trajectory::RobotTrajectory& new_trajectory) { // Reset trajectory operator to delete old reference trajectory reset(); @@ -67,7 +71,9 @@ bool SimpleSampler::addTrajectorySegment(const robot_trajectory::RobotTrajectory // Parametrize trajectory and calculate velocity and accelerations time_parametrization_.computeTimeStamps(*reference_trajectory_); - return true; + + // Return empty feedback + return feedback_; } bool SimpleSampler::reset() @@ -77,14 +83,18 @@ bool SimpleSampler::reset() reference_trajectory_->clear(); return true; } -robot_trajectory::RobotTrajectory SimpleSampler::getLocalTrajectory(const moveit::core::RobotState& current_state) +moveit_msgs::action::LocalPlanner::Feedback +SimpleSampler::getLocalTrajectory(const moveit::core::RobotState& current_state, + robot_trajectory::RobotTrajectory& local_trajectory) { - robot_trajectory::RobotTrajectory local_trajectory(reference_trajectory_->getRobotModel(), - reference_trajectory_->getGroupName()); + // Delete previous local trajectory + local_trajectory.clear(); + + // Determine current local trajectory based on configured behavior if (pass_through_) { // Use reference_trajectory as local trajectory - local_trajectory.swap(*reference_trajectory_); + local_trajectory.append(*reference_trajectory_, 0.0, 0, reference_trajectory_->getWayPointCount()); } else { @@ -98,11 +108,13 @@ robot_trajectory::RobotTrajectory SimpleSampler::getLocalTrajectory(const moveit next_waypoint_index_ += 1; } - // Construct local trajectory containing the next three global trajectory waypoints + // Construct local trajectory containing the next global trajectory waypoint local_trajectory.addSuffixWayPoint(reference_trajectory_->getWayPoint(next_waypoint_index_), reference_trajectory_->getWayPointDurationFromPrevious(next_waypoint_index_)); } - return local_trajectory; + + // Return empty feedback + return feedback_; } double SimpleSampler::getTrajectoryProgress(const moveit::core::RobotState& current_state) diff --git a/moveit_ros/hybrid_planning/test/config/demo_controller.yaml b/moveit_ros/hybrid_planning/test/config/demo_controller.yaml index 272d57e8cc..0e2905a546 100644 --- a/moveit_ros/hybrid_planning/test/config/demo_controller.yaml +++ b/moveit_ros/hybrid_planning/test/config/demo_controller.yaml @@ -10,6 +10,14 @@ controller_manager: panda_arm_controller: ros__parameters: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 command_interfaces: - position state_interfaces: diff --git a/moveit_ros/hybrid_planning/test/config/local_planner.yaml b/moveit_ros/hybrid_planning/test/config/local_planner.yaml index 2ffca40366..819b5a0a98 100644 --- a/moveit_ros/hybrid_planning/test/config/local_planner.yaml +++ b/moveit_ros/hybrid_planning/test/config/local_planner.yaml @@ -6,9 +6,10 @@ local_planning_frequency: 100.0 global_solution_topic: "global_trajectory" local_solution_topic: "/panda_joint_group_position_controller/commands" # or panda_arm_controller/joint_trajectory local_solution_topic_type: "std_msgs/Float64MultiArray" # or trajectory_msgs/JointTrajectory +group_name: "panda_arm" # SimpleSampler param pass_through: false # ForwardTrajectory param -stop_before_collision: false +stop_before_collision: true