Add forward_trajectory local solver plugin

moveit_ros/hybrid_planning/CMakeLists.txt

@@ -30,6 +30,7 @@ set(LIBRARIES
   single_plan_execution_plugin
   decelerate_before_collision_plugin
   replan_invalidated_trajectory_plugin
+  forward_trajectory_plugin
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
@@ -82,6 +83,7 @@ pluginlib_export_plugin_description_file(moveit_hybrid_planning single_plan_exec
 pluginlib_export_plugin_description_file(moveit_hybrid_planning next_waypoint_sampler_plugin.xml)
 pluginlib_export_plugin_description_file(moveit_hybrid_planning decelerate_before_collision_plugin.xml)
 pluginlib_export_plugin_description_file(moveit_hybrid_planning replan_invalidated_trajectory_plugin.xml)
+pluginlib_export_plugin_description_file(moveit_hybrid_planning forward_trajectory_plugin.xml)
 
 ament_export_include_directories(include)
 ament_export_libraries(${LIBRARIES})

moveit_ros/hybrid_planning/forward_trajectory_plugin.xml

@@ -0,0 +1,7 @@
+<library path="forward_trajectory_plugin">
+  <class name="moveit_hybrid_planning/ForwardTrajectory" type="moveit_hybrid_planning::ForwardTrajectory" base_class_type="moveit_hybrid_planning::LocalConstraintSolverInterface">
+    <description>
+    Simple trajectory operator plugin that appends new global trajectories to the local planner's reference trajectory and extracts the next trajectory point based on the current robot state and an index.
+    </description>
+  </class>
+</library>

moveit_ros/hybrid_planning/local_constraint_solver_plugins/CMakeLists.txt

@@ -4,3 +4,10 @@ add_library(decelerate_before_collision_plugin SHARED
 set_target_properties(decelerate_before_collision_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 ament_target_dependencies(decelerate_before_collision_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})
 target_link_libraries(decelerate_before_collision_plugin local_constraint_solver_interface)
+
+add_library(forward_trajectory_plugin SHARED
+  src/forward_trajectory.cpp
+)
+set_target_properties(forward_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+ament_target_dependencies(forward_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(forward_trajectory_plugin local_constraint_solver_interface)

moveit_ros/hybrid_planning/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h

@@ -0,0 +1,59 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, PickNik LLC
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik LLC nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Sebastian Jahr
+   Description: Simple local solver plugin that forwards the next waypoint of the sampled local trajectory.
+ */
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <moveit/local_planner/local_constraint_solver_interface.h>
+
+namespace moveit_hybrid_planning
+{
+class ForwardTrajectory : public LocalConstraintSolverInterface
+{
+public:
+  ForwardTrajectory(){};
+  ~ForwardTrajectory() override{};
+  bool initialize(const rclcpp::Node::SharedPtr& node,
+                  const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                  const std::string& group_name) override;
+
+  moveit_msgs::action::LocalPlanner::Feedback solve(const robot_trajectory::RobotTrajectory& local_trajectory,
+                                                    const std::vector<moveit_msgs::msg::Constraints>& local_constraints,
+                                                    trajectory_msgs::msg::JointTrajectory& local_solution) override;
+};
+}  // namespace moveit_hybrid_planning

moveit_ros/hybrid_planning/local_constraint_solver_plugins/src/forward_trajectory.cpp

@@ -0,0 +1,76 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, PickNik LLC
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik LLC nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Sebastian Jahr
+ */
+
+#include <moveit/local_constraint_solver_plugins/forward_trajectory.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_state/conversions.h>
+
+namespace moveit_hybrid_planning
+{
+bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node,
+                                   const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                   const std::string& group_name)
+{
+  return true;
+}
+
+moveit_msgs::action::LocalPlanner::Feedback
+ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajectory,
+                         const std::vector<moveit_msgs::msg::Constraints>& local_constraints,
+                         trajectory_msgs::msg::JointTrajectory& local_solution)
+{
+  // Create controller command trajectory
+  robot_trajectory::RobotTrajectory robot_command(local_trajectory.getRobotModel(), local_trajectory.getGroupName());
+
+  // Forward next waypoint to the robot controller
+  robot_command.addSuffixWayPoint(local_trajectory.getWayPoint(0), 0.0);
+
+  // Transform into ROS 2 msg
+  moveit_msgs::msg::RobotTrajectory robot_command_msg;
+  robot_command.getRobotTrajectoryMsg(robot_command_msg);
+  local_solution = robot_command_msg.joint_trajectory;
+
+  // Empty feedback result
+  moveit_msgs::action::LocalPlanner::Feedback feedback_result;
+  return feedback_result;
+}
+}  // namespace moveit_hybrid_planning
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(moveit_hybrid_planning::ForwardTrajectory,
+                       moveit_hybrid_planning::LocalConstraintSolverInterface);

moveit_ros/hybrid_planning/package.xml

@@ -37,6 +37,7 @@
     <moveit_hybrid_planning plugin="${prefix}/next_waypoint_sampler_plugin.xml"/>
     <moveit_hybrid_planning plugin="${prefix}/replan_invalidated_trajectory_plugin.xml"/>
     <moveit_hybrid_planning plugin="${prefix}/single_plan_execution_plugin.xml"/>
+    <moveit_hybrid_planning plugin="${prefix}/forward_trajectory_plugin.xml"/>
   </export>
 
 </package>