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