From 30a47726ecc650c40eb04e0bcf8fa88164bb8bf1 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Thu, 9 Nov 2023 08:03:17 -0500
Subject: [PATCH] Handle missing effort limit in URDF (#1084) (#1085)

* Handle missing effort limit in URDF

Signed-off-by: Patrick Roncagliolo <ronca.pat@gmail.com>
Co-authored-by: Chris Lalancette <clalancette@gmail.com>
(cherry picked from commit a83d8195a16cdcdbd417938cb8d3a30b4c826b12)

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>
---
 .../displays/effort/effort_display.cpp              | 13 +++++++++++--
 1 file changed, 11 insertions(+), 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
index fc948ddfa..bf0b91f78 100644
--- 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());
+      }
     }
   }
 }