From 7e7f0c25444850379d691d7904f35334db91680a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 5 May 2022 10:29:32 +0200 Subject: [PATCH 1/4] fix --- tricycle_controller/src/tricycle_controller.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index b9d9f62f22..919b527c43 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -31,15 +31,13 @@ namespace { - // TODO: Find out how to remap these topics from launch files and keep defaults -constexpr auto DEFAULT_COMMAND_TOPIC = "cmd_vel"; -constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = - "cmd_vel_smoothed"; -constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "cmd_vel_out"; -constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "ackermann"; -constexpr auto DEFAULT_VALIDATED_ACKERMANN_TOPIC = "ackermann"; -constexpr auto DEFAULT_ODOMETRY_TOPIC = "odom"; -constexpr auto DEFAULT_TRANSFORM_TOPIC = "tf"; +constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; +constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; +constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; +constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "~/ackermann"; +constexpr auto DEFAULT_VALIDATED_ACKERMANN_TOPIC = "~/validated_ackermann"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; } // namespace namespace tricycle_controller From 2d1d5e81f0e7340ecce4117cdc1b9c5a18881fb4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 5 May 2022 13:54:02 +0200 Subject: [PATCH 2/4] fixes --- .../tricycle_controller.hpp | 18 ++-- .../src/tricycle_controller.cpp | 93 ++++++------------- 2 files changed, 36 insertions(+), 75 deletions(-) diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 292aca44a9..5df9ef4a89 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -49,7 +49,8 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface class TricycleController : public controller_interface::ControllerInterface { - using Twist = geometry_msgs::msg::TwistStamped; + using Twist = geometry_msgs::msg::Twist; + using TwistStamped = geometry_msgs::msg::TwistStamped; using AckermannDrive = ackermann_msgs::msg::AckermannDrive; public: @@ -104,9 +105,7 @@ class TricycleController : public controller_interface::ControllerInterface CallbackReturn get_steering( const std::string & steering_joint_name, std::vector & joint); double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase); - int sgn(double num); std::tuple process_twist_command(double linear_command, double angular_command); - controller_interface::return_type send_commands(const AckermannDrive::SharedPtr msg); std::string traction_joint_name_; std::string steering_joint_name_; @@ -132,6 +131,7 @@ class TricycleController : public controller_interface::ControllerInterface std::array twist_covariance_diagonal; } odom_params_; + bool publish_ackermann_command_ = false; std::shared_ptr> ackermann_command_publisher_ = nullptr; std::shared_ptr> realtime_ackermann_command_publisher_ = nullptr; @@ -151,14 +151,13 @@ class TricycleController : public controller_interface::ControllerInterface std::chrono::milliseconds cmd_vel_timeout_{500}; bool subscriber_is_active_ = false; - rclcpp::Subscription::SharedPtr validated_command_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; rclcpp::Subscription::SharedPtr velocity_command_unstamped_subscriber_ = nullptr; - realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - std::queue previous_commands_; // last two commands + std::queue previous_commands_; // last two commands std::queue previous_ackermann_command_; // last two steering angles bool is_turning_on_spot_ = false; @@ -167,11 +166,6 @@ class TricycleController : public controller_interface::ControllerInterface SpeedLimiter limiter_linear_; SpeedLimiter limiter_angular_; - bool publish_limited_velocity_ = false; - std::shared_ptr> limited_velocity_publisher_ = nullptr; - std::shared_ptr> realtime_limited_velocity_publisher_ = - nullptr; - rclcpp::Time previous_update_timestamp_{0}; // publish rate limiter diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 919b527c43..12cfe9f889 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -32,10 +32,7 @@ namespace { constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel"; -constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped"; -constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out"; -constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "~/ackermann"; -constexpr auto DEFAULT_VALIDATED_ACKERMANN_TOPIC = "~/validated_ackermann"; +constexpr auto DEFAULT_ACKERMANN_OUT_TOPIC = "~/cmd_ackermann"; constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; } // namespace @@ -71,7 +68,7 @@ CallbackReturn TricycleController::on_init() auto_declare("odom_only_twist", odom_params_.odom_only_twist); auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); - auto_declare("publish_limited_velocity", publish_limited_velocity_); + auto_declare("publish_ackermann_command", publish_ackermann_command_); auto_declare("velocity_rolling_window_size", 10); auto_declare("use_stamped_vel", use_stamped_vel_); @@ -131,7 +128,7 @@ controller_interface::return_type TricycleController::update( ->get_clock() ->now(); // original is current_time = time; time is always sim time when using gazebo_ros2_control. This is a hack to use system time instead. - std::shared_ptr last_command_msg; + std::shared_ptr last_command_msg; received_velocity_msg_ptr_.get(last_command_msg); if (last_command_msg == nullptr) { @@ -149,7 +146,7 @@ controller_interface::return_type TricycleController::update( // command may be limited further by SpeedLimit, // without affecting the stored twist command - Twist command = *last_command_msg; + TwistStamped command = *last_command_msg; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; @@ -221,34 +218,21 @@ controller_interface::return_type TricycleController::update( previous_commands_.pop(); previous_commands_.emplace(command); - // Publish limited velocity - if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) - { - auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; - limited_velocity_command.header.stamp = current_time; - limited_velocity_command.twist = command.twist; - realtime_limited_velocity_publisher_->unlockAndPublish(); - } - // Compute wheel velocity and angle auto [alpha, Ws] = process_twist_command(linear_command, angular_command); - // Publish tricycle command - if (realtime_ackermann_command_publisher_->trylock()) + // Publish ackermann command + if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock()) { auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; - realtime_ackermann_command.speed = Ws; + realtime_ackermann_command.speed = + Ws; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) realtime_ackermann_command.steering_angle = alpha; realtime_ackermann_command_publisher_->unlockAndPublish(); } - return controller_interface::return_type::OK; -} -controller_interface::return_type TricycleController::send_commands( - const AckermannDrive::SharedPtr msg) -{ - traction_joint_[0].velocity_command.get().set_value(msg->speed); - steering_joint_[0].position_command.get().set_value(msg->steering_angle); + traction_joint_[0].velocity_command.get().set_value(Ws); + steering_joint_[0].position_command.get().set_value(alpha); return controller_interface::return_type::OK; } @@ -294,7 +278,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & cmd_vel_timeout_ = std::chrono::milliseconds{ static_cast(get_node()->get_parameter("cmd_vel_timeout").as_double() * 1000.0)}; - publish_limited_velocity_ = get_node()->get_parameter("publish_limited_velocity").as_bool(); + publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); try @@ -338,17 +322,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & return CallbackReturn::ERROR; } - if (publish_limited_velocity_) - { - limited_velocity_publisher_ = - get_node()->create_publisher(DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); - realtime_limited_velocity_publisher_ = - std::make_shared>(limited_velocity_publisher_); - } - - const Twist empty_twist; + const TwistStamped empty_twist; const AckermannDrive empty_ackermann_command; - received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); // Fill last two commands with default constructed commands previous_commands_.emplace(empty_twist); @@ -356,24 +332,22 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & previous_ackermann_command_.emplace(empty_ackermann_command); previous_ackermann_command_.emplace(empty_ackermann_command); - // initialize simple command publisher - ackermann_command_publisher_ = get_node()->create_publisher( - DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); - realtime_ackermann_command_publisher_ = - std::make_shared>( - ackermann_command_publisher_); - - // initialize validated simple command subscriber - validated_command_subscriber_ = get_node()->create_subscription( - DEFAULT_VALIDATED_ACKERMANN_TOPIC, rclcpp::SystemDefaultsQoS(), - std::bind(&TricycleController::send_commands, this, std::placeholders::_1)); + // initialize ackermann command publisher + if (publish_ackermann_command_) + { + ackermann_command_publisher_ = get_node()->create_publisher( + DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_ackermann_command_publisher_ = + std::make_shared>( + ackermann_command_publisher_); + } // initialize command subscriber if (use_stamped_vel_) { - velocity_command_subscriber_ = get_node()->create_subscription( + velocity_command_subscriber_ = get_node()->create_subscription( DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { @@ -395,9 +369,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & else { velocity_command_unstamped_subscriber_ = - get_node()->create_subscription( - DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void { if (!subscriber_is_active_) { @@ -407,7 +381,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; + std::shared_ptr twist_stamped; received_velocity_msg_ptr_.get(twist_stamped); twist_stamped->twist = *msg; twist_stamped->header.stamp = get_node()->get_clock()->now(); @@ -502,7 +476,7 @@ CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) return CallbackReturn::ERROR; } - received_velocity_msg_ptr_.set(std::make_shared()); + received_velocity_msg_ptr_.set(std::make_shared()); return CallbackReturn::SUCCESS; } @@ -520,7 +494,7 @@ bool TricycleController::reset() odometry_.resetOdometry(); // release the old queue - std::queue empty_twist; + std::queue empty_twist; std::swap(previous_commands_, empty_twist); std::queue empty_ackermann_command; std::swap(previous_ackermann_command_, empty_ackermann_command); @@ -639,13 +613,6 @@ double TricycleController::convert_trans_rot_vel_to_steering_angle( return std::atan(theta_dot / (Vx * wheelbase)); } -int TricycleController::sgn(double num) -{ - if (num < 0) return -1; - if (num > 0) return 1; - return 0; -} - std::tuple TricycleController::process_twist_command(double Vx, double theta_dot) { // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf From eb04bdf2dbd0247f6797d24d3429854b035b5670 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 5 May 2022 14:32:16 +0200 Subject: [PATCH 3/4] fixes --- .../tricycle_controller.hpp | 7 ++-- .../src/tricycle_controller.cpp | 36 +++++++++---------- 2 files changed, 18 insertions(+), 25 deletions(-) diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 5df9ef4a89..820f30d6dc 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -124,7 +124,7 @@ class TricycleController : public controller_interface::ControllerInterface { bool open_loop = false; bool enable_odom_tf = false; - bool odom_only_twist = false; // for doing the pose integration in seperate node + bool odom_only_twist = false; // for doing the pose integration in seperate node std::string base_frame_id = "base_link"; std::string odom_frame_id = "odom"; std::array pose_covariance_diagonal; @@ -157,10 +157,7 @@ class TricycleController : public controller_interface::ControllerInterface realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; - std::queue previous_commands_; // last two commands - std::queue previous_ackermann_command_; // last two steering angles - - bool is_turning_on_spot_ = false; + std::queue previous_commands_; // last two commands // speed limiters SpeedLimiter limiter_linear_; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 12cfe9f889..89deaf9347 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -329,8 +329,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & // Fill last two commands with default constructed commands previous_commands_.emplace(empty_twist); previous_commands_.emplace(empty_twist); - previous_ackermann_command_.emplace(empty_ackermann_command); - previous_ackermann_command_.emplace(empty_ackermann_command); // initialize ackermann command publisher if (publish_ackermann_command_) @@ -368,24 +366,23 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } else { - velocity_command_unstamped_subscriber_ = - get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + velocity_command_unstamped_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - - // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; - received_velocity_msg_ptr_.get(twist_stamped); - twist_stamped->twist = *msg; - twist_stamped->header.stamp = get_node()->get_clock()->now(); - }); + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + + // Write fake header in the stored stamped command + std::shared_ptr twist_stamped; + received_velocity_msg_ptr_.get(twist_stamped); + twist_stamped->twist = *msg; + twist_stamped->header.stamp = get_node()->get_clock()->now(); + }); } // initialize odometry publisher and messasge @@ -497,7 +494,6 @@ bool TricycleController::reset() std::queue empty_twist; std::swap(previous_commands_, empty_twist); std::queue empty_ackermann_command; - std::swap(previous_ackermann_command_, empty_ackermann_command); // TODO: clear handles // traction_joint_.clear(); From 79cb82255db80f642074ab7d7b1fb1fd3f832c65 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 5 May 2022 14:33:04 +0200 Subject: [PATCH 4/4] . --- tricycle_controller/src/tricycle_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 89deaf9347..c1fa90a3ca 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -323,7 +323,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } const TwistStamped empty_twist; - const AckermannDrive empty_ackermann_command; received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); // Fill last two commands with default constructed commands @@ -493,7 +492,6 @@ bool TricycleController::reset() // release the old queue std::queue empty_twist; std::swap(previous_commands_, empty_twist); - std::queue empty_ackermann_command; // TODO: clear handles // traction_joint_.clear();