Handle missing effort limit in URDF (#1084)

* Handle missing effort limit in URDF Signed-off-by: Patrick Roncagliolo <[email protected]> Co-authored-by: Chris Lalancette <[email protected]>
ros2 · Nov 8, 2023 · a83d819 · a83d819
1 parent 6245af4
commit a83d819
Showing 1 changed file with 11 additions and 2 deletions.
diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/effort/effort_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/effort/effort_display.cpp
@@ -47,6 +47,7 @@
 #include <rviz_common/properties/property.hpp>
 #include <rviz_rendering/objects/effort_visual.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <rclcpp/logging.hpp>
 
 using namespace std::chrono_literals;
 
@@ -196,8 +197,16 @@ void EffortDisplay::topic_callback(const std_msgs::msg::String & msg)
     if (joint->type == urdf::Joint::REVOLUTE || joint->type == 2) {
       std::string joint_name = it->first;
       urdf::JointLimitsSharedPtr limit = joint->limits;
-      joints_[joint_name] = std::make_shared<JointInfo>(joint_name, joints_category_);
-      joints_[joint_name]->setMaxEffort(limit->effort);
+      if (limit) {
+        joints_[joint_name] = std::make_shared<JointInfo>(joint_name, joints_category_);
+        joints_[joint_name]->setMaxEffort(limit->effort);
+      } else {
+        RCLCPP_WARN(
+          context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(),
+          "Joint '%s' has no <limit> tag in URDF. Effort plugin needs to know the effort "
+          "limit to determine the size of the corresponding visual marker. "
+          "Effort display for this joint will be inhibited.", joint_name.c_str());
+      }
     }
   }
 }