deepcopy option for RobotTrajectory's copy constructor (moveit#1760)

ubi-agni · Dec 19, 2019 · 01ff035 · 01ff035
1 parent e95beda
commit 01ff035
Show file tree

Hide file tree

Showing 5 changed files with 242 additions and 0 deletions.
diff --git a/MIGRATION.md b/MIGRATION.md
@@ -9,6 +9,7 @@ API changes in MoveIt releases
 - Requests to `get_planning_scene` service without explicitly setting "components" now return full scene
 - `moveit_ros_plannning` no longer depends on `moveit_ros_perception`
 - `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. Note that calling `checkRobotCollision` of the `CollisionEnv` does not take a `CollisionRobot` as an argument anymore as it is implicitly contained in the `CollisionEnv`.
+- `RobotTrajectory` provides a copy constructor that allows a shallow (default) and deep copy of waypoints
 
 ## ROS Melodic
 

diff --git a/moveit_core/robot_trajectory/CMakeLists.txt b/moveit_core/robot_trajectory/CMakeLists.txt
@@ -12,3 +12,11 @@ install(TARGETS ${MOVEIT_LIB_NAME}
   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
 
 install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(moveit_resources REQUIRED)
+  include_directories(${moveit_resources_INCLUDE_DIRS})
+
+  catkin_add_gtest(test_robot_trajectory test/test_robot_trajectory.cpp)
+  target_link_libraries(test_robot_trajectory moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
+endif()
diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h
@@ -55,6 +55,15 @@ class RobotTrajectory
 
   RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const robot_model::JointModelGroup* group);
 
+  /** Assignment operator, performing a shallow copy, i.e. copying waypoints by pointer */
+  RobotTrajectory& operator=(const RobotTrajectory&) = default;
+
+  /** @brief  Copy constructor allowing a shallow or deep copy of waypoints
+   *  @param  other - RobotTrajectory to copy from
+   *  @param  deepcopy - copy waypoints by value (true) or by pointer (false)?
+   */
+  RobotTrajectory(const RobotTrajectory& other, bool deepcopy = false);
+
   const robot_model::RobotModelConstPtr& getRobotModel() const
   {
     return robot_model_;

diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp
@@ -53,6 +53,19 @@ RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_mo
 {
 }
 
+RobotTrajectory::RobotTrajectory(const RobotTrajectory& other, bool deepcopy)
+{
+  *this = other;  // default assignment operator performs a shallow copy
+  this->waypoints_.clear();
+  if (deepcopy)
+  {
+    for (const auto& waypoint : other.waypoints_)
+    {
+      this->waypoints_.emplace_back(std::make_shared<moveit::core::RobotState>(*waypoint));
+    }
+  }
+}
+
 void RobotTrajectory::setGroupName(const std::string& group_name)
 {
   group_ = robot_model_->getJointModelGroup(group_name);

diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp
@@ -0,0 +1,211 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013, Willow Garage, Inc.
+*  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 Willow Garage 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: Ioan Sucan */
+
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/utils/robot_model_test_utils.h>
+#include <gtest/gtest.h>
+
+class RobotTrajectoryTestFixture : public testing::Test
+{
+protected:
+  moveit::core::RobotModelConstPtr robot_model_;
+  robot_state::RobotStatePtr robot_state_;
+  const std::string robot_model_name_ = "panda";
+  const std::string arm_jmg_name_ = "panda_arm";
+
+protected:
+  void SetUp() override
+  {
+    robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_);
+    robot_state_ = std::make_shared<robot_state::RobotState>(robot_model_);
+    robot_state_->setToDefaultValues();
+    robot_state_->update();
+  }
+
+  void TearDown() override
+  {
+  }
+
+  void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory)
+  {
+    // Init a traj
+    ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_)) << "Robot model does not have group: "
+                                                                 << arm_jmg_name_;
+
+    trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, arm_jmg_name_);
+
+    EXPECT_EQ(trajectory->getGroupName(), arm_jmg_name_) << "Generated trajectory group name does not match";
+    EXPECT_TRUE(trajectory->empty()) << "Generated trajectory not empty";
+
+    double duration_from_previous = 0.1;
+    std::size_t waypoint_count = 5;
+    for (std::size_t ix = 0; ix < waypoint_count; ++ix)
+      trajectory->addSuffixWayPoint(robot_state_, duration_from_previous);
+    // Quick check that getDuration is working correctly
+    EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count)
+        << "Generated trajectory duration incorrect";
+    EXPECT_EQ(waypoint_count, trajectory->getWayPointDurations().size())
+        << "Generated trajectory has the wrong number of waypoints";
+  }
+
+  void copyTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory,
+                      robot_trajectory::RobotTrajectoryPtr& trajectory_copy, bool deepcopy)
+  {
+    // Copy the trajectory
+    trajectory_copy = std::make_shared<robot_trajectory::RobotTrajectory>(*trajectory, deepcopy);
+    // Quick check that the getDuration values match
+    EXPECT_EQ(trajectory_copy->getDuration(), trajectory->getDuration());
+    EXPECT_EQ(trajectory_copy->getWayPointDurations().size(), trajectory->getWayPointDurations().size());
+  }
+
+  void modifyFirstWaypointPtrAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory)
+  {
+    ///////////////////////////
+    // Get the first waypoint by POINTER, modify it, and check that the value WAS updated in trajectory
+    ///////////////////////////
+    // Get the first waypoint by shared pointer
+    robot_state::RobotStatePtr trajectory_first_waypoint = trajectory->getWayPointPtr(0);
+    // Get the first waypoint joint values
+    std::vector<double> trajectory_first_state;
+    trajectory_first_waypoint->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state);
+
+    // Modify the first waypoint joint values
+    trajectory_first_state[0] += 0.01;
+    trajectory_first_waypoint->setJointGroupPositions(arm_jmg_name_, trajectory_first_state);
+
+    // Check that the trajectory's first waypoint was updated
+    robot_state::RobotStatePtr trajectory_first_waypoint_after_update = trajectory->getWayPointPtr(0);
+    std::vector<double> trajectory_first_state_after_update;
+    trajectory_first_waypoint_after_update->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
+    EXPECT_EQ(trajectory_first_state[0], trajectory_first_state_after_update[0]);
+  }
+
+  void modifyFirstWaypointAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory)
+  {
+    ///////////////////////////
+    // Get the first waypoint by VALUE, modify it, and check that the value WAS NOT updated in trajectory
+    ///////////////////////////
+    // Get the first waypoint by shared pointer
+    robot_state::RobotState trajectory_first_waypoint = trajectory->getWayPoint(0);
+    // Get the first waypoint joint values
+    std::vector<double> trajectory_first_state;
+    trajectory_first_waypoint.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state);
+
+    // Modify the first waypoint joint values
+    trajectory_first_state[0] += 0.01;
+    trajectory_first_waypoint.setJointGroupPositions(arm_jmg_name_, trajectory_first_state);
+
+    // Check that the trajectory's first waypoint was updated
+    robot_state::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0);
+    std::vector<double> trajectory_first_state_after_update;
+    trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
+    EXPECT_NE(trajectory_first_state[0], trajectory_first_state_after_update[0]);
+  }
+};
+
+TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByPtr)
+{
+  robot_trajectory::RobotTrajectoryPtr trajectory;
+  initTestTrajectory(trajectory);
+  modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
+}
+
+TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByValue)
+{
+  robot_trajectory::RobotTrajectoryPtr trajectory;
+  initTestTrajectory(trajectory);
+  modifyFirstWaypointAndCheckTrajectory(trajectory);
+}
+
+TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy)
+{
+  bool deepcopy = false;
+
+  robot_trajectory::RobotTrajectoryPtr trajectory;
+  robot_trajectory::RobotTrajectoryPtr trajectory_copy;
+
+  initTestTrajectory(trajectory);
+  copyTrajectory(trajectory, trajectory_copy, deepcopy);
+  modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
+
+  // Check that modifying the waypoint also modified the trajectory
+  robot_state::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0);
+  std::vector<double> trajectory_first_state_after_update;
+  trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
+
+  // Get the first waypoint in the modified trajectory_copy
+  robot_state::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0);
+  std::vector<double> trajectory_copy_first_state_after_update;
+  trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_,
+                                                                      trajectory_copy_first_state_after_update);
+
+  // Check that we updated the value correctly in the trajectory
+  EXPECT_EQ(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]);
+}
+
+TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDeepCopy)
+{
+  bool deepcopy = true;
+
+  robot_trajectory::RobotTrajectoryPtr trajectory;
+  robot_trajectory::RobotTrajectoryPtr trajectory_copy;
+
+  initTestTrajectory(trajectory);
+  copyTrajectory(trajectory, trajectory_copy, deepcopy);
+  modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
+
+  // Check that modifying the waypoint also modified the trajectory
+  robot_state::RobotState trajectory_first_waypoint_after_update = trajectory->getWayPoint(0);
+  std::vector<double> trajectory_first_state_after_update;
+  trajectory_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
+
+  // Get the first waypoint in the modified trajectory_copy
+  robot_state::RobotState trajectory_copy_first_waypoint_after_update = trajectory_copy->getWayPoint(0);
+  std::vector<double> trajectory_copy_first_state_after_update;
+  trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_,
+                                                                      trajectory_copy_first_state_after_update);
+
+  // Check that we updated the value correctly in the trajectory
+  EXPECT_NE(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}