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

fix problems reported by CLion linter #146

Open
wants to merge 1 commit into
base: noetic-devel
Choose a base branch
from
Open

fix problems reported by CLion linter #146

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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: 5 additions & 5 deletions include/robot_state_publisher/joint_state_listener.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,15 +65,15 @@ class JointStateListener {
*/
JointStateListener(const KDL::Tree& tree, const MimicMap& m, const urdf::Model& model = urdf::Model());

JointStateListener(const std::shared_ptr<RobotStatePublisher>& rsp, const MimicMap& m);
JointStateListener(std::shared_ptr<RobotStatePublisher> rsp, MimicMap m);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I agree with the change to the rsp shared_ptr, but not with the change to the MimicMap. That will cause a copy of the map to be made, which isn't what we want.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Take a look at the mimic_ member variable. We initialize using move semantics now.



/// Destructor
~JointStateListener();

protected:
virtual void callbackJointState(const JointStateConstPtr& state);
virtual void callbackFixedJoint(const ros::TimerEvent& e);
void callbackJointState(const JointStateConstPtr& state);
void callbackFixedJoint(const ros::TimerEvent& e);
Comment on lines +75 to +76
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe that these are meant to be virtual, so that derivative classes can properly override them.


ros::Duration publish_interval_;
std::shared_ptr<RobotStatePublisher> state_publisher_;
Expand All @@ -82,8 +82,8 @@ class JointStateListener {
ros::Time last_callback_time_;
std::map<std::string, ros::Time> last_publish_time_;
MimicMap mimic_;
bool use_tf_static_;
bool ignore_timestamp_;
bool use_tf_static_{};
bool ignore_timestamp_{};

};
}
Expand Down
15 changes: 8 additions & 7 deletions include/robot_state_publisher/robot_state_publisher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,14 +47,15 @@
#include <kdl/frames.hpp>
#include <kdl/segment.hpp>
#include <kdl/tree.hpp>
#include <utility>

namespace robot_state_publisher {

class SegmentPair
{
public:
SegmentPair(const KDL::Segment& p_segment, const std::string& p_root, const std::string& p_tip):
segment(p_segment), root(p_root), tip(p_tip){}
SegmentPair(const KDL::Segment& p_segment, std::string p_root, std::string p_tip):
segment(p_segment), root(std::move(p_root)), tip(std::move(p_tip)){}

KDL::Segment segment;
std::string root, tip;
Expand All @@ -71,20 +72,20 @@ class RobotStatePublisher
/** Constructor
* \param tree The kinematic model of a robot, represented by a KDL Tree
*/
RobotStatePublisher(const KDL::Tree& tree, const urdf::Model& model = urdf::Model());
explicit RobotStatePublisher(const KDL::Tree& tree, urdf::Model model = urdf::Model());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think changing the constref to an object is what we want; that will cause the copy constructor to be called.


/// Destructor
~RobotStatePublisher(){};
~RobotStatePublisher() = default;;

/** Publish transforms to tf
* \param joint_positions A map of joint names and joint positions.
* \param time The time at which the joint positions were recorded
*/
virtual void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);
virtual void publishFixedTransforms(bool use_tf_static = false);
void publishFixedTransforms(bool use_tf_static = false);
void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);

protected:
virtual void addChildren(const KDL::SegmentMap::const_iterator segment);
void addChildren(KDL::SegmentMap::const_iterator segment);
Comment on lines +84 to +88
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think all of these are meant to be virtual so this can be subclassed.


std::map<std::string, SegmentPair> segments_, segments_fixed_;
urdf::Model model_;
Expand Down
32 changes: 16 additions & 16 deletions src/joint_state_listener.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <urdf/model.h>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <utility>

#include "robot_state_publisher/robot_state_publisher.h"
#include "robot_state_publisher/joint_state_listener.h"
Expand All @@ -57,8 +58,8 @@ JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m,
{
}

JointStateListener::JointStateListener(const std::shared_ptr<RobotStatePublisher>& rsp, const MimicMap& m)
: state_publisher_(rsp), mimic_(m)
JointStateListener::JointStateListener(std::shared_ptr<RobotStatePublisher> rsp, MimicMap m)
: state_publisher_(std::move(rsp)), mimic_(std::move(m))
{
ros::NodeHandle n_tilde("~");
ros::NodeHandle n;
Expand Down Expand Up @@ -87,8 +88,7 @@ JointStateListener::JointStateListener(const std::shared_ptr<RobotStatePublisher
}


JointStateListener::~JointStateListener()
{}
JointStateListener::~JointStateListener() = default;


void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e)
Expand All @@ -114,22 +114,22 @@ void JointStateListener::callbackJointState(const JointStateConstPtr& state)
}

// check if we moved backwards in time (e.g. when playing a bag file)
ros::Time now = ros::Time::now();
if (last_callback_time_ > now) {
ros::Time time_now = ros::Time::now();
if (last_callback_time_ > time_now) {
// force re-publish of joint transforms
ROS_WARN("Moved backwards in time (probably because ROS clock was reset), re-publishing joint transforms!");
last_publish_time_.clear();
}
ros::Duration warning_threshold(30.0);
if ((state->header.stamp + warning_threshold) < now) {
ROS_WARN_THROTTLE(10, "Received JointState is %f seconds old.", (now - state->header.stamp).toSec());
if ((state->header.stamp + warning_threshold) < time_now) {
ROS_WARN_THROTTLE(10, "Received JointState is %f seconds old.", (time_now - state->header.stamp).toSec());
}
last_callback_time_ = now;
last_callback_time_ = time_now;

// determine least recently published joint
ros::Time last_published = now;
for (size_t i = 0; i < state->name.size(); ++i) {
ros::Time t = last_publish_time_[state->name[i]];
ros::Time last_published = time_now;
for (const auto & i : state->name) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Let's not use auto; it's hard to tell what the type is supposed to be then. I guess this would be const std::string & name then.

ros::Time t = last_publish_time_[i];
last_published = (t < last_published) ? t : last_published;
}
// note: if a joint was seen for the first time,
Expand All @@ -143,10 +143,10 @@ void JointStateListener::callbackJointState(const JointStateConstPtr& state)
joint_positions.insert(make_pair(state->name[i], state->position[i]));
}

for (MimicMap::iterator i = mimic_.begin(); i != mimic_.end(); i++) {
if(joint_positions.find(i->second->joint_name) != joint_positions.end()) {
double pos = joint_positions[i->second->joint_name] * i->second->multiplier + i->second->offset;
joint_positions.insert(make_pair(i->first, pos));
for (auto & i : mimic_) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We can change this to use the new loop style, but again, let's not make it auto. Also, we should have a more descriptive name for the variable than i. Finally, can the loop iteration variable be const here?

if(joint_positions.find(i.second->joint_name) != joint_positions.end()) {
double pos = joint_positions[i.second->joint_name] * i.second->multiplier + i.second->offset;
joint_positions.insert(make_pair(i.first, pos));
}
}

Expand Down
33 changes: 16 additions & 17 deletions src/robot_state_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@
#include <map>
#include <string>

#include <kdl/frames_io.hpp>
#include <utility>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_kdl/tf2_kdl.h>

Expand All @@ -49,8 +49,8 @@ RobotStatePublisher::RobotStatePublisher() : RobotStatePublisher(KDL::Tree())
{
}

RobotStatePublisher::RobotStatePublisher(const KDL::Tree& tree, const urdf::Model& model)
: model_(model)
RobotStatePublisher::RobotStatePublisher(const KDL::Tree& tree, urdf::Model model)
: model_(std::move(model))
{
// walk the tree and add segments to segments_
addChildren(tree.getRootSegment());
Expand All @@ -62,9 +62,9 @@ void RobotStatePublisher::addChildren(const KDL::SegmentMap::const_iterator segm
const std::string& root = GetTreeElementSegment(segment->second).getName();

const std::vector<KDL::SegmentMap::const_iterator>& children = GetTreeElementChildren(segment->second);
for (size_t i = 0; i < children.size(); ++i) {
const KDL::Segment& child = GetTreeElementSegment(children[i]->second);
SegmentPair s(GetTreeElementSegment(children[i]->second), root, child.getName());
for (auto i : children) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Again, no auto, can this be constref, and can we give this a better name?

const KDL::Segment& child = GetTreeElementSegment(i->second);
SegmentPair s(GetTreeElementSegment(i->second), root, child.getName());
if (child.getJoint().getType() == KDL::Joint::None) {
if (model_.getJoint(child.getJoint().getName()) && model_.getJoint(child.getJoint().getName())->type == urdf::Joint::FLOATING) {
ROS_INFO("Floating joint. Not adding segment from %s to %s. This TF can not be published based on joint_states info", root.c_str(), child.getName().c_str());
Expand All @@ -78,13 +78,13 @@ void RobotStatePublisher::addChildren(const KDL::SegmentMap::const_iterator segm
segments_.insert(make_pair(child.getJoint().getName(), s));
ROS_DEBUG("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
}
addChildren(children[i]);
addChildren(i);
}
}

std::string stripSlash(const std::string & in)
{
if (in.size() && in[0] == '/')
if (!in.empty() && in[0] == '/')
{
return in.substr(1);
}
Expand All @@ -98,17 +98,17 @@ void RobotStatePublisher::publishTransforms(const std::map<std::string, double>&
std::vector<geometry_msgs::TransformStamped> tf_transforms;

// loop over all joints
for (std::map<std::string, double>::const_iterator jnt = joint_positions.begin(); jnt != joint_positions.end(); jnt++) {
std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jnt->first);
for (auto & jnt : joint_positions) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No auto, and can this be constref?

std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jnt.first);
if (seg != segments_.end()) {
geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(jnt->second));
geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(jnt.second));
tf_transform.header.stamp = time;
tf_transform.header.frame_id = stripSlash(seg->second.root);
tf_transform.child_frame_id = stripSlash(seg->second.tip);
tf_transforms.push_back(tf_transform);
}
else {
ROS_WARN_THROTTLE(10, "Joint state with name: \"%s\" was received but not found in URDF", jnt->first.c_str());
ROS_WARN_THROTTLE(10, "Joint state with name: \"%s\" was received but not found in URDF", jnt.first.c_str());
}
}
tf_broadcaster_.sendTransform(tf_transforms);
Expand All @@ -119,17 +119,16 @@ void RobotStatePublisher::publishFixedTransforms(bool use_tf_static)
{
ROS_DEBUG("Publishing transforms for fixed joints");
std::vector<geometry_msgs::TransformStamped> tf_transforms;
geometry_msgs::TransformStamped tf_transform;

// loop over all fixed segments
for (std::map<std::string, SegmentPair>::const_iterator seg = segments_fixed_.begin(); seg != segments_fixed_.end(); seg++) {
geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(0));
for (const auto & seg : segments_fixed_) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No auto.

geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg.second.segment.pose(0));
tf_transform.header.stamp = ros::Time::now();
if (!use_tf_static) {
tf_transform.header.stamp += ros::Duration(0.5);
}
tf_transform.header.frame_id = stripSlash(seg->second.root);
tf_transform.child_frame_id = stripSlash(seg->second.tip);
tf_transform.header.frame_id = stripSlash(seg.second.root);
tf_transform.child_frame_id = stripSlash(seg.second.tip);
tf_transforms.push_back(tf_transform);
}
if (use_tf_static) {
Expand Down
6 changes: 3 additions & 3 deletions src/robot_state_publisher_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,9 +66,9 @@ int main(int argc, char** argv)

robot_state_publisher::MimicMap mimic;

for (std::map< std::string, urdf::JointSharedPtr >::iterator i = model.joints_.begin(); i != model.joints_.end(); i++) {
if (i->second->mimic) {
mimic.insert(std::make_pair(i->first, i->second->mimic));
for (auto & joint : model.joints_) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No auto, and can this be constref?

if (joint.second->mimic) {
mimic.insert(std::make_pair(joint.first, joint.second->mimic));
}
}

Expand Down