Skip to content

Commit 856375b

Browse files
v4hnConstructor Robotics
authored andcommitted
include continuous (revolute) joints in efforts display
Discovered while exploring visualizations for the Kinova Gen3 arms, where every second joint is continuous. The display only added arrow indicators for the non-continuous joints.
1 parent b0dae05 commit 856375b

File tree

1 file changed

+2
-2
lines changed

1 file changed

+2
-2
lines changed

src/rviz/default_plugin/effort_display.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -232,7 +232,7 @@ void EffortDisplay::load()
232232
it != robot_model_->joints_.end(); it++)
233233
{
234234
urdf::JointSharedPtr joint = it->second;
235-
if (joint->type == urdf::Joint::REVOLUTE)
235+
if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::CONTINUOUS)
236236
{
237237
std::string joint_name = it->first;
238238
urdf::JointLimitsSharedPtr limit = joint->limits;
@@ -295,7 +295,7 @@ void EffortDisplay::processMessage(const sensor_msgs::JointState::ConstPtr& msg)
295295

296296
const urdf::Joint* joint = robot_model_->getJoint(joint_name).get();
297297
int joint_type = joint->type;
298-
if (joint_type == urdf::Joint::REVOLUTE)
298+
if (joint_type == urdf::Joint::REVOLUTE || joint_type == urdf::Joint::CONTINUOUS)
299299
{
300300
std::string tf_frame_id = concat(tf_prefix_property_->getStdString(), joint->child_link_name);
301301
Ogre::Quaternion orientation;

0 commit comments

Comments
 (0)