Merge branch 'moveit:jazzy' into jazzy

Author: valmikikothare

moveit_core/robot_state/src/robot_state.cpp

@@ -1217,14 +1217,24 @@ void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bo
   attached_bodies.clear();
   attached_bodies.reserve(attached_body_map_.size());
   for (const auto& it : attached_body_map_)
-    attached_bodies.push_back(it.second.get());
+  {
+    if (it.second)
+    {
+      attached_bodies.push_back(it.second.get());
+    }
+  }
 }
 
 void RobotState::getAttachedBodies(std::map<std::string, const AttachedBody*>& attached_bodies) const
 {
   attached_bodies.clear();
   for (const auto& it : attached_body_map_)
-    attached_bodies[it.first] = it.second.get();
+  {
+    if (it.second && !it.first.empty())
+    {
+      attached_bodies[it.first] = it.second.get();
+    }
+  }
 }
 
 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const

moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp

@@ -0,0 +1,89 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, Shobin Vinod
+ *  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 the copyright holder 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: Shobin Vinod */
+
+#include <pybind11/pybind11.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/serialization.hpp>
+#include <rclcpp/duration.hpp>
+
+namespace py = pybind11;
+
+namespace pybind11
+{
+namespace detail
+{
+template <>
+struct type_caster<rclcpp::Time>
+{
+  PYBIND11_TYPE_CASTER(rclcpp::Time, _("rclcpp::Time"));
+
+  // convert from rclpy::Time to rclcpp::Time
+  bool load(py::handle src, bool)
+  {
+    if (src.is_none())
+      return false;
+
+    // check to validate if the object is a rclcpp::Time object
+    if (!py::hasattr(src, "nanoseconds") || !py::hasattr(src, "clock_type"))
+    {
+      return false;
+    }
+
+    // Extract the value for constructing the rclcpp::Time object
+    int64_t nanoseconds = src.attr("nanoseconds").cast<int64_t>();
+    int clock_type = src.attr("clock_type").cast<int>();
+
+    // Construct the rclcpp::Time object
+    value = rclcpp::Time(nanoseconds, static_cast<rcl_clock_type_t>(clock_type));
+    return true;
+  }
+
+  // convert from rclcpp::Time to rclpy::Time
+  static py::handle cast(const rclcpp::Time& src, return_value_policy /* policy */, py::handle /* parent */)
+  {
+    py::module rclpy_time = py::module::import("rclpy.time");
+    py::object Time = rclpy_time.attr("Time");
+
+    int64_t nanoseconds = src.nanoseconds();
+    int clock_type = static_cast<int>(src.get_clock_type());
+
+    return Time(py::arg("nanoseconds") = nanoseconds,
+                py::arg("clock_type") = clock_type)
+        .release();  // release the ownership of the object
+  }
+};
+}  // namespace detail
+}  // namespace pybind11

moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp

@@ -44,6 +44,7 @@
 #include <moveit_msgs/msg/planning_scene.hpp>
 #include <moveit_msgs/srv/apply_planning_scene.hpp>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.hpp>
+#include <moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp>
 
 namespace py = pybind11;
 

moveit_ros/planning/plan_execution/src/plan_execution.cpp

@@ -283,17 +283,20 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
     collision_detection::CollisionRequest req;
     req.group_name = t.getGroupName();
     req.pad_environment_collisions = false;
-    moveit::core::RobotState state = plan.planning_scene->getCurrentState();
+    moveit::core::RobotState start_state = plan.planning_scene->getCurrentState();
     std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects, waypoint_attached_objects;
-    state.getAttachedBodies(current_attached_objects);
-    waypoint_attached_objects = plan_components_attached_objects_[path_segment.first];
+    start_state.getAttachedBodies(current_attached_objects);
+    if (plan_components_attached_objects_.size() > static_cast<size_t>(path_segment.first))
+      waypoint_attached_objects = plan_components_attached_objects_[path_segment.first];
+    moveit::core::RobotState waypoint_state(start_state);
     for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
     {
       collision_detection::CollisionResult res;
-      state = t.getWayPoint(i);
+      waypoint_attached_objects.clear();  // clear out the last waypoints attached objects
+      waypoint_state = t.getWayPoint(i);
       if (plan_components_attached_objects_[path_segment.first].empty())
       {
-        state.getAttachedBodies(waypoint_attached_objects);
+        waypoint_state.getAttachedBodies(waypoint_attached_objects);
       }
 
       // If sample state has attached objects that are not in the current state, remove them from the sample state
@@ -302,7 +305,7 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
         if (current_attached_objects.find(name) == current_attached_objects.end())
         {
           RCLCPP_DEBUG(logger_, "Attached object '%s' is not in the current scene. Removing it.", name.c_str());
-          state.clearAttachedBody(name);
+          waypoint_state.clearAttachedBody(name);
         }
       }
 
@@ -312,35 +315,35 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
         if (waypoint_attached_objects.find(name) == waypoint_attached_objects.end())
         {
           RCLCPP_DEBUG(logger_, "Attached object '%s' is not in the robot state. Adding it.", name.c_str());
-          state.attachBody(std::make_unique<moveit::core::AttachedBody>(*object));
+          waypoint_state.attachBody(std::make_unique<moveit::core::AttachedBody>(*object));
         }
       }
 
       if (acm)
       {
-        plan.planning_scene->checkCollision(req, res, state, *acm);
+        plan.planning_scene->checkCollision(req, res, waypoint_state, *acm);
       }
       else
       {
-        plan.planning_scene->checkCollision(req, res, state);
+        plan.planning_scene->checkCollision(req, res, waypoint_state);
       }
 
-      if (res.collision || !plan.planning_scene->isStateFeasible(state, false))
+      if (res.collision || !plan.planning_scene->isStateFeasible(waypoint_state, false))
       {
         RCLCPP_INFO(logger_, "Trajectory component '%s' is invalid for waypoint %ld out of %ld",
                     plan.plan_components[path_segment.first].description.c_str(), i, wpc);
 
         // call the same functions again, in verbose mode, to show what issues have been detected
-        plan.planning_scene->isStateFeasible(state, true);
+        plan.planning_scene->isStateFeasible(waypoint_state, true);
         req.verbose = true;
         res.clear();
         if (acm)
         {
-          plan.planning_scene->checkCollision(req, res, state, *acm);
+          plan.planning_scene->checkCollision(req, res, waypoint_state, *acm);
         }
         else
         {
-          plan.planning_scene->checkCollision(req, res, state);
+          plan.planning_scene->checkCollision(req, res, waypoint_state);
         }
         return false;
       }
@@ -469,7 +472,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
   {
     const auto& trajectory = component.trajectory;
     std::map<std::string, const moveit::core::AttachedBody*> trajectory_attached_objects;
-    if (trajectory)
+    if (trajectory && trajectory->getWayPointCount() > 0)
     {
       std::map<std::string, const moveit::core::AttachedBody*> attached_objects;
       trajectory->getWayPoint(0).getAttachedBodies(trajectory_attached_objects);
@@ -483,7 +486,8 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
         }
       }
     }
-    plan_components_attached_objects_.push_back(trajectory_attached_objects);
+    if (!trajectory_attached_objects.empty())
+      plan_components_attached_objects_.push_back(trajectory_attached_objects);
   }
 
   while (rclcpp::ok() && !execution_complete_ && !path_became_invalid_)