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.

Sign up for GitHub

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

Fix for missing end effector markers because clear #51

Merged
merged 2 commits into from
Aug 14, 2019
Merged

Fix for missing end effector markers because clear #51

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
25bd32b
Fix for missing end effector markers because clear
davetcoleman Aug 1, 2019
aa998af
Apply suggestions from code review
davetcoleman Aug 13, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 6 additions & 4 deletions include/moveit_visual_tools/moveit_visual_tools.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -222,26 +222,26 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
*/
bool publishEEMarkers(const Eigen::Isometry3d& pose, const robot_model::JointModelGroup* ee_jmg,
const std::vector<double>& ee_joint_pos,
const rviz_visual_tools::colors& color = rviz_visual_tools::CLEAR,
const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT,
const std::string& ns = "end_effector")
{
return publishEEMarkers(convertPose(pose), ee_jmg, ee_joint_pos, color, ns);
}
bool publishEEMarkers(const Eigen::Isometry3d& pose, const robot_model::JointModelGroup* ee_jmg,
const rviz_visual_tools::colors& color = rviz_visual_tools::CLEAR,
const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT,
const std::string& ns = "end_effector")
{
return publishEEMarkers(convertPose(pose), ee_jmg, {}, color, ns);
}
bool publishEEMarkers(const geometry_msgs::Pose& pose, const robot_model::JointModelGroup* ee_jmg,
const rviz_visual_tools::colors& color = rviz_visual_tools::CLEAR,
const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT,
const std::string& ns = "end_effector")
{
return publishEEMarkers(pose, ee_jmg, {}, color, ns);
}
bool publishEEMarkers(const geometry_msgs::Pose& pose, const robot_model::JointModelGroup* ee_jmg,
const std::vector<double>& ee_joint_pos,
const rviz_visual_tools::colors& color = rviz_visual_tools::CLEAR,
const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT,
const std::string& ns = "end_effector");

/**
Expand All @@ -255,6 +255,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools

/**
* \brief Display an animated vector of grasps including its approach movement in Rviz
* Note this function calls trigger() automatically in order to achieve animations
davetcoleman marked this conversation as resolved.
Show resolved Hide resolved
* \param possible_grasps - a set of grasp positions to visualize
* \param ee_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm"
* \param animate_speed - how fast the gripper approach is animated, optional
Expand All @@ -264,6 +265,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools

/**
* \brief Animate a single grasp in its movement direction
* Note this function calls trigger() automatically in order to achieve animations
davetcoleman marked this conversation as resolved.
Show resolved Hide resolved
* \param grasp
* \param ee_jmg - the set of joints to use, e.g. the MoveIt planning group, e.g. "left_arm"
* \param animate_speed - how fast the gripper approach is animated
Expand Down
30 changes: 19 additions & 11 deletions src/moveit_visual_tools.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -275,7 +275,6 @@ bool MoveItVisualTools::loadEEMarker(const robot_model::JointModelGroup* ee_jmg,
// Keep track of how many unique markers we have between different EEs
static std::size_t marker_id_offset = 0;

// -----------------------------------------------------------------------------------------------
// Get end effector group

// Create color to use for EE markers
Expand All @@ -284,13 +283,19 @@ bool MoveItVisualTools::loadEEMarker(const robot_model::JointModelGroup* ee_jmg,
// Get link names that are in end effector
const std::vector<std::string>& ee_link_names = ee_jmg->getLinkModelNames();

// -----------------------------------------------------------------------------------------------
// Get EE link markers for Rviz

shared_robot_state_->getRobotMarkers(ee_markers_map_[ee_jmg], ee_link_names, marker_color, ee_jmg->getName(),
ros::Duration());
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Number of rviz markers in end effector: " << ee_markers_map_[ee_jmg].markers.size());

// Error check
if (ee_markers_map_[ee_jmg].markers.empty())
{
ROS_ERROR_STREAM_NAMED(LOGNAME,
"No links found to visualize in end effector for joint model group: " << ee_jmg->getName());
return false;
}

const std::string& ee_parent_link_name = ee_jmg->getEndEffectorParentGroup().second;
// ROS_DEBUG_STREAM_NAMED(LOGNAME,"EE Parent link: " << ee_parent_link_name);
const moveit::core::LinkModel* ee_parent_link = robot_model_->getLinkModel(ee_parent_link_name);
Expand Down Expand Up @@ -373,9 +378,6 @@ bool MoveItVisualTools::publishEEMarkers(const geometry_msgs::Pose& pose, const
Eigen::Isometry3d eigen_goal_ee_pose = convertPose(pose);
Eigen::Isometry3d eigen_this_marker;

// publishArrow( pose, rviz_visual_tools::RED, rviz_visual_tools::LARGE );

// -----------------------------------------------------------------------------------------------
// Process each link of the end effector
for (std::size_t i = 0; i < ee_markers_map_[ee_jmg].markers.size(); ++i)
{
Expand All @@ -393,15 +395,23 @@ bool MoveItVisualTools::publishEEMarkers(const geometry_msgs::Pose& pose, const
ee_markers_map_[ee_jmg].markers[i].lifetime = marker_lifetime_;

// Color
ee_markers_map_[ee_jmg].markers[i].color = getColor(color);
if (color != rviz_visual_tools::DEFAULT)
ee_markers_map_[ee_jmg].markers[i].color = getColor(color);

// Convert pose
eigen_this_marker = eigen_goal_ee_pose * ee_poses_map_[ee_jmg][i];
ee_markers_map_[ee_jmg].markers[i].pose = convertPose(eigen_this_marker);
}

// Helper for publishing rviz markers
return publishMarkers(ee_markers_map_[ee_jmg]);
// Does not require trigger() because publishing array auto-triggers
if (!publishMarkers(ee_markers_map_[ee_jmg]))
{
ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to publish EE markers");
return false;
}

return true;
}

bool MoveItVisualTools::publishGrasps(const std::vector<moveit_msgs::Grasp>& possible_grasps,
Expand Down Expand Up @@ -511,10 +521,8 @@ bool MoveItVisualTools::publishAnimatedGrasp(const moveit_msgs::Grasp& grasp,
// Convert eigen pre-grasp position back to regular message
pre_grasp_pose = tf2::toMsg(pre_grasp_pose_eigen);

// publishArrow(pre_grasp_pose, moveit_visual_tools::BLUE);
publishEEMarkers(pre_grasp_pose, ee_jmg);
if (batch_publishing_enabled_)
trigger();

Copy link
Contributor

Choose a reason for hiding this comment

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

it took me a while to get the difference between publishMarker which would need a trigger in this case and publishMarkers which does not follow the batch_publishing_enabled_ and therefore does not need a separate trigger/publish ever

Copy link
Member Author

Choose a reason for hiding this comment

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

yea, its counter intuitive for sure. i'll make a TODO, but i can't address this now...

Copy link
Member Author

Choose a reason for hiding this comment

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

Actually i just went ahead and fixed it: PickNikRobotics/rviz_visual_tools#123

ros::Duration(animate_speed).sleep();

// Pause more at initial pose for debugging purposes
Expand Down