diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 3be176e572..0318177be3 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -19,6 +19,7 @@ joint_state_broadcaster joint_trajectory_controller position_controllers + tricycle_controller velocity_controllers diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt new file mode 100644 index 0000000000..e0410a6b28 --- /dev/null +++ b/tricycle_controller/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 3.8) +project(tricycle_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(ackermann_msgs REQUIRED) + +add_library( + ${PROJECT_NAME} + SHARED + src/tricycle_controller.cpp + src/odometry.cpp + src/speed_limiter.cpp +) +target_include_directories( + ${PROJECT_NAME} + PRIVATE + include +) + + +ament_target_dependencies( + ${PROJECT_NAME} + ackermann_msgs + builtin_interfaces + controller_interface + geometry_msgs + hardware_interface + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs +) + +pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml) + +install(TARGETS ${PROJECT_NAME} + +RUNTIME DESTINATION bin +ARCHIVE DESTINATION lib +LIBRARY DESTINATION lib +) +install( + DIRECTORY + + include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_load_tricycle_controller + test/test_load_tricycle_controller.cpp + ) + target_include_directories(test_load_tricycle_controller PRIVATE include) + ament_target_dependencies(test_load_tricycle_controller + controller_manager + ros2_control_test_assets + ) + +endif() + + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + controller_interface + geometry_msgs + hardware_interface + rclcpp + rclcpp_lifecycle +) + +ament_package() diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst new file mode 100644 index 0000000000..70a968d964 --- /dev/null +++ b/tricycle_controller/doc/userdoc.rst @@ -0,0 +1,26 @@ +.. _tricycle_controller_userdoc: + +tricycle_controller +===================== + +Controller for mobile robots with tricycle drive. +Input for control are robot body velocity commands which are translated to wheel commands for the tricycle drive base. +Odometry is computed from hardware feedback and published. + +Velocity commands +----------------- + +The controller works with a velocity twist from which it extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored. + +Hardware interface type +----------------------- + +The controller works with wheel joints through a velocity interface. + +Other features +-------------- + + Realtime-safe implementation. + Odometry publishing + Task-space velocity, acceleration and jerk limits + Automatic stop after command time-out \ No newline at end of file diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp new file mode 100644 index 0000000000..be2dda63a8 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -0,0 +1,80 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + * Author: Sara Al Arab + */ + +#ifndef TRICYCLE_CONTROLLER__ODOMETRY_HPP_ +#define TRICYCLE_CONTROLLER__ODOMETRY_HPP_ + +#include + +#include "rclcpp/time.hpp" +#include "rcppmath/rolling_mean_accumulator.hpp" + +namespace tricycle_controller +{ +class Odometry +{ +public: + explicit Odometry(size_t velocity_rolling_window_size = 10); + + void init(const rclcpp::Time & time); + bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time); + void updateOpenLoop(double linear, double angular, const rclcpp::Time & time); + void resetOdometry(); + + double getX() const { return x_; } + double getY() const { return y_; } + double getHeading() const { return heading_; } + double getLinear() const { return linear_; } + double getAngular() const { return angular_; } + + void setWheelParams(double wheel_separation, double wheel_radius); + void setVelocityRollingWindowSize(size_t velocity_rolling_window_size); + +private: + using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; + + void integrateRungeKutta2(double linear, double angular); + void integrateExact(double linear, double angular); + void resetAccumulators(); + + // Current timestamp: + rclcpp::Time timestamp_; + + // Current pose: + double x_; // [m] + double y_; // [m] + double heading_; // [rad] + + // Current velocity: + double linear_; // [m/s] + double angular_; // [rad/s] + + // Wheel kinematic parameters [m]: + double wheelbase_; + double wheel_radius_; + + // Rolling mean accumulators for the linear and angular velocities: + size_t velocity_rolling_window_size_; + RollingMeanAccumulator linear_accumulator_; + RollingMeanAccumulator angular_accumulator_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__ODOMETRY_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/speed_limiter.hpp b/tricycle_controller/include/tricycle_controller/speed_limiter.hpp new file mode 100644 index 0000000000..f779dbecfa --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/speed_limiter.hpp @@ -0,0 +1,105 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Enrique Fernández + */ + +#ifndef TRICYCLE_CONTROLLER__SPEED_LIMITER_HPP_ +#define TRICYCLE_CONTROLLER__SPEED_LIMITER_HPP_ + +#include + +namespace tricycle_controller +{ +class SpeedLimiter +{ +public: + /** + * \brief Constructor + * \param [in] has_velocity_limits if true, applies velocity limits + * \param [in] has_acceleration_limits if true, applies acceleration limits + * \param [in] has_jerk_limits if true, applies jerk limits + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + SpeedLimiter( + bool has_velocity_limits = false, bool has_acceleration_limits = false, + bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, + double max_jerk = NAN); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & v, double v0, double v1, double dt); + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & v, double v0, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double & v, double v0, double v1, double dt); + +private: + // Enable/Disable velocity/acceleration/jerk limits: + bool has_velocity_limits_; + bool has_acceleration_limits_; + bool has_jerk_limits_; + + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + + // Jerk limits: + double min_jerk_; + double max_jerk_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__SPEED_LIMITER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp new file mode 100644 index 0000000000..292aca44a9 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -0,0 +1,189 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + * Author: Sara Al Arab + */ + +#ifndef TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ +#define TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "ackermann_msgs/msg/ackermann_drive.hpp" +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "hardware_interface/handle.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_box.h" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tricycle_controller/odometry.hpp" +#include "tricycle_controller/speed_limiter.hpp" +#include "tricycle_controller/visibility_control.h" + +namespace tricycle_controller +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class TricycleController : public controller_interface::ControllerInterface +{ + using Twist = geometry_msgs::msg::TwistStamped; + using AckermannDrive = ackermann_msgs::msg::AckermannDrive; + +public: + TRICYCLE_CONTROLLER_PUBLIC + TricycleController(); + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + TRICYCLE_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_init() override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state) override; + + TRICYCLE_CONTROLLER_PUBLIC + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override; + +protected: + struct TractionHandle + { + std::reference_wrapper velocity_state; + std::reference_wrapper velocity_command; + }; + struct SteeringHandle + { + std::reference_wrapper position_state; + std::reference_wrapper position_command; + }; + + CallbackReturn get_traction( + const std::string & traction_joint_name, std::vector & joint); + 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_; + + // HACK: put into vector to avoid initializing structs because they have no default constructors + std::vector traction_joint_; + std::vector steering_joint_; + + struct WheelParams + { + double wheelbase = 0.0; // w.r.t. the midpoint of the wheel width + double radius = 0.0; // Assumed to be the same for both wheels + } wheel_params_; + + struct OdometryParams + { + bool open_loop = false; + bool enable_odom_tf = false; + 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; + std::array twist_covariance_diagonal; + } odom_params_; + + std::shared_ptr> ackermann_command_publisher_ = nullptr; + std::shared_ptr> + realtime_ackermann_command_publisher_ = nullptr; + + Odometry odometry_; + + std::shared_ptr> odometry_publisher_ = nullptr; + std::shared_ptr> + realtime_odometry_publisher_ = nullptr; + + std::shared_ptr> odometry_transform_publisher_ = + nullptr; + std::shared_ptr> + realtime_odometry_transform_publisher_ = nullptr; + + // Timeout to consider cmd_vel commands old + 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_unstamped_subscriber_ = nullptr; + + 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; + + // speed limiters + 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 + double publish_rate_ = 50.0; + rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Time previous_publish_timestamp_{0}; + + bool is_halted = false; + bool use_stamped_vel_ = true; + + bool reset(); + void halt(); +}; +} // namespace tricycle_controller +#endif // TRICYCLE_CONTROLLER__TRICYCLE_CONTROLLER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/visibility_control.h b/tricycle_controller/include/tricycle_controller/visibility_control.h new file mode 100644 index 0000000000..434bb38ca2 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/visibility_control.h @@ -0,0 +1,50 @@ +// Copyright (c) 2021, Bence Magyar and Denis Stogl +// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TRICYCLE_CONTROLLER_EXPORT __attribute__((dllexport)) +#define TRICYCLE_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define TRICYCLE_CONTROLLER_EXPORT __declspec(dllexport) +#define TRICYCLE_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef TRICYCLE_CONTROLLER_BUILDING_DLL +#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_EXPORT +#else +#define TRICYCLE_CONTROLLER_PUBLIC TRICYCLE_CONTROLLER_IMPORT +#endif +#define TRICYCLE_CONTROLLER_PUBLIC_TYPE TRICYCLE_CONTROLLER_PUBLIC +#define TRICYCLE_CONTROLLER_LOCAL +#else +#define TRICYCLE_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define TRICYCLE_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define TRICYCLE_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define TRICYCLE_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define TRICYCLE_CONTROLLER_PUBLIC +#define TRICYCLE_CONTROLLER_LOCAL +#endif +#define TRICYCLE_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // TRICYCLE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml new file mode 100644 index 0000000000..bd58840171 --- /dev/null +++ b/tricycle_controller/package.xml @@ -0,0 +1,33 @@ + + + + tricycle_controller + 1.0.0 + Controller for a tricycle drive mobile base + root + Apache License 2.0 + + ament_cmake + + controller_interface + geometry_msgs + hardware_interface + nav_msgs + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs + ackermann_msgs + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + diff --git a/tricycle_controller/src/odometry.cpp b/tricycle_controller/src/odometry.cpp new file mode 100644 index 0000000000..6b7936f4e5 --- /dev/null +++ b/tricycle_controller/src/odometry.cpp @@ -0,0 +1,133 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + * Author: Sara Al Arab + */ + +#include "tricycle_controller/odometry.hpp" + +namespace tricycle_controller +{ +Odometry::Odometry(size_t velocity_rolling_window_size) +: timestamp_(0.0), + x_(0.0), + y_(0.0), + heading_(0.0), + linear_(0.0), + angular_(0.0), + wheelbase_(0.0), + wheel_radius_(0.0), + velocity_rolling_window_size_(velocity_rolling_window_size), + linear_accumulator_(velocity_rolling_window_size), + angular_accumulator_(velocity_rolling_window_size) +{ +} + +void Odometry::init(const rclcpp::Time & time) +{ + // Reset accumulators and timestamp: + resetAccumulators(); + timestamp_ = time; +} + +bool Odometry::updateFromVelocity(double Ws, double alpha, const rclcpp::Time & time) +{ + // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf + double Vs = Ws * wheel_radius_; + double Vx = Vs * std::cos(alpha); + double theta_dot = Vs * std::sin(alpha) / wheelbase_; + + // Integrate odometry: + integrateExact(Vx, theta_dot); + + timestamp_ = time; + + // Estimate speeds using a rolling mean to filter them out: + linear_accumulator_.accumulate(Vx); + angular_accumulator_.accumulate(theta_dot); + + linear_ = linear_accumulator_.getRollingMean(); + angular_ = angular_accumulator_.getRollingMean(); + + return true; +} + +void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time & time) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + const double dt = time.seconds() - timestamp_.seconds(); + timestamp_ = time; + integrateExact(linear * dt, angular * dt); +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; +} + +void Odometry::setWheelParams(double wheelbase, double wheel_radius) +{ + wheelbase_ = wheelbase; + wheel_radius_ = wheel_radius; +} + +void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) +{ + velocity_rolling_window_size_ = velocity_rolling_window_size; + + resetAccumulators(); +} + +void Odometry::integrateRungeKutta2(double linear, double angular) +{ + const double direction = heading_ + angular * 0.5; + + /// Runge-Kutta 2nd order integration: + x_ += linear * cos(direction); + y_ += linear * sin(direction); + heading_ += angular; +} + +void Odometry::integrateExact(double linear, double angular) +{ + if (fabs(angular) < 1e-6) + { + integrateRungeKutta2(linear, angular); + } + else + { + /// Exact integration (should solve problems when angular is zero): + const double heading_old = heading_; + const double r = linear / angular; + heading_ += angular; + x_ += r * (sin(heading_) - sin(heading_old)); + y_ += -r * (cos(heading_) - cos(heading_old)); + } +} + +void Odometry::resetAccumulators() +{ + linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); + angular_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_); +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/speed_limiter.cpp b/tricycle_controller/src/speed_limiter.cpp new file mode 100644 index 0000000000..bae6b9170b --- /dev/null +++ b/tricycle_controller/src/speed_limiter.cpp @@ -0,0 +1,140 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Enrique Fernández + */ + +#include +#include + +#include "tricycle_controller/speed_limiter.hpp" +#include "rcppmath/clamp.hpp" + +namespace tricycle_controller +{ +SpeedLimiter::SpeedLimiter( + bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity, + double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, + double max_jerk) +: has_velocity_limits_(has_velocity_limits), + has_acceleration_limits_(has_acceleration_limits), + has_jerk_limits_(has_jerk_limits), + min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration), + min_jerk_(min_jerk), + max_jerk_(max_jerk) +{ + // Check if limits are valid, max must be specified, min defaults to -max if unspecified + if (has_velocity_limits_) + { + if (std::isnan(max_velocity_)) + { + throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified"); + } + if (std::isnan(min_velocity_)) + { + min_velocity_ = -max_velocity_; + } + } + if (has_acceleration_limits_) + { + if (std::isnan(max_acceleration_)) + { + throw std::runtime_error( + "Cannot apply acceleration limits if max_acceleration is not specified"); + } + if (std::isnan(min_acceleration_)) + { + min_acceleration_ = -max_acceleration_; + } + } + if (has_jerk_limits_) + { + if (std::isnan(max_jerk_)) + { + throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified"); + } + if (std::isnan(min_jerk_)) + { + min_jerk_ = -max_jerk_; + } + } +} + +double SpeedLimiter::limit(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + limit_jerk(v, v0, v1, dt); + limit_acceleration(v, v0, dt); + limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_velocity(double & v) +{ + const double tmp = v; + + if (has_velocity_limits_) + { + v = rcppmath::clamp(v, min_velocity_, max_velocity_); + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_acceleration(double & v, double v0, double dt) +{ + const double tmp = v; + + if (has_acceleration_limits_) + { + const double dv_min = min_acceleration_ * dt; + const double dv_max = max_acceleration_ * dt; + + const double dv = rcppmath::clamp(v - v0, dv_min, dv_max); + + v = v0 + dv; + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + if (has_jerk_limits_) + { + const double dv = v - v0; + const double dv0 = v0 - v1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_jerk_ * dt2; + const double da_max = max_jerk_ * dt2; + + const double da = rcppmath::clamp(dv - dv0, da_min, da_max); + + v = v0 + dv0 + da; + } + + return tmp != 0.0 ? v / tmp : 1.0; +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp new file mode 100644 index 0000000000..2f28e5700b --- /dev/null +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -0,0 +1,670 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + * Author: Sara Al Arab + */ + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/logging.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tricycle_controller/tricycle_controller.hpp" + +namespace +{ +constexpr auto DEFAULT_COMMAND_TOPIC = "cmd_vel"; +constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = + "cmd_vel"; // TODO: revert to cmd_vel_unstamped and remap topic +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 +{ +using namespace std::chrono_literals; +using controller_interface::interface_configuration_type; +using controller_interface::InterfaceConfiguration; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using lifecycle_msgs::msg::State; + +TricycleController::TricycleController() : controller_interface::ControllerInterface() {} + +CallbackReturn TricycleController::on_init() +{ + try + { + // with the lifecycle node being initialized, we can declare parameters + auto_declare("traction_joint_name", std::string()); + auto_declare("steering_joint_name", std::string()); + + auto_declare("wheelbase", wheel_params_.wheelbase); + auto_declare("wheel_radius", wheel_params_.radius); + + auto_declare("odom_frame_id", odom_params_.odom_frame_id); + auto_declare("base_frame_id", odom_params_.base_frame_id); + auto_declare>("pose_covariance_diagonal", std::vector()); + auto_declare>("twist_covariance_diagonal", std::vector()); + auto_declare("open_loop", odom_params_.open_loop); + auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); + 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("velocity_rolling_window_size", 10); + auto_declare("use_stamped_vel", use_stamped_vel_); + + auto_declare("linear.x.has_velocity_limits", false); + auto_declare("linear.x.has_acceleration_limits", false); + auto_declare("linear.x.has_jerk_limits", false); + auto_declare("linear.x.max_velocity", NAN); + auto_declare("linear.x.min_velocity", NAN); + auto_declare("linear.x.max_acceleration", NAN); + auto_declare("linear.x.min_acceleration", NAN); + auto_declare("linear.x.max_jerk", NAN); + auto_declare("linear.x.min_jerk", NAN); + + auto_declare("angular.z.has_velocity_limits", false); + auto_declare("angular.z.has_acceleration_limits", false); + auto_declare("angular.z.has_jerk_limits", false); + auto_declare("angular.z.max_velocity", NAN); + auto_declare("angular.z.min_velocity", NAN); + auto_declare("angular.z.max_acceleration", NAN); + auto_declare("angular.z.min_acceleration", NAN); + auto_declare("angular.z.max_jerk", NAN); + auto_declare("angular.z.min_jerk", NAN); + auto_declare("publish_rate", publish_rate_); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +InterfaceConfiguration TricycleController::command_interface_configuration() const +{ + InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); + command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + return command_interfaces_config; +} + +InterfaceConfiguration TricycleController::state_interface_configuration() const +{ + InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY); + state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION); + return state_interfaces_config; +} + +controller_interface::return_type TricycleController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + const auto current_time = + get_node() + ->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; + received_velocity_msg_ptr_.get(last_command_msg); + if (last_command_msg == nullptr) + { + RCLCPP_WARN(get_node()->get_logger(), "Velocity message received was a nullptr."); + return controller_interface::return_type::ERROR; + } + + const auto age_of_last_command = current_time - last_command_msg->header.stamp; + // Brake if cmd_vel has timeout, override the stored command + if (age_of_last_command > cmd_vel_timeout_) + { + last_command_msg->twist.linear.x = 0.0; + last_command_msg->twist.angular.z = 0.0; + } + + // command may be limited further by SpeedLimit, + // without affecting the stored twist command + Twist command = *last_command_msg; + double & linear_command = command.twist.linear.x; + double & angular_command = command.twist.angular.z; + + if (odom_params_.open_loop) + { + odometry_.updateOpenLoop(linear_command, angular_command, current_time); + } + else + { + const double Ws = traction_joint_[0].velocity_state.get().get_value(); // in radians/s + double alpha = steering_joint_[0].position_state.get().get_value(); // in radians + if (std::isnan(Ws) || std::isnan(alpha)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not read feeback value"); + return controller_interface::return_type::ERROR; + } + odometry_.updateFromVelocity(Ws, alpha, current_time); + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + if (previous_publish_timestamp_ + publish_period_ < current_time) + { + previous_publish_timestamp_ += publish_period_; + + if (realtime_odometry_publisher_->trylock()) + { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = current_time; + if (!odom_params_.odom_only_twist) + { + odometry_message.pose.pose.position.x = odometry_.getX(); + odometry_message.pose.pose.position.y = odometry_.getY(); + odometry_message.pose.pose.orientation.x = orientation.x(); + odometry_message.pose.pose.orientation.y = orientation.y(); + odometry_message.pose.pose.orientation.z = orientation.z(); + odometry_message.pose.pose.orientation.w = orientation.w(); + } + odometry_message.twist.twist.linear.x = odometry_.getLinear(); + odometry_message.twist.twist.angular.z = -odometry_.getAngular(); + realtime_odometry_publisher_->unlockAndPublish(); + } + + if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + { + auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + transform.header.stamp = current_time; + transform.transform.translation.x = odometry_.getX(); + transform.transform.translation.y = odometry_.getY(); + transform.transform.rotation.x = orientation.x(); + transform.transform.rotation.y = orientation.y(); + transform.transform.rotation.z = orientation.z(); + transform.transform.rotation.w = orientation.w(); + realtime_odometry_transform_publisher_->unlockAndPublish(); + } + } + + const auto update_dt = current_time - previous_update_timestamp_; + previous_update_timestamp_ = current_time; + + auto & last_command = previous_commands_.back().twist; + auto & second_to_last_command = previous_commands_.front().twist; + limiter_linear_.limit( + linear_command, last_command.linear.x, second_to_last_command.linear.x, update_dt.seconds()); + limiter_angular_.limit( + angular_command, last_command.angular.z, second_to_last_command.angular.z, update_dt.seconds()); + + 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()) + { + auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; + realtime_ackermann_command.speed = Ws; + 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); + return controller_interface::return_type::OK; +} + +CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +{ + auto logger = get_node()->get_logger(); + + // update parameters + traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string(); + steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string(); + if (traction_joint_name_.empty()) + { + RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty"); + return CallbackReturn::ERROR; + } + if (steering_joint_name_.empty()) + { + RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty"); + return CallbackReturn::ERROR; + } + + wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double(); + wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double(); + + odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius); + odometry_.setVelocityRollingWindowSize( + get_node()->get_parameter("velocity_rolling_window_size").as_int()); + + odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string(); + odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string(); + + auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array(); + std::copy( + pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin()); + + auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array(); + std::copy( + twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin()); + + odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool(); + odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool(); + odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool(); + + 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(); + use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + + try + { + limiter_linear_ = SpeedLimiter( + get_node()->get_parameter("linear.x.has_velocity_limits").as_bool(), + get_node()->get_parameter("linear.x.has_acceleration_limits").as_bool(), + get_node()->get_parameter("linear.x.has_jerk_limits").as_bool(), + get_node()->get_parameter("linear.x.min_velocity").as_double(), + get_node()->get_parameter("linear.x.max_velocity").as_double(), + get_node()->get_parameter("linear.x.min_acceleration").as_double(), + get_node()->get_parameter("linear.x.max_acceleration").as_double(), + get_node()->get_parameter("linear.x.min_jerk").as_double(), + get_node()->get_parameter("linear.x.max_jerk").as_double()); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring linear speed limiter: %s", e.what()); + } + + try + { + limiter_angular_ = SpeedLimiter( + get_node()->get_parameter("angular.z.has_velocity_limits").as_bool(), + get_node()->get_parameter("angular.z.has_acceleration_limits").as_bool(), + get_node()->get_parameter("angular.z.has_jerk_limits").as_bool(), + get_node()->get_parameter("angular.z.min_velocity").as_double(), + get_node()->get_parameter("angular.z.max_velocity").as_double(), + get_node()->get_parameter("angular.z.min_acceleration").as_double(), + get_node()->get_parameter("angular.z.max_acceleration").as_double(), + get_node()->get_parameter("angular.z.min_jerk").as_double(), + get_node()->get_parameter("angular.z.max_jerk").as_double()); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring angular speed limiter: %s", e.what()); + } + + if (!reset()) + { + 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 AckermannDrive empty_ackermann_command; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + + // 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 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 command subscriber + if (use_stamped_vel_) + { + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) + { + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); + }); + } + else + { + velocity_command_unstamped_subscriber_ = + get_node()->create_subscription( + DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + 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(); + }); + } + + // initialize odometry publisher and messasge + odometry_publisher_ = get_node()->create_publisher( + DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_publisher_ = + std::make_shared>( + odometry_publisher_); + + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.frame_id = odom_params_.odom_frame_id; + odometry_message.child_frame_id = odom_params_.base_frame_id; + + // limit the publication on the topics /odom and /tf + publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); + publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + previous_publish_timestamp_ = get_node()->get_clock()->now(); + + // initialize odom values zeros + odometry_message.twist = + geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); + + constexpr size_t NUM_DIMENSIONS = 6; + for (size_t index = 0; index < 6; ++index) + { + // 0, 7, 14, 21, 28, 35 + const size_t diagonal_index = NUM_DIMENSIONS * index + index; + odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index]; + odometry_message.twist.covariance[diagonal_index] = + odom_params_.twist_covariance_diagonal[index]; + } + + // initialize transform publisher and message + if (odom_params_.enable_odom_tf) + { + odometry_transform_publisher_ = get_node()->create_publisher( + DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_odometry_transform_publisher_ = + std::make_shared>( + odometry_transform_publisher_); + + // keeping track of odom and base_link transforms only + auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; + odometry_transform_message.transforms.resize(1); + odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id; + odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id; + } + + previous_update_timestamp_ = get_node()->get_clock()->now(); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints"); + + // Initialize the joints + const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_); + const auto steering_result = get_steering(steering_joint_name_, steering_joint_); + if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR) + { + return CallbackReturn::ERROR; + } + if (traction_joint_.empty() || steering_joint_.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Either steering or traction interfaces are non existent"); + return CallbackReturn::ERROR; + } + + is_halted = false; + subscriber_is_active_ = true; + + RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active."); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_deactivate(const rclcpp_lifecycle::State &) +{ + subscriber_is_active_ = false; + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_cleanup(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + + received_velocity_msg_ptr_.set(std::make_shared()); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::on_error(const rclcpp_lifecycle::State &) +{ + if (!reset()) + { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +bool TricycleController::reset() +{ + odometry_.resetOdometry(); + + // release the old queue + 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(); + // steering_joint_.clear(); + + subscriber_is_active_ = false; + velocity_command_subscriber_.reset(); + velocity_command_unstamped_subscriber_.reset(); + + received_velocity_msg_ptr_.set(nullptr); + is_halted = false; + return true; +} + +CallbackReturn TricycleController::on_shutdown(const rclcpp_lifecycle::State &) +{ + return CallbackReturn::SUCCESS; +} + +void TricycleController::halt() { traction_joint_[0].velocity_command.get().set_value(0.0); } + +CallbackReturn TricycleController::get_traction( + const std::string & traction_joint_name, std::vector & joint) +{ + RCLCPP_INFO(get_node()->get_logger(), "Get Wheel Joint Instance"); + + // Lookup the velocity state interface + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&traction_joint_name](const auto & interface) + { + return interface.get_name() == traction_joint_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + traction_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Lookup the velocity command interface + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&traction_joint_name](const hardware_interface::LoanedCommandInterface & interface) + { + return interface.get_name() == traction_joint_name && + interface.get_interface_name() == HW_IF_VELOCITY; + }); + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + traction_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Create the traction joint instance + joint.emplace_back(TractionHandle{std::ref(*state_handle), std::ref(*command_handle)}); + return CallbackReturn::SUCCESS; +} + +CallbackReturn TricycleController::get_steering( + const std::string & steering_joint_name, std::vector & joint) +{ + RCLCPP_INFO(get_node()->get_logger(), "Get Steering Joint Instance"); + + // Lookup the velocity state interface + const auto state_handle = std::find_if( + state_interfaces_.cbegin(), state_interfaces_.cend(), + [&steering_joint_name](const auto & interface) + { + return interface.get_name() == steering_joint_name && + interface.get_interface_name() == HW_IF_POSITION; + }); + if (state_handle == state_interfaces_.cend()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + steering_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Lookup the velocity command interface + const auto command_handle = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&steering_joint_name](const hardware_interface::LoanedCommandInterface & interface) + { + return interface.get_name() == steering_joint_name && + interface.get_interface_name() == HW_IF_POSITION; + }); + if (command_handle == command_interfaces_.end()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Unable to obtain joint state handle for %s", + steering_joint_name.c_str()); + return CallbackReturn::ERROR; + } + + // Create the steering joint instance + joint.emplace_back(SteeringHandle{std::ref(*state_handle), std::ref(*command_handle)}); + return CallbackReturn::SUCCESS; +} + +double TricycleController::convert_trans_rot_vel_to_steering_angle( + double Vx, double theta_dot, double wheelbase) +{ + if (theta_dot == 0 || Vx == 0) + { + return 0; + } + 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 + double alpha, Ws; + theta_dot = -theta_dot; + + if (Vx == 0 && abs(theta_dot) > 0.1) + { // is spin action + Vx = 0.1; + } + + alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase); + Ws = Vx / (wheel_params_.radius * std::cos(alpha)); + return std::make_tuple(alpha, Ws); +} + +} // namespace tricycle_controller + +#include +PLUGINLIB_EXPORT_CLASS( + tricycle_controller::TricycleController, controller_interface::ControllerInterface) diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp new file mode 100644 index 0000000000..80863aa92e --- /dev/null +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -0,0 +1,44 @@ +// Copyright 2022 Pixel Robotics. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Tony Najjar + * Author: Sara Al Arab + */ + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadTricycleController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_tricycle_controller", "tricycle_controller/TricycleController")); + + rclcpp::shutdown(); +} diff --git a/tricycle_controller/tricycle_controller.xml b/tricycle_controller/tricycle_controller.xml new file mode 100644 index 0000000000..568b13532b --- /dev/null +++ b/tricycle_controller/tricycle_controller.xml @@ -0,0 +1,7 @@ + + + + The tricycle controller transforms linear and angular velocity messages into signals for steering and traction joints for a tricycle drive robot. + + +