Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Duplicated code RobotJoint #702

Merged
merged 3 commits into from
Jun 24, 2021
Merged

Duplicated code RobotJoint #702

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a2625ec
Duplicated code RobotJoint
ahcorde Jun 8, 2021
3b0c8a6
update
ahcorde Jun 8, 2021
89ff83e
initialize variables
ahcorde Jun 24, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 9 additions & 1 deletion rviz_default_plugins/include/rviz_default_plugins/robot/robot_joint.hpp
View file
Open in desktop
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
103 changes: 57 additions & 46 deletions rviz_default_plugins/src/rviz_default_plugins/robot/robot_joint.cpp
View file
Open in desktop
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,24 +211,50 @@ 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,
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) {
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 +265,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 +277,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;
ahcorde marked this conversation as resolved.
Show resolved Hide resolved
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