From 569c4dbf293fa4af8275cb8459d1c3981af99917 Mon Sep 17 00:00:00 2001 From: Matthew Powelson Date: Thu, 20 Jul 2023 15:40:03 -0600 Subject: [PATCH] Implement planar joints in CurrentStateMonitor --- tesseract_monitoring/demos/demo_scene.rviz | 158 ++++++++++++++++++ .../current_state_monitor.h | 21 +++ .../launch/demo_planar_joint.launch | 20 +++ .../src/current_state_monitor.cpp | 92 +++++++++- 4 files changed, 284 insertions(+), 7 deletions(-) create mode 100644 tesseract_monitoring/demos/demo_scene.rviz create mode 100644 tesseract_monitoring/launch/demo_planar_joint.launch diff --git a/tesseract_monitoring/demos/demo_scene.rviz b/tesseract_monitoring/demos/demo_scene.rviz new file mode 100644 index 000000000..5d18778f0 --- /dev/null +++ b/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: + 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: + 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 diff --git a/tesseract_monitoring/include/tesseract_monitoring/current_state_monitor.h b/tesseract_monitoring/include/tesseract_monitoring/current_state_monitor.h index 7806c1fa7..896516e93 100644 --- a/tesseract_monitoring/include/tesseract_monitoring/current_state_monitor.h +++ b/tesseract_monitoring/include/tesseract_monitoring/current_state_monitor.h @@ -48,6 +48,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -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 getPlanerJointNames(const std::vector& 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 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_; diff --git a/tesseract_monitoring/launch/demo_planar_joint.launch b/tesseract_monitoring/launch/demo_planar_joint.launch new file mode 100644 index 000000000..71b865f46 --- /dev/null +++ b/tesseract_monitoring/launch/demo_planar_joint.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + diff --git a/tesseract_monitoring/src/current_state_monitor.cpp b/tesseract_monitoring/src/current_state_monitor.cpp index a82f2ea5a..a2bcdb437 100644 --- a/tesseract_monitoring/src/current_state_monitor.cpp +++ b/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::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::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 CurrentStateMonitor::getPlanerJointNames(const std::vector& all_joint_names) const +{ + std::vector ret; + for (const auto& name : all_joint_names) + { + // Planar joints get split into __planar, so only search for x to get + 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::epsilon()) + { + env_state_.joints[joint_name] = joint_value; + update = true; + } + joint_time_[joint_name] = stamp; + } + return update; +} } // namespace tesseract_monitoring