From 577fcdbf986e1e8e4924a1023f996847f2c0b3e5 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Sun, 9 Jul 2017 02:45:58 +0200 Subject: [PATCH] Fixed compiler warnings --- include/ur_modern_driver/ros/action_server.h | 6 +++--- include/ur_modern_driver/ros/controller.h | 2 +- .../ur_modern_driver/ros/hardware_interface.h | 5 +++-- .../ros/trajectory_follower.h | 4 ++-- include/ur_modern_driver/ur/state_parser.h | 20 ++++++++----------- src/ros/action_server.cpp | 8 +++----- src/ros/controller.cpp | 4 ++-- src/ros/hardware_interface.cpp | 10 +++++----- src/ros/trajectory_follower.cpp | 5 ++--- src/ros_main.cpp | 2 +- 10 files changed, 30 insertions(+), 36 deletions(-) diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index f18c6e9..1d62b8f 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -32,9 +32,6 @@ private: std::set joint_set_; double max_velocity_; - RobotState state_; - std::array q_actual_, qd_actual_; - GoalHandle curr_gh_; std::atomic interrupt_traj_; std::atomic has_goal_, running_; @@ -44,6 +41,9 @@ private: TrajectoryFollower& follower_; + RobotState state_; + std::array q_actual_, qd_actual_; + void onGoal(GoalHandle gh); void onCancel(GoalHandle gh); diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h index 3532ee6..b2780d5 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -55,7 +55,7 @@ private: void reset(); public: - ROSController(URCommander& commander, std::vector& joint_names, double max_vel_change); + ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector& joint_names, double max_vel_change); virtual ~ROSController() { } // from RobotHW void doSwitch(const std::list& start_list, const std::list& stop_list); diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h index 325faa4..93bd0c7 100644 --- a/include/ur_modern_driver/ros/hardware_interface.h +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -6,6 +6,7 @@ #include #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ros/trajectory_follower.h" class HardwareInterface { @@ -56,11 +57,11 @@ public: class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface { private: - URCommander &commander_; + TrajectoryFollower& follower_; std::array position_cmd_; public: - PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names); + PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names); virtual bool write(); virtual void start(); virtual void stop(); diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index be59a56..da01004 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -33,12 +33,12 @@ struct TrajectoryPoint class TrajectoryFollower { private: - double servoj_time_, servoj_lookahead_time_, servoj_gain_; std::atomic running_; std::array last_positions_; URCommander &commander_; URServer server_; - int reverse_port_; + + double servoj_time_, servoj_lookahead_time_, servoj_gain_; std::string program_; template diff --git a/include/ur_modern_driver/ur/state_parser.h b/include/ur_modern_driver/ur/state_parser.h index 494558f..fc80242 100644 --- a/include/ur_modern_driver/ur/state_parser.h +++ b/include/ur_modern_driver/ur/state_parser.h @@ -33,17 +33,16 @@ public: bp.parse(packet_size); bp.parse(type); - //quietly ignore the intial version message - if (type == message_type::ROBOT_MESSAGE || ((int)type) == 5) - { - bp.consume(); - return true; - } - if (type != message_type::ROBOT_STATE) { - LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); - return false; + //quietly ignore the intial version message + if(type != message_type::ROBOT_MESSAGE) + { + LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); + } + + bp.consume(); + return true; } while (!bp.empty()) @@ -60,8 +59,6 @@ public: return false; } - //LOG_DEBUG("sub-packet size: %" PRIu32, sub_size); - // deconstruction of a sub parser will increment the position of the parent parser BinParser sbp(bp, sub_size); sbp.consume(sizeof(sub_size)); @@ -73,7 +70,6 @@ public: if (packet == nullptr) { sbp.consume(); - //LOG_DEBUG("Skipping sub-packet of type %d", type); continue; } diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index fc3f7ad..102df31 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -13,10 +13,10 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vectortrajectory.points) { std::array pos, vel; @@ -302,12 +301,11 @@ void ActionServer::trajectoryThread() vel[idx] = point.velocities[i]; } auto t = convert(point.time_from_start); - LOG_DEBUG("P[%d] = %ds, %dns -> %dus", j++, point.time_from_start.sec, point.time_from_start.nsec, t.count()); trajectory.push_back(TrajectoryPoint(pos, vel, t)); } double t = std::chrono::duration_cast>(trajectory[trajectory.size()-1].time_from_start).count(); - LOG_INFO("Executing trajectory with %d points and duration of %4.3fs", trajectory.size(), t); + LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t); Result res; diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp index 79102fa..f8896d7 100644 --- a/src/ros/controller.cpp +++ b/src/ros/controller.cpp @@ -1,10 +1,10 @@ #include "ur_modern_driver/ros/controller.h" -ROSController::ROSController(URCommander &commander, std::vector &joint_names, double max_vel_change) +ROSController::ROSController(URCommander &commander, TrajectoryFollower& follower, std::vector &joint_names, double max_vel_change) : controller_(this, nh_) , joint_interface_(joint_names) , wrench_interface_() - , position_interface_(commander, joint_interface_, joint_names) + , position_interface_(follower, joint_interface_, joint_names) , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) { registerInterface(&joint_interface_); diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index 09a0798..793769b 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -57,8 +57,8 @@ void VelocityInterface::reset() } -PositionInterface:: PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) - : commander_(commander) +PositionInterface:: PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector &joint_names) + : follower_(follower) { for (size_t i = 0; i < 6; i++) { @@ -68,15 +68,15 @@ PositionInterface:: PositionInterface(URCommander &commander, hardware_interfac bool PositionInterface::write() { - + return follower_.execute(position_cmd_); } void PositionInterface::start() { - + follower_.start(); } void PositionInterface::stop() { - + follower_.stop(); } \ No newline at end of file diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp index 0093cba..52b79f2 100644 --- a/src/ros/trajectory_follower.cpp +++ b/src/ros/trajectory_follower.cpp @@ -68,11 +68,10 @@ end TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3) : running_(false) , commander_(commander) - , reverse_port_(reverse_port) + , server_(reverse_port) , servoj_time_(0.008) , servoj_lookahead_time_(0.03) , servoj_gain_(300.) - , server_(reverse_port) { ros::param::get("~servoj_time", servoj_time_); ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_); @@ -89,7 +88,7 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip); - res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_)); + res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port)); program_ = res; if(!server_.bind()) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index dc5255c..f069ac0 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -110,7 +110,7 @@ int main(int argc, char **argv) if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - controller = new ROSController(*rt_commander, args.joint_names, args.max_vel_change); + controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change); rt_vec.push_back(controller); services.push_back(controller); }