Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Port robot_state to ROS2 #38

Closed
wants to merge 5 commits into from

Conversation

vmayoral
Copy link
Contributor

Follows from #13

Copy link
Member

@henningkayser henningkayser left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 Outdated Show resolved Hide resolved
moveit_core/robot_state/CMakeLists.txt Outdated Show resolved Hide resolved
@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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

@@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is this removed?

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same

@@ -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))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same

@@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does not belong in this PR

@@ -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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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
Copy link
Contributor

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

@vmayoral
Copy link
Contributor Author

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?

Copy link
Member

@henningkayser henningkayser left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See comment below

@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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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.

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

/** \brief Invert velocity if present. */
void invertVelocity();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same

@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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@@ -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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@vmayoral
Copy link
Contributor Author

Follows at #59.

@vmayoral vmayoral closed this Mar 30, 2019
JafarAbdi pushed a commit to JafarAbdi/moveit2 that referenced this pull request Oct 28, 2019
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants