From 1dab0dbaea1d632bebd92d1a0726f3af189ed2c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alexander=20R=C3=B6ssler?= Date: Wed, 2 Sep 2020 16:53:36 +0200 Subject: [PATCH] fix problems reported by linter --- .../joint_state_listener.h | 10 +++--- .../robot_state_publisher.h | 13 ++++---- src/joint_state_listener.cpp | 32 +++++++++--------- src/robot_state_publisher.cpp | 33 +++++++++---------- src/robot_state_publisher_node.cpp | 6 ++-- 5 files changed, 47 insertions(+), 47 deletions(-) diff --git a/include/robot_state_publisher/joint_state_listener.h b/include/robot_state_publisher/joint_state_listener.h index 16dac0c..e4d95e5 100644 --- a/include/robot_state_publisher/joint_state_listener.h +++ b/include/robot_state_publisher/joint_state_listener.h @@ -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& rsp, const MimicMap& m); + JointStateListener(std::shared_ptr rsp, MimicMap m); /// 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); ros::Duration publish_interval_; std::shared_ptr state_publisher_; @@ -82,8 +82,8 @@ class JointStateListener { ros::Time last_callback_time_; std::map last_publish_time_; MimicMap mimic_; - bool use_tf_static_; - bool ignore_timestamp_; + bool use_tf_static_{}; + bool ignore_timestamp_{}; }; } diff --git a/include/robot_state_publisher/robot_state_publisher.h b/include/robot_state_publisher/robot_state_publisher.h index ca363d2..9dd0d5d 100644 --- a/include/robot_state_publisher/robot_state_publisher.h +++ b/include/robot_state_publisher/robot_state_publisher.h @@ -47,14 +47,15 @@ #include #include #include +#include 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; @@ -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()); /// 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& joint_positions, const ros::Time& time); virtual void publishFixedTransforms(bool use_tf_static = false); + void publishTransforms(const std::map& joint_positions, const ros::Time& time); protected: - virtual void addChildren(const KDL::SegmentMap::const_iterator segment); + void addChildren(KDL::SegmentMap::const_iterator segment); std::map segments_, segments_fixed_; urdf::Model model_; diff --git a/src/joint_state_listener.cpp b/src/joint_state_listener.cpp index 0c6f3d0..5acd38e 100644 --- a/src/joint_state_listener.cpp +++ b/src/joint_state_listener.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "robot_state_publisher/robot_state_publisher.h" #include "robot_state_publisher/joint_state_listener.h" @@ -57,8 +58,8 @@ JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m, { } -JointStateListener::JointStateListener(const std::shared_ptr& rsp, const MimicMap& m) - : state_publisher_(rsp), mimic_(m) +JointStateListener::JointStateListener(std::shared_ptr rsp, MimicMap m) + : state_publisher_(std::move(rsp)), mimic_(std::move(m)) { ros::NodeHandle n_tilde("~"); ros::NodeHandle n; @@ -87,8 +88,7 @@ JointStateListener::JointStateListener(const std::shared_ptr 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) { + 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, @@ -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_) { + 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)); } } diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp index fbedb90..0424b1a 100644 --- a/src/robot_state_publisher.cpp +++ b/src/robot_state_publisher.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include #include @@ -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()); @@ -62,9 +62,9 @@ void RobotStatePublisher::addChildren(const KDL::SegmentMap::const_iterator segm const std::string& root = GetTreeElementSegment(segment->second).getName(); const std::vector& 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) { + 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()); @@ -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); } @@ -98,17 +98,17 @@ void RobotStatePublisher::publishTransforms(const std::map& std::vector tf_transforms; // loop over all joints - for (std::map::const_iterator jnt = joint_positions.begin(); jnt != joint_positions.end(); jnt++) { - std::map::const_iterator seg = segments_.find(jnt->first); + for (auto & jnt : joint_positions) { + std::map::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); @@ -119,17 +119,16 @@ void RobotStatePublisher::publishFixedTransforms(bool use_tf_static) { ROS_DEBUG("Publishing transforms for fixed joints"); std::vector tf_transforms; - geometry_msgs::TransformStamped tf_transform; // loop over all fixed segments - for (std::map::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_) { + 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) { diff --git a/src/robot_state_publisher_node.cpp b/src/robot_state_publisher_node.cpp index 85f80a5..438acc6 100644 --- a/src/robot_state_publisher_node.cpp +++ b/src/robot_state_publisher_node.cpp @@ -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_) { + if (joint.second->mimic) { + mimic.insert(std::make_pair(joint.first, joint.second->mimic)); } }