Port robot_state to ROS2 #38

vmayoral · 2019-03-10T16:58:37Z

Follows from #13

moveit_core/robot_state/include/moveit/robot_state/log.h

moveit_core/robot_state/src/conversions.cpp

@anasarrak

henningkayser

There seems to be some mismatch between this branch and the current master sync. Also some conflicts are not fully resolved.

moveit_core/robot_state/include/moveit/robot_state/log.h

moveit_core/robot_state/CMakeLists.txt

henningkayser · 2019-03-11T21:33:45Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

      @param timeout The timeout passed to the kinematics solver on each attempt
      @param constraint A state validity constraint to be required for IK solutions */
-  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0,
-                 const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
+  bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, unsigned int attempts = 0,


All these changes in setFromIK() functions are invalid because the attempts parameter is not supported anymore.
It seems like these are not properly resolved merge conflicts.

@henningkayser can you help addressing this? what's your suggestion? is this something that will get fixed once we sync with moveit1 master? If so, shall we skip your comment for now?

@vmayoral I think the feature branch was created by merging an out-of-sync branch with your commit history into master. The resulting conflicts where not resolved completely so your commits include additional and in this case outdated code. This particular conflict comes from eebfe9c (moveit/moveit#1288) not being part of the feature base branch yet. I would suggest creating a new branch that is based on master and then using cherry-pick to apply the clean history of your changes (not from this merged feature branch). Then force push to the feature branch.
This problem won't be fixed by syncing master again (only if conflicts are resolved by hand since the history is broken). Also I'm strongly against skipping this since this PR includes a bunch of these conflicts.

Thanks @henningkayser, that makes sense. I recall @rhaschke pushing some stuff accidentally so it may be from that time since this PR is from a while ago.

I've created a new PR at #59. Can you please have a look there?

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

henningkayser · 2019-03-11T21:34:43Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -1565,17 +1511,6 @@ as the new values that correspond to the group */
      updateMimicJoint(joint);
    }
  }
-
-  /// Call harmonizePosition() for all joints / all joints in group / given joint


Why is this removed?

henningkayser · 2019-03-11T21:49:30Z

moveit_core/robot_state/src/robot_state.cpp

-    elapsed = (ros::WallTime::now() - start).toSec();
-    first_seed = false;
-  } while (elapsed < timeout);
+  }


henningkayser · 2019-03-11T21:49:47Z

moveit_core/robot_state/src/robot_state.cpp

@@ -1974,7 +1992,7 @@ double RobotState::computeCartesianPath(const JointModelGroup* group, std::vecto

    // Explicitly use a single IK attempt only: We want a smooth trajectory.
    // Random seeding (of additional attempts) would probably create IK jumps.
-    if (setFromIK(group, pose, link->getName(), consistency_limits, 0.0, validCallback, options))
+    if (setFromIK(group, pose, link->getName(), consistency_limits, 1, 0.0, validCallback, options))


henningkayser · 2019-03-11T21:50:23Z

moveit_core/robot_state/src/robot_state.cpp

@@ -2174,6 +2192,58 @@ void RobotState::printStatePositions(std::ostream& out) const
    out << nm[i] << "=" << position_[i] << std::endl;
 }

+void RobotState::printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out) const


Does not belong in this PR

henningkayser · 2019-03-11T21:51:16Z

moveit_core/robot_state/test/robot_state_test.cpp

@@ -579,6 +579,7 @@ std::size_t generateTestTraj(std::vector<std::shared_ptr<robot_state::RobotState
 TEST_F(OneRobot, testGenerateTrajectory)
 {
  const robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e");
+  ASSERT_TRUE(joint_model_group);


There shouldn't be any changes in this file, no?

henningkayser · 2019-03-11T22:02:06Z

moveit_core/robot_state/test/test_aabb.cpp

-  msg.type = visualization_msgs::Marker::CUBE;
-  msg.color.a = 0.5;
-  msg.lifetime.sec = 3000;
+  auto msg = std::make_shared<visualization_msgs::Marker>();


I'm not sure if it's always safe to reuse this shared pointer for multiple publish calls. Since we don't rely on performance in this test we might just pass the message by reference.

mlautman · 2019-03-12T19:11:45Z

@anasarrak (Since the original PR was done by them)

vmayoral · 2019-03-16T23:12:15Z

I've included @davetcoleman's approach from #21 here. From @henningkayser's comments, it sounds like some sync is required to move forward with this PR. @henningkayser, you seem to be knowledgeable about what should be in or out in this submodule, can you help addressing your comments above?

henningkayser

See comment below

henningkayser · 2019-03-18T18:48:20Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

      @param timeout The timeout passed to the kinematics solver on each attempt
      @param constraint A state validity constraint to be required for IK solutions */
-  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0,
-                 const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
+  bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, unsigned int attempts = 0,


@vmayoral I think the feature branch was created by merging an out-of-sync branch with your commit history into master. The resulting conflicts where not resolved completely so your commits include additional and in this case outdated code. This particular conflict comes from eebfe9c (moveit/moveit#1288) not being part of the feature base branch yet. I would suggest creating a new branch that is based on master and then using cherry-pick to apply the clean history of your changes (not from this merged feature branch). Then force push to the feature branch.
This problem won't be fixed by syncing master again (only if conflicts are resolved by hand since the history is broken). Also I'm strongly against skipping this since this PR includes a bunch of these conflicts.

henningkayser · 2019-03-18T18:49:35Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -516,9 +516,6 @@ class RobotState
    return effort_[index];
  }

-  /** \brief Invert velocity if present. */
-  void invertVelocity();


Reverts 8121e46 from moveit/moveit#1335

henningkayser · 2019-03-18T18:50:07Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

-  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
-                 double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
+  bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, const std::string& tip,
+                 unsigned int attempts = 0, double timeout = 0.0,


Reverts eebfe9c from moveit/moveit#1288

henningkayser · 2019-03-18T18:50:24Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

      @param timeout The timeout passed to the kinematics solver on each attempt */
-  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
-                 const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
+  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int attempts = 0,


henningkayser · 2019-03-18T18:50:33Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

      @param timeout The timeout passed to the kinematics solver on each attempt
      @param constraint A state validity constraint to be required for IK solutions */
  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
-                 double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
+                 unsigned int attempts = 0, double timeout = 0.0,


henningkayser · 2019-03-18T18:52:27Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
-  [[deprecated("The attempts argument is not supported anymore.")]] bool


I think removing deprecated functions is fine. Did we decide to do this?

henningkayser · 2019-03-18T18:54:35Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -1792,6 +1727,10 @@ as the new values that correspond to the group */

  void printStatePositions(std::ostream& out = std::cout) const;

+  /** \brief Output to console the current state of the robot's joint limits */
+  void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg,


moveit/moveit#1358

vmayoral · 2019-03-30T10:42:01Z

Follows at #59.

Port Robot model loader

vmayoral commented Mar 10, 2019

View reviewed changes

moveit_core/robot_state/include/moveit/robot_state/log.h Outdated Show resolved Hide resolved

vmayoral commented Mar 10, 2019

View reviewed changes

moveit_core/robot_state/src/conversions.cpp Outdated Show resolved Hide resolved

Port robot_state to ROS2

870578f

Authored by @anasarrak

vmayoral force-pushed the robot-state branch from 09a3361 to 870578f Compare March 10, 2019 17:03

henningkayser requested changes Mar 11, 2019

View reviewed changes

vmayoral added 3 commits March 16, 2019 23:54

robot_state, adapt to ROS2

efbfdde

robot_state, remove log.h

b2f0f46

Restore broken comment

c377881

robot_state, apply clang format

622eae6

henningkayser requested changes Mar 18, 2019

View reviewed changes

vmayoral mentioned this pull request Mar 30, 2019

Port robot_state to ROS 2 #59

Closed

vmayoral closed this Mar 30, 2019

JafarAbdi pushed a commit to JafarAbdi/moveit2 that referenced this pull request Oct 28, 2019

Merge pull request moveit#38 from AcutronicRobotics/robot_model_loader

09571cb

Port Robot model loader

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Port robot_state to ROS2 #38

Port robot_state to ROS2 #38

vmayoral commented Mar 10, 2019

henningkayser left a comment

henningkayser Mar 11, 2019

vmayoral Mar 16, 2019

henningkayser Mar 18, 2019

vmayoral Mar 30, 2019

henningkayser Mar 11, 2019

henningkayser Mar 11, 2019

henningkayser Mar 11, 2019

henningkayser Mar 11, 2019

henningkayser Mar 11, 2019

henningkayser Mar 11, 2019

mlautman commented Mar 12, 2019

vmayoral commented Mar 16, 2019

henningkayser left a comment

henningkayser Mar 18, 2019

henningkayser Mar 18, 2019

henningkayser Mar 18, 2019

henningkayser Mar 18, 2019

henningkayser Mar 18, 2019

henningkayser Mar 18, 2019

henningkayser Mar 18, 2019

vmayoral commented Mar 30, 2019

		const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
		[[deprecated("The attempts argument is not supported anymore.")]] bool

Port robot_state to ROS2 #38

Port robot_state to ROS2 #38

Conversation

vmayoral commented Mar 10, 2019

henningkayser left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mlautman commented Mar 12, 2019

vmayoral commented Mar 16, 2019

henningkayser left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

vmayoral commented Mar 30, 2019