exposing the publishers to the user (#48)

Author: mlautman

include/moveit_visual_tools/moveit_visual_tools.h

@@ -52,6 +52,7 @@
 #include <moveit_msgs/Grasp.h>
 #include <moveit_msgs/DisplayRobotState.h>
 #include <moveit_msgs/WorkspaceParameters.h>
+#include <moveit_msgs/DisplayTrajectory.h>
 
 // ROS Messages
 #include <trajectory_msgs/JointTrajectory.h>
@@ -554,6 +555,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
                              const moveit::core::RobotState& robot_state, bool blocking = false);
   bool publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg,
                              const moveit_msgs::RobotState& robot_state, bool blocking = false);
+  void publishTrajectoryPath(const moveit_msgs::DisplayTrajectory& display_trajectory_msg);
 
   /**
    * \brief Display a line of the end effector path from a robot trajectory path
@@ -633,14 +635,16 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
    *        To use, add a RobotState marker to Rviz and subscribe to the DISPLAY_ROBOT_STATE_TOPIC, above
    * \param robot_state - joint values of robot
    * \param color - how to highlight the robot (solid-ly) if desired, default keeps color as specified in URDF
-   * \param highlight_links - if the |color| is not |DEFAULT|, allows selective robot links to be highlighted. by default (empty) all links are highlighted 
+   * \param highlight_links - if the |color| is not |DEFAULT|, allows selective robot links to be highlighted. by
+   * default (empty) all links are highlighted
    */
   bool publishRobotState(const moveit::core::RobotState& robot_state,
                          const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT,
                          const std::vector<std::string>& highlight_links = {});
   bool publishRobotState(const moveit::core::RobotStatePtr& robot_state,
                          const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT,
                          const std::vector<std::string>& highlight_links = {});
+  void publishRobotState(const moveit_msgs::DisplayRobotState& display_robot_state_msg);
 
   /**
    * \brief Fake removing a Robot State display in Rviz by simply moving it very far away

src/moveit_visual_tools.cpp

@@ -38,7 +38,6 @@
 #include <moveit_visual_tools/moveit_visual_tools.h>
 
 // MoveIt Messages
-#include <moveit_msgs/DisplayTrajectory.h>
 #include <moveit_msgs/CollisionObject.h>
 
 // MoveIt
@@ -1236,10 +1235,7 @@ bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory
   display_trajectory_msg.trajectory[0] = trajectory_msg;
   display_trajectory_msg.trajectory_start = robot_state;
 
-  // Publish message
-  loadTrajectoryPub();  // always call this before publishing
-  pub_display_path_.publish(display_trajectory_msg);
-  ros::spinOnce();
+  publishTrajectoryPath(display_trajectory_msg);
 
   // Wait the duration of the trajectory
   if (blocking)
@@ -1266,6 +1262,14 @@ bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory
   return true;
 }
 
+void MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::DisplayTrajectory& display_trajectory_msg)
+{
+  // Publish message
+  loadTrajectoryPub();  // always call this before publishing
+  pub_display_path_.publish(display_trajectory_msg);
+  ros::spinOnce();
+}
+
 bool MoveItVisualTools::publishTrajectoryLine(const moveit_msgs::RobotTrajectory& trajectory_msg,
                                               const moveit::core::LinkModel* ee_parent_link,
                                               const robot_model::JointModelGroup* arm_jmg,
@@ -1439,7 +1443,8 @@ bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_s
                                           const rviz_visual_tools::colors& color,
                                           const std::vector<std::string>& highlight_links)
 {
-  // when only a subset of links should be colored, the default message is used rather than the cached solid robot messages
+  // when only a subset of links should be colored, the default message is used rather than the cached solid robot
+  // messages
   rviz_visual_tools::colors base_color = color;
   if (!highlight_links.empty())
     base_color = rviz_visual_tools::DEFAULT;
@@ -1491,9 +1496,7 @@ bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_s
   }
 
   // Publish
-  loadRobotStatePub();
-  pub_robot_state_.publish(display_robot_msg);
-  ros::spinOnce();
+  publishRobotState(display_robot_msg);
 
   // remove highlight links from default message
   if (!highlight_links.empty())
@@ -1502,6 +1505,13 @@ bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_s
   return true;
 }
 
+void MoveItVisualTools::publishRobotState(const moveit_msgs::DisplayRobotState& robot_state_msg)
+{
+  loadRobotStatePub();
+  pub_robot_state_.publish(robot_state_msg);
+  ros::spinOnce();
+}
+
 bool MoveItVisualTools::hideRobot()
 {
   static const std::string VJOINT_NAME = "virtual_joint";