Draft: Implement planar joints in CurrentStateMonitor

tesseract_monitoring/demos/demo_scene.rviz

@@ -0,0 +1,158 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 408
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: tesseract_rviz/TesseractEnvironment
+      Enabled: true
+      Environment Properties:
+        Display Mode: URDF
+        Joint State Topic: /joint_states
+        Monitor Topic: /tesseract_environment
+        Snapshot Topic: /tesseract_environment_snapshot
+        URDF Parameter: robot_description
+        Value: ""
+      Name: TesseractEnvironment
+      Value: true
+      tesseract::EnvMonitorJointStateTopic: /joint_states
+      tesseract::EnvMonitorMode: URDF
+      tesseract::EnvMonitorSnapshotTopic: /tesseract_environment
+      tesseract::EnvMonitorTopic: /tesseract_environment
+      tesseract::EnvMonitorURDFDescription: robot_description
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        world:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          base_link:
+            {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 6.814720153808594
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1043
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000223000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280054006500730073006500720061006300740045006e007600690072006f006e006d0065006e007400000001c2000001f00000014c00fffffffb000000280054006500730073006500720061006300740045006e007600690072006f006e006d0065006e007401000002660000014c0000014c00ffffff000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  TesseractEnvironment:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 0

tesseract_monitoring/include/tesseract_monitoring/current_state_monitor.h

@@ -48,6 +48,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <unordered_map>
 #include <map>
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_environment/environment.h>
@@ -185,6 +187,22 @@ class CurrentStateMonitor
   void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
   bool isPassiveOrMimicDOF(const std::string& dof) const;
 
+  /**
+   * @brief getPlanerJointNames
+   * @details Assumes the base joint name (in the urdf) does not include the substring _x_planar
+   * @param all_joint_names All joints in the environment
+   * @return Base joint names of the planar joints (the name in the urdf)
+   */
+  std::vector<std::string> getPlanerJointNames(const std::vector<std::string>& all_joint_names) const;
+
+  /**
+   * @brief Update a specified joint in env_state_
+   * @param joint_name Name of the joint to update
+   * @param joint_value Value of the joint to update
+   * @return Whether or not this updated a joint
+   */
+  bool updateJoint(const std::string& joint_name, double joint_value, const ros::Time& stamp);
+
   ros::NodeHandle nh_;
   tesseract_environment::Environment::ConstPtr env_;
   tesseract_scene_graph::SceneState env_state_;
@@ -195,6 +213,9 @@ class CurrentStateMonitor
   ros::Time monitor_start_time_;
   double error_;
   ros::Subscriber joint_state_subscriber_;
+  std::vector<std::string> tf_updated_joints_;  /// Joints updated from TF instead of joint_states
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
   tf2_ros::TransformBroadcaster tf_broadcaster_;
   ros::Time current_state_time_;
   ros::Time last_tf_update_;

tesseract_monitoring/launch/demo_planar_joint.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- Load universal robot description format (URDF) -->
+    <param name="robot_description" textfile="$(find tesseract_support)/urdf/lbr_iiwa_14_r820_with_planar.urdf" />
+
+    <!-- The semantic description that corresponds to the URDF -->
+    <param name="robot_description_semantic" textfile="$(find tesseract_support)/urdf/lbr_iiwa_14_r820.srdf" />
+
+    <!-- Launch visualization -->
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find tesseract_monitoring)/demos/demo_scene.rviz" />
+
+    <!-- STATE PUBLISHER -->
+    <node name="joint_state_publisher_gui"
+          pkg="joint_state_publisher_gui"
+          type="joint_state_publisher_gui" >
+    </node>
+
+    <node pkg="tf" type="static_transform_publisher" name="world_broadcaster" args="0.1 0.2 0.0 0.0 0.0 0.3  world base_link 1"/>
+
+</launch>

tesseract_monitoring/src/current_state_monitor.cpp

@@ -60,6 +60,7 @@ CurrentStateMonitor::CurrentStateMonitor(const tesseract_environment::Environmen
   , state_monitor_started_(false)
   , copy_dynamics_(false)
   , error_(std::numeric_limits<double>::epsilon())
+  , tf_listener_(tf_buffer_)
 {
 }
 
@@ -107,6 +108,9 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi
       ROS_ERROR("The joint states topic cannot be an empty string");
     else
       joint_state_subscriber_ = nh_.subscribe(joint_states_topic, 25, &CurrentStateMonitor::jointStateCallback, this);
+
+    tf_updated_joints_ = getPlanerJointNames(env_->getJointNames());
+
     state_monitor_started_ = true;
     monitor_start_time_ = ros::Time::now();
     ROS_DEBUG("Listening to joint states on topic '%s'", nh_.resolveName(joint_states_topic).c_str());
@@ -330,23 +334,66 @@ void CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstP
     if (last_environment_revision_ != env_->getRevision())
     {
       env_state_ = env_->getState();
+      tf_updated_joints_ = getPlanerJointNames(env_->getJointNames());
       last_environment_revision_ = env_->getRevision();
     }
 
-    for (unsigned i = 0; i < joint_state->name.size(); ++i)
+    // Update joints updated from TF
+    for (const auto& tf_updated_joint : tf_updated_joints_)
     {
-      if (env_state_.joints.find(joint_state->name[i]) != env_state_.joints.end())
+      // Get the TF for the base
+      const auto tf_wait_duration = ros::Duration(1);
+      std::string error_string;
+      const std::string planar_parent = env_->getJoint(tf_updated_joint + "_x_planar")->parent_link_name;
+      const std::string planar_child = env_->getJoint(tf_updated_joint + "_yaw_planar")->child_link_name;
+      geometry_msgs::TransformStamped planar_tf;
+      if (tf_buffer_.canTransform(planar_parent, planar_child, current_state_time_, tf_wait_duration, &error_string))
       {
-        double diff = env_state_.joints[joint_state->name[i]] - joint_state->position[i];
-        if (std::fabs(diff) > std::numeric_limits<double>::epsilon())
+        try
+        {
+          planar_tf = tf_buffer_.lookupTransform(planar_parent, planar_child, current_state_time_, tf_wait_duration);
+        }
+        catch (const std::runtime_error& ex)
         {
-          env_state_.joints[joint_state->name[i]] = joint_state->position[i];
-          update = true;
+          ROS_ERROR_STREAM("TF lookupTransform failed for planar joint: " << ex.what());
+          continue;
         }
-        joint_time_[joint_state->name[i]] = joint_state->header.stamp;
+      }
+      else
+      {
+        ROS_ERROR_STREAM("TF canTransform failed for planar joint: " << error_string);
+        continue;
+      }
+
+      // Set env_state_
+      {
+        const std::string joint_name = tf_updated_joint + "_x_planar";
+        const double joint_position = planar_tf.transform.translation.x;
+        update = update || updateJoint(joint_name, joint_position, joint_state->header.stamp);
+      }
+      {
+        const std::string joint_name = tf_updated_joint + "_y_planar";
+        const double joint_position = planar_tf.transform.translation.y;
+        update = update || updateJoint(joint_name, joint_position, joint_state->header.stamp);
+      }
+      {
+        const std::string joint_name = tf_updated_joint + "_yaw_planar";
+        const Eigen::Isometry3d pose = tf2::transformToEigen(planar_tf.transform);
+        const Eigen::Matrix3d rot_mat = pose.matrix().block<3, 3>(0, 0);
+        const Eigen::Vector3d euler = rot_mat.eulerAngles(2, 1, 0);
+        const double joint_position = euler[2];
+        update = update || updateJoint(joint_name, joint_position, joint_state->header.stamp);
       }
     }
 
+    // Update joints updated from JointStates
+    for (unsigned i = 0; i < joint_state->name.size(); ++i)
+    {
+      const std::string& joint_name = joint_state->name[i];
+      const double& joint_position = joint_state->position[i];
+      update = update || updateJoint(joint_name, joint_position, joint_state->header.stamp);
+    }
+
     if (update)
       env_state_ = env_->getState(env_state_.joints);
 
@@ -378,4 +425,35 @@ void CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstP
   // notify waitForCurrentState() *after* potential update callbacks
   state_update_condition_.notify_all();
 }
+
+std::vector<std::string> CurrentStateMonitor::getPlanerJointNames(const std::vector<std::string>& all_joint_names) const
+{
+  std::vector<std::string> ret;
+  for (const auto& name : all_joint_names)
+  {
+    // Planar joints get split into <base_joint_name>_<x,y,yaw>_planar, so only search for x to get <base_joint_name>
+    const auto pos = name.find("_x_planar");
+    if (pos != std::string::npos)
+    {
+      ret.push_back(name.substr(0, pos));
+    }
+  }
+  return ret;
+}
+
+bool CurrentStateMonitor::updateJoint(const std::string& joint_name, double joint_value, const ros::Time& stamp)
+{
+  bool update = false;
+  if (env_state_.joints.find(joint_name) != env_state_.joints.end())
+  {
+    const double diff = env_state_.joints[joint_name] - joint_value;
+    if (std::fabs(diff) > std::numeric_limits<double>::epsilon())
+    {
+      env_state_.joints[joint_name] = joint_value;
+      update = true;
+    }
+    joint_time_[joint_name] = stamp;
+  }
+  return update;
+}
 }  // namespace tesseract_monitoring