Skip to content

Commit

Permalink
Fix for missing end effector markers because clear
Browse files Browse the repository at this point in the history
- Do not change the default color to clear
- Addresses issues in moveit_grasps
- Minor cleanup of EE functions
  • Loading branch information
davetcoleman committed Aug 1, 2019
1 parent 883beb7 commit 737a15f
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 12 deletions.
10 changes: 6 additions & 4 deletions include/moveit_visual_tools/moveit_visual_tools.h
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
* \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
* \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
19 changes: 11 additions & 8 deletions src/moveit_visual_tools.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -373,9 +373,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 +390,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 +516,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();

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

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

0 comments on commit 737a15f

Please sign in to comment.