Ports to ROS2 and fixes problems introduced in merge conflicts.

moveit_core/robot_model/include/moveit/robot_model/robot_model.hpp

@@ -268,11 +268,11 @@ class RobotModel
    * what you went for. Instead, updateStateWithLinkAt(getRigidlyConnectedParentLinkModel(grasp_frame), ...)
    * will actually warp wrist (and all its descendants).
    */
-  static const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link,
-                                                                           Eigen::Isometry3d& transform,
-                                                                           const JointModelGroup* jmg = nullptr);
-  static const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link,
-                                                                           const JointModelGroup* jmg = nullptr)
+  static const LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link,
+                                                             Eigen::Isometry3d& transform,
+                                                             const JointModelGroup* jmg = nullptr);
+  static const LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link,
+                                                             const JointModelGroup* jmg = nullptr)
   {
     Eigen::Isometry3d unused;
     return getRigidlyConnectedParentLinkModel(link, unused, jmg);

moveit_core/robot_model/src/robot_model.cpp

@@ -1380,8 +1380,8 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel*
     end = jmg->getJointModels().cend();
   }
 
-  auto is_fixed_or_not_in_jmg = [begin, end](const moveit::core::JointModel* joint) {
-    if (joint->getType() == moveit::core::JointModel::FIXED)
+  auto is_fixed_or_not_in_jmg = [begin, end](const JointModel* joint) {
+    if (joint->getType() == JointModel::FIXED)
       return true;
     if (begin != end &&                       // we do have a non-empty jmg
         std::find(begin, end, joint) == end)  // joint does not belong to jmg

moveit_core/robot_state/CMakeLists.txt

@@ -21,8 +21,8 @@ if(BUILD_TESTING)
   find_package(benchmark REQUIRED)
   find_package(kdl_parser REQUIRED)
 
-  catkin_add_gtest(test_robot_state test/robot_state_test.cpp)
-  target_link_libraries(test_robot_state ${MOVEIT_LIB_NAME} moveit_test_utils gmock_main)
+  find_package(tf2_geometry_msgs REQUIRED)
+  find_package(resource_retriever REQUIRED)
 
   if(WIN32)
     # TODO add windows paths

moveit_core/robot_state/src/cartesian_interpolator.cpp

@@ -450,10 +450,14 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
     Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion));
     pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation();
 
-    // Explicitly use a single IK attempt only: We want a smooth trajectory.
-    // Random seeding (of additional attempts) would probably create IK jumps.
-    if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options))
-      traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
+    // Explicitly use a single IK attempt only (by setting a timeout of 0.0), using the current state as the seed.
+    // Random seeding (of additional attempts) would create large joint-space jumps.
+    if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options,
+                               cost_function))
+    {
+      start_state->update();
+      path.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
+    }
     else
     {
       break;

moveit_core/robot_state/src/robot_state.cpp

@@ -1870,9 +1870,6 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
   else if (consistency_limit_sets.size() == 1)
     consistency_limits = consistency_limit_sets[0];
 
-  // ensure RobotState is up-to-date before employing it in the IK solver
-  update(false);
-
   const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
 
   // Track which possible tips frames we have filled in so far
@@ -1896,7 +1893,8 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
     if (!setToIKSolverFrame(pose, solver))
       return false;
 
-    // try all of the solver's possible tip frames to see if they match with any of the passed-in pose tip frames
+    // try all of the solver's possible tip frames to see if they uniquely align with any of our passed in pose tip
+    // frames
     bool found_valid_frame = false;
     std::size_t solver_tip_id;  // our current index
     for (solver_tip_id = 0; solver_tip_id < solver_tip_frames.size(); ++solver_tip_id)
@@ -1915,20 +1913,20 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
 
       if (pose_frame != solver_tip_frame)
       {
-        Eigen::Isometry3d pose_parent_to_frame;
-        auto* pose_parent = getRigidlyConnectedParentLinkModel(pose_frame, &pose_parent_to_frame, jmg);
+        auto* pose_parent = getRigidlyConnectedParentLinkModel(pose_frame);
         if (!pose_parent)
         {
-          ROS_ERROR_STREAM_NAMED(LOGNAME, "Pose frame '" << pose_frame << "' does not exist.");
+          RCLCPP_ERROR_STREAM(getLogger(), "The following Pose Frame does not exist: " << pose_frame);
           return false;
         }
-        Eigen::Isometry3d tip_parent_to_tip;
-        auto* tip_parent = getRigidlyConnectedParentLinkModel(solver_tip_frame, &tip_parent_to_tip, jmg);
+        Eigen::Isometry3d pose_parent_to_frame = getFrameTransform(pose_frame);
+        auto* tip_parent = getRigidlyConnectedParentLinkModel(solver_tip_frame);
         if (!tip_parent)
         {
-          ROS_ERROR_STREAM_NAMED(LOGNAME, "Solver tip frame '" << solver_tip_frame << "' does not exist.");
+          RCLCPP_ERROR_STREAM(getLogger(), "The following Solver Tip Frame does not exist: " << solver_tip_frame);
           return false;
         }
+        Eigen::Isometry3d tip_parent_to_tip = getFrameTransform(solver_tip_frame);
         if (pose_parent == tip_parent)
         {
           // transform goal pose as target for solver_tip_frame (instead of pose_frame)
@@ -1941,8 +1939,8 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
       {
         found_valid_frame = true;
         break;
-      }
-    }  // end for solver_tip_frames
+      }  // end if pose_frame
+    }    // end for solver_tip_frames
 
     // Make sure one of the tip frames worked
     if (!found_valid_frame)

moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp

@@ -182,28 +182,11 @@ bool MoveGroupCartesianPathService::computeService(
           {
             jump_threshold = moveit::core::JumpThreshold::relative(req->jump_threshold);
           }
-          // resolve link_name
-          bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req.header.frame_id);
-          const moveit::core::LinkModel* link_model = nullptr;
-          bool found = false;
-          const Eigen::Isometry3d frame_pose = start_state.getFrameInfo(link_name, link_model, found);
-          if (!found)
-          {
-            ROS_ERROR_STREAM_NAMED(getName(), "Unknown frame: " << link_name);
-            res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
-          }
-          ROS_INFO_NAMED(getName(),
-                         "Attempting to follow %u waypoints for link '%s' using a step of %lf m "
-                         "and jump threshold %lf (in %s reference frame)",
-                         (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold,
-                         global_frame ? "global" : "link");
-
           std::vector<moveit::core::RobotStatePtr> traj;
-          res.fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
-              &start_state, jmg, traj, link_model, waypoints, global_frame, moveit::core::MaxEEFStep(req.max_step),
-              moveit::core::JumpThreshold(req.jump_threshold), constraint_fn, kinematics::KinematicsQueryOptions(),
-              start_state.getGlobalLinkTransform(link_model).inverse() * frame_pose);
-          moveit::core::robotStateToRobotStateMsg(start_state, res.start_state);
+          res->fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
+              &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame,
+              moveit::core::MaxEEFStep(req->max_step), moveit::core::CartesianPrecision{}, constraint_fn);
+          moveit::core::robotStateToRobotStateMsg(start_state, res->start_state);
 
           robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req->group_name);
           for (const moveit::core::RobotStatePtr& traj_state : traj)