Skip to content

Commit

Permalink
Merge pull request #722 from ros2/ahcorde/forwardport/702
Browse files Browse the repository at this point in the history 
[Forwardport ros2] Duplicated code RobotJoint (#702)
  • Loading branch information
@ahcorde
ahcorde authored Jun 30, 2021
2 parents fa6e126 + 5007fb5 commit 013b8d1
Show file tree
Hide file tree
Showing 2 changed files with 66 additions and 45 deletions.
10 changes: 9 additions & 1 deletion rviz_default_plugins/include/rviz_default_plugins/robot/robot_joint.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@
#include "rviz_common/interaction/forwards.hpp"

#include "rviz_default_plugins/robot/robot_element_base_class.hpp"
#include "rviz_default_plugins/robot/robot_link.hpp"
#include "rviz_default_plugins/visibility_control.hpp"

namespace Ogre
Expand Down Expand Up @@ -141,7 +142,7 @@ private Q_SLOTS:
int & links_with_geom, // returns # of children with geometry
int & links_with_geom_checked, // returns # of enabled children with geometry
int & links_with_geom_unchecked, // returns # of disabled children with geometry
bool recursive) const; // True: all descendant links.
bool recursive); // True: all descendant links.
// False: just single child link.

// set the value of the enable checkbox without touching child joints/links
Expand All @@ -163,6 +164,13 @@ private Q_SLOTS:
rviz_common::properties::FloatProperty * upper_limit_property_;

private:
RobotLink * links_checked_and_unchecked(
int & links_with_geom_checked,
int & links_with_geom_unchecked);
int links_with_geom(
RobotLink * link, int & links_with_geom_checked,
int & links_with_geom_unchecked, int n_args, ...);

Ogre::Vector3 joint_origin_pos_;
Ogre::Quaternion joint_origin_rot_;
bool has_decendent_links_with_geometry_;
Expand Down
101 changes: 57 additions & 44 deletions rviz_default_plugins/src/rviz_default_plugins/robot/robot_joint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@
#include <string>
#include <vector>

#include "rviz_default_plugins/robot/robot_link.hpp"
#include "rviz_default_plugins/robot/robot.hpp"

#include "rviz_common/load_resource.hpp"
Expand Down Expand Up @@ -212,6 +211,37 @@ void RobotJoint::setJointCheckbox(QVariant val)
doing_set_checkbox_ = false;
}

int RobotJoint::links_with_geom(
RobotLink * link, int & links_with_geom_checked,
int & links_with_geom_unchecked, int n_args, ...)
{
va_list arg_ptr;
va_start(arg_ptr, n_args);

for (auto & child_joint_name : link->getChildJointNames()) {
RobotJoint * child_joint = robot_->getJoint(child_joint_name);
if (child_joint) {
int child_links_with_geom;
int child_links_with_geom_checked;
int child_links_with_geom_unchecked;
if (n_args == 1) {
child_joint->getChildLinkState(
child_links_with_geom, child_links_with_geom_checked,
child_links_with_geom_unchecked, va_arg(arg_ptr, int));
} else {
child_joint->calculateJointCheckboxesRecursive(
child_links_with_geom,
child_links_with_geom_checked,
child_links_with_geom_unchecked);
}
links_with_geom_checked += child_links_with_geom_checked;
links_with_geom_unchecked += child_links_with_geom_unchecked;
}
}
va_end(arg_ptr);
return links_with_geom_checked + links_with_geom_unchecked;
}

void RobotJoint::calculateJointCheckboxesRecursive(
int & links_with_geom,
int & links_with_geom_checked,
Expand All @@ -221,15 +251,12 @@ void RobotJoint::calculateJointCheckboxesRecursive(
links_with_geom_checked = 0;
links_with_geom_unchecked = 0;

RobotLink * link = robot_->getLink(child_link_name_);
if (link == nullptr) {
RobotLink * link =
links_checked_and_unchecked(links_with_geom_checked, links_with_geom_unchecked);
if (!link) {
return;
}
if (link->hasGeometry()) {
bool checked = link->getLinkProperty()->getValue().toBool();
links_with_geom_checked += checked ? 1 : 0;
links_with_geom_unchecked += checked ? 0 : 1;
}

links_with_geom = links_with_geom_checked + links_with_geom_unchecked;

if (!styleIsTree()) {
Expand All @@ -240,21 +267,8 @@ void RobotJoint::calculateJointCheckboxesRecursive(
}
}

for (auto & child_joint_name : link->getChildJointNames()) {
RobotJoint * child_joint = robot_->getJoint(child_joint_name);
if (child_joint) {
int child_links_with_geom;
int child_links_with_geom_checked;
int child_links_with_geom_unchecked;
child_joint->calculateJointCheckboxesRecursive(
child_links_with_geom,
child_links_with_geom_checked,
child_links_with_geom_unchecked);
links_with_geom_checked += child_links_with_geom_checked;
links_with_geom_unchecked += child_links_with_geom_unchecked;
}
}
links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
links_with_geom = this->links_with_geom(
link, links_with_geom_checked, links_with_geom_unchecked, 0);

if (styleIsTree()) {
if (!links_with_geom) {
Expand All @@ -265,40 +279,39 @@ void RobotJoint::calculateJointCheckboxesRecursive(
}
}

void RobotJoint::getChildLinkState(
int & links_with_geom,
int & links_with_geom_checked,
int & links_with_geom_unchecked,
bool recursive) const
RobotLink * RobotJoint::links_checked_and_unchecked(
int & links_with_geom_checked, int & links_with_geom_unchecked)
{
links_with_geom = 0;
links_with_geom_checked = 0;
links_with_geom_unchecked = 0;

RobotLink * link = robot_->getLink(child_link_name_);
if (link == nullptr) {
return;
if (!link) {
return nullptr;
}
if (link->hasGeometry()) {
bool checked = link->getLinkProperty()->getValue().toBool();
links_with_geom_checked += checked ? 1 : 0;
links_with_geom_unchecked += checked ? 0 : 1;
}
return link;
}

void RobotJoint::getChildLinkState(
int & links_with_geom,
int & links_with_geom_checked,
int & links_with_geom_unchecked,
bool recursive)
{
links_with_geom = 0;
RobotLink * link = this->links_checked_and_unchecked(
links_with_geom_checked, links_with_geom_unchecked);
if (!link) {
return;
}

if (recursive) {
for (auto & child_joint_name : link->getChildJointNames()) {
RobotJoint * child_joint = robot_->getJoint(child_joint_name);
if (child_joint) {
int child_links_with_geom;
int child_links_with_geom_checked;
int child_links_with_geom_unchecked;
child_joint->getChildLinkState(
child_links_with_geom, child_links_with_geom_checked,
child_links_with_geom_unchecked, recursive);
links_with_geom_checked += child_links_with_geom_checked;
links_with_geom_unchecked += child_links_with_geom_unchecked;
}
}
this->links_with_geom(link, links_with_geom_checked, links_with_geom_unchecked, 1, recursive);
}

links_with_geom = links_with_geom_checked + links_with_geom_unchecked;
Expand Down

0 comments on commit 013b8d1

Please sign in to comment.