diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 0f6f2b31d8..30a03dd10d 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..c499bca983 --- /dev/null +++ b/tricycle_controller/CMakeLists.txt @@ -0,0 +1,118 @@ +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(std_srvs 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/traction_limiter.cpp + src/steering_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 + std_srvs + 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_tricycle_controller + test/test_tricycle_controller.cpp + ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml) + target_include_directories(test_tricycle_controller PRIVATE include) + target_link_libraries(test_tricycle_controller + ${PROJECT_NAME} + ) + + ament_target_dependencies(test_tricycle_controller + geometry_msgs + hardware_interface + nav_msgs + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_msgs + ) + + 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/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml new file mode 100644 index 0000000000..4eabf615ef --- /dev/null +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -0,0 +1,43 @@ +tricycle_controller: + ros__parameters: + # Model + traction_joint_name: traction_joint # Name of traction joint in URDF + steering_joint_name: steering_joint # Name of steering joint in URDF + wheel_radius: 0.0 # Radius of front wheel + wheelbase: 0.0 # Distance between center of back wheels and front wheel + + # Odometry + odom_frame_id: odom + base_frame_id: base_link + publish_rate: 50.0 # publish rate of odom and tf + open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry + enable_odom_tf: true # If True, publishes odom<-base_link TF + odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source + velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom + + # Rate Limiting + traction: # All values should be positive + # min_velocity: 0.0 + # max_velocity: 1000.0 + # min_acceleration: 0.0 + max_acceleration: 5.0 + # min_deceleration: 0.0 + max_deceleration: 8.0 + # min_jerk: 0.0 + # max_jerk: 1000.0 + steering: + min_position: -1.57 + max_position: 1.57 + # min_velocity: 0.0 + max_velocity: 1.0 + # min_acceleration: 0.0 + # max_acceleration: 1000.0 + + # cmd_vel input + cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received + use_stamped_vel: false # Set to True if using TwistStamped. + + # Debug + publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/tricycle_controller/doc/userdoc.rst b/tricycle_controller/doc/userdoc.rst new file mode 100644 index 0000000000..2271b8c9cf --- /dev/null +++ b/tricycle_controller/doc/userdoc.rst @@ -0,0 +1,24 @@ +.. _tricycle_controller_userdoc: + +tricycle_controller +===================== + +Controller for mobile robots with tricycle drive. +Input for control are robot base_link twist commands which are translated to traction and steering +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. + + +Other features +-------------- + + Realtime-safe implementation. + Odometry publishing + Velocity, acceleration and jerk limits + Automatic stop after command timeout \ 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..4a958a93c6 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/odometry.hpp @@ -0,0 +1,75 @@ +// 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 + */ + +#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); + + bool update(double left_vel, double right_vel, const rclcpp::Duration & dt); + void updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt); + 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 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/steering_limiter.hpp b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp new file mode 100644 index 0000000000..51ecac4cbd --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/steering_limiter.hpp @@ -0,0 +1,98 @@ +// 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 + */ + +#ifndef TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ +#define TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ + +#include + +namespace tricycle_controller +{ + +class SteeringLimiter +{ +public: + /** + * \brief Constructor + * \param [in] min_position Minimum position [m] or [rad] + * \param [in] max_position Maximum position [m] or [rad] + * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] + * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] + * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] + * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] + */ + SteeringLimiter( + double min_position = NAN, double max_position = NAN, + double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN); + + /** + * \brief Limit the position, velocity and acceleration + * \param [in, out] p position [m] or [rad] + * \param [in] p0 Previous position to p [m] or [rad] + * \param [in] p1 Previous position to p0 [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & p, double p0, double p1, double dt); + + /** + * \brief Limit the jerk + * \param [in, out] p position [m] or [rad] + * \param [in] p0 Previous position to p [m] or [rad] + * \param [in] p1 Previous position to p0 [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_position(double & p); + + /** + * \brief Limit the velocity + * \param [in, out] p position [m] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & p, double p0, double dt); + + /** + * \brief Limit the acceleration + * \param [in, out] p Position [m] or [rad] + * \param [in] p0 Previous position [m] or [rad] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & p, double p0, double p1, double dt); + +private: + + // Position limits: + double min_position_; + double max_position_; + + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_ diff --git a/tricycle_controller/include/tricycle_controller/traction_limiter.hpp b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp new file mode 100644 index 0000000000..4c0f297ee8 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/traction_limiter.hpp @@ -0,0 +1,104 @@ +// 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 + */ + +#ifndef TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ +#define TRICYCLE_CONTROLLER__TRACTION_LIMITER_HPP_ + +#include + +namespace tricycle_controller +{ + +class TractionLimiter +{ +public: + /** + * \brief Constructor + * \param [in] min_velocity Minimum velocity [m/s] or [rad/s] + * \param [in] max_velocity Maximum velocity [m/s] or [rad/s] + * \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2] + * \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2] + * \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2] + * \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2] + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + TractionLimiter( + double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, + double min_deceleration = NAN, double max_deceleration = NAN, + double min_jerk = NAN, double max_jerk = NAN); + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/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] or [rad/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & v); + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] or [rad/s] + * \param [in] v0 Previous velocity [m/s] or [rad/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] or [rad/s] + * \param [in] v0 Previous velocity to v [m/s] or [rad/s] + * \param [in] v1 Previous velocity to v0 [m/s] or [rad/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: + // Velocity limits: + double min_velocity_; + double max_velocity_; + + // Acceleration limits: + double min_acceleration_; + double max_acceleration_; + + // Deceleration limits: + double min_deceleration_; + double max_deceleration_; + + // Jerk limits: + double min_jerk_; + double max_jerk_; +}; + +} // namespace tricycle_controller + +#endif // TRICYCLE_CONTROLLER__TRACTION_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..c5ba1c62e3 --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -0,0 +1,185 @@ +// 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 + */ + +#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 "std_srvs/srv/empty.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/traction_limiter.hpp" +#include "tricycle_controller/steering_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::Twist; + using TwistStamped = 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); + std::tuple twist_to_ackermann(double linear_command, double angular_command); + + 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; + double radius = 0.0; + } 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_; + + bool publish_ackermann_command_ = false; + 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 velocity_command_subscriber_ = nullptr; + rclcpp::Subscription::SharedPtr + velocity_command_unstamped_subscriber_ = nullptr; + + realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; + + rclcpp::Service::SharedPtr reset_odom_service_; + + std::queue previous_commands_; // last two commands + + // speed limiters + TractionLimiter limiter_traction_; + SteeringLimiter limiter_steering_; + + // 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; + + void reset_odometry( + const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr res); + 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..bc9b34898b --- /dev/null +++ b/tricycle_controller/include/tricycle_controller/visibility_control.h @@ -0,0 +1,53 @@ +// 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 + */ + +#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..9dec765d60 --- /dev/null +++ b/tricycle_controller/package.xml @@ -0,0 +1,35 @@ + + + + tricycle_controller + 1.0.0 + Controller for a tricycle drive mobile base + Tony Najjar + Apache License 2.0 + Tony Najjar + + ament_cmake + + controller_interface + geometry_msgs + hardware_interface + nav_msgs + std_srvs + rclcpp + rclcpp_lifecycle + rcpputils + realtime_tools + tf2 + tf2_msgs + ackermann_msgs + + pluginlib + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + \ No newline at end of file diff --git a/tricycle_controller/src/odometry.cpp b/tricycle_controller/src/odometry.cpp new file mode 100644 index 0000000000..e0e670480a --- /dev/null +++ b/tricycle_controller/src/odometry.cpp @@ -0,0 +1,121 @@ +// 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 + */ + +#include "tricycle_controller/odometry.hpp" + +namespace tricycle_controller +{ +Odometry::Odometry(size_t velocity_rolling_window_size) +: 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) +{ +} + +bool Odometry::update(double Ws, double alpha, const rclcpp::Duration & dt) +{ + // 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 * dt.seconds(), theta_dot * dt.seconds()); + + // 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::Duration & dt) +{ + /// Save last linear and angular velocity: + linear_ = linear; + angular_ = angular; + + /// Integrate odometry: + integrateExact(linear * dt.seconds(), angular * dt.seconds()); +} + +void Odometry::resetOdometry() +{ + x_ = 0.0; + y_ = 0.0; + heading_ = 0.0; + resetAccumulators(); +} + +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/steering_limiter.cpp b/tricycle_controller/src/steering_limiter.cpp new file mode 100644 index 0000000000..6912144bd0 --- /dev/null +++ b/tricycle_controller/src/steering_limiter.cpp @@ -0,0 +1,116 @@ +// 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 + */ + +#include +#include + +#include "rcppmath/clamp.hpp" +#include "tricycle_controller/steering_limiter.hpp" + +namespace tricycle_controller +{ +SteeringLimiter::SteeringLimiter( + double min_position, double max_position, double min_velocity, double max_velocity, + double min_acceleration, double max_acceleration) +: min_position_(min_position), + max_position_(max_position), + min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration) +{ + if (!std::isnan(min_position_) && std::isnan(max_position_)) max_position_ = -min_position_; + if (!std::isnan(max_position_) && std::isnan(min_position_)) min_position_ = -max_position_; + + if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) + max_velocity_ = 1000.0; // Arbitrarily big number + if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; + + if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; + if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; + + const std::string error = + "The positive limit will be applied to both directions. Setting different limits for positive " + "and negative directions is not supported. Actuators are " + "assumed to have the same constraints in both directions"; + + if (min_velocity_ < 0 || max_velocity_ < 0) + { + throw std::invalid_argument("Velocity cannot be negative." + error); + } + + if (min_acceleration_ < 0 || max_acceleration_ < 0) + { + throw std::invalid_argument("Acceleration cannot be negative." + error); + } +} + +double SteeringLimiter::limit(double & p, double p0, double p1, double dt) +{ + const double tmp = p; + + if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) + limit_acceleration(p, p0, p1, dt); + if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(p, p0, dt); + if (!std::isnan(min_position_) && !std::isnan(max_position_)) limit_position(p); + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_position(double & p) +{ + const double tmp = p; + p = rcppmath::clamp(p, min_position_, max_position_); + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_velocity(double & p, double p0, double dt) +{ + const double tmp = p; + + const double dv_min = min_velocity_ * dt; + const double dv_max = max_velocity_ * dt; + + double dv = rcppmath::clamp((double)std::abs(p - p0), dv_min, dv_max); + dv *= (p - p0 >= 0 ? 1 : -1); + p = p0 + dv; + + return tmp != 0.0 ? p / tmp : 1.0; +} + +double SteeringLimiter::limit_acceleration(double & p, double p0, double p1, double dt) +{ + const double tmp = p; + + const double dv = p - p0; + const double dp0 = p0 - p1; + + const double dt2 = 2. * dt * dt; + + const double da_min = min_acceleration_ * dt2; + const double da_max = max_acceleration_ * dt2; + + double da = rcppmath::clamp((double)std::abs(dv - dp0), da_min, da_max); + da *= (dv - dp0 >= 0 ? 1 : -1); + p = p0 + dp0 + da; + + return tmp != 0.0 ? p / tmp : 1.0; +} + +} // namespace tricycle_controller diff --git a/tricycle_controller/src/traction_limiter.cpp b/tricycle_controller/src/traction_limiter.cpp new file mode 100644 index 0000000000..c2a0d88ab4 --- /dev/null +++ b/tricycle_controller/src/traction_limiter.cpp @@ -0,0 +1,142 @@ +// 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 + */ + +#include +#include +#include + +#include "rcppmath/clamp.hpp" +#include "tricycle_controller/traction_limiter.hpp" + +namespace tricycle_controller +{ +TractionLimiter::TractionLimiter( + double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, + double min_deceleration, double max_deceleration, double min_jerk, double max_jerk) +: min_velocity_(min_velocity), + max_velocity_(max_velocity), + min_acceleration_(min_acceleration), + max_acceleration_(max_acceleration), + min_deceleration_(min_deceleration), + max_deceleration_(max_deceleration), + min_jerk_(min_jerk), + max_jerk_(max_jerk) +{ + if (!std::isnan(min_velocity_) && std::isnan(max_velocity_)) + max_velocity_ = 1000.0; // Arbitrarily big number + if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0; + + if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0; + if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0; + + if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0; + if (!std::isnan(max_deceleration_) && std::isnan(min_acceleration_)) min_deceleration_ = 0.0; + + if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0; + if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0; + + const std::string error = + "The positive limit will be applied to both directions. Setting different limits for positive " + "and negative directions is not supported. Actuators are " + "assumed to have the same constraints in both directions"; + if (min_velocity_ < 0 || max_velocity_ < 0) + { + throw std::invalid_argument("Velocity cannot be negative." + error); + } + + if (min_acceleration_ < 0 || max_acceleration_ < 0) + { + throw std::invalid_argument("Acceleration cannot be negative." + error); + } + + if (min_deceleration_ < 0 || max_deceleration_ < 0) + { + throw std::invalid_argument("Deceleration cannot be negative." + error); + } + + if (min_jerk_ < 0 || max_jerk_ < 0) + { + throw std::invalid_argument("Jerk cannot be negative." + error); + } +} + +double TractionLimiter::limit(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + if (!std::isnan(min_jerk_) && !std::isnan(max_jerk_)) limit_jerk(v, v0, v1, dt); + if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_)) + limit_acceleration(v, v0, dt); + if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(v); + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_velocity(double & v) +{ + const double tmp = v; + + v = rcppmath::clamp((double)std::abs(v), min_velocity_, max_velocity_); + + v *= tmp >= 0 ? 1 : -1; + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_acceleration(double & v, double v0, double dt) +{ + const double tmp = v; + + double dv_min; + double dv_max; + if (abs(v) >= abs(v0)) + { + dv_min = min_acceleration_ * dt; + dv_max = max_acceleration_ * dt; + } + else + { + dv_min = min_deceleration_ * dt; + dv_max = max_deceleration_ * dt; + } + double dv = rcppmath::clamp((double)std::abs(v - v0), dv_min, dv_max); + dv *= (v - v0 >= 0 ? 1 : -1); + v = v0 + dv; + + return tmp != 0.0 ? v / tmp : 1.0; +} + +double TractionLimiter::limit_jerk(double & v, double v0, double v1, double dt) +{ + const double tmp = v; + + 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; + + double da = rcppmath::clamp((double)std::abs(dv - dv0), da_min, da_max); + da *= (dv - dv0 >= 0 ? 1 : -1); + 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..7cec107a91 --- /dev/null +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -0,0 +1,668 @@ +// 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 + */ + +#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_ACKERMANN_OUT_TOPIC = "~/cmd_ackermann"; +constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom"; +constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf"; +constexpr auto DEFAULT_RESET_ODOM_SERVICE = "~/reset_odometry"; +} // 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()); + auto_declare("publish_ackermann_command", publish_ackermann_command_); + auto_declare("velocity_rolling_window_size", 10); + auto_declare("use_stamped_vel", use_stamped_vel_); + auto_declare("publish_rate", publish_rate_); + + auto_declare("traction.max_velocity", NAN); + auto_declare("traction.min_velocity", NAN); + auto_declare("traction.max_acceleration", NAN); + auto_declare("traction.min_acceleration", NAN); + auto_declare("traction.max_deceleration", NAN); + auto_declare("traction.min_deceleration", NAN); + auto_declare("traction.max_jerk", NAN); + auto_declare("traction.min_jerk", NAN); + + auto_declare("steering.max_position", NAN); + auto_declare("steering.min_position", NAN); + auto_declare("steering.max_velocity", NAN); + auto_declare("steering.min_velocity", NAN); + auto_declare("steering.max_acceleration", NAN); + auto_declare("steering.min_acceleration", NAN); + } + 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) +{ + if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + { + if (!is_halted) + { + halt(); + is_halted = true; + } + return controller_interface::return_type::OK; + } + 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 = 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 Limiters, + // without affecting the stored twist command + TwistStamped command = *last_command_msg; + double & linear_command = command.twist.linear.x; + double & angular_command = command.twist.angular.z; + double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s + double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians + + if (odom_params_.open_loop) + { + odometry_.updateOpenLoop(linear_command, angular_command, period); + } + else + { + if (std::isnan(Ws_read) || std::isnan(alpha_read)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Could not read feeback value"); + return controller_interface::return_type::ERROR; + } + odometry_.update(Ws_read, alpha_read, period); + } + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, odometry_.getHeading()); + + if (previous_publish_timestamp_ + publish_period_ < time) + { + previous_publish_timestamp_ += publish_period_; + + if (realtime_odometry_publisher_->trylock()) + { + auto & odometry_message = realtime_odometry_publisher_->msg_; + odometry_message.header.stamp = 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 = 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(); + } + } + + // Compute wheel velocity and angle + auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command); + + // Reduce wheel speed until the target angle has been reached + double alpha_delta = abs(alpha_write - alpha_read); + double scale; + if (alpha_delta < M_PI / 6) + { + scale = 1; + } + else if (alpha_delta > M_PI_2) + { + scale = 0.01; + } + else + { + // TODO: find the best function, e.g convex power functions + scale = cos(alpha_delta); + } + Ws_write *= scale; + + auto & last_command = previous_commands_.back(); + auto & second_to_last_command = previous_commands_.front(); + + limiter_traction_.limit( + Ws_write, last_command.speed, second_to_last_command.speed, period.seconds()); + + limiter_steering_.limit( + alpha_write, last_command.steering_angle, second_to_last_command.steering_angle, + period.seconds()); + + previous_commands_.pop(); + AckermannDrive ackermann_command; + ackermann_command.speed = + Ws_write; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel speed (rad/s) + ackermann_command.steering_angle = alpha_write; + previous_commands_.emplace(ackermann_command); + + // 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_write; // 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_write; + realtime_ackermann_command_publisher_->unlockAndPublish(); + } + + traction_joint_[0].velocity_command.get().set_value(Ws_write); + steering_joint_[0].position_command.get().set_value(alpha_write); + 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{get_node()->get_parameter("cmd_vel_timeout").as_int()}; + publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool(); + use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool(); + + publish_rate_ = get_node()->get_parameter("publish_rate").as_double(); + publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_); + + try + { + limiter_traction_ = TractionLimiter( + get_node()->get_parameter("traction.min_velocity").as_double(), + get_node()->get_parameter("traction.max_velocity").as_double(), + get_node()->get_parameter("traction.min_acceleration").as_double(), + get_node()->get_parameter("traction.max_acceleration").as_double(), + get_node()->get_parameter("traction.min_deceleration").as_double(), + get_node()->get_parameter("traction.max_deceleration").as_double(), + get_node()->get_parameter("traction.min_jerk").as_double(), + get_node()->get_parameter("traction.max_jerk").as_double()); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what()); + return CallbackReturn::ERROR; + } + + try + { + limiter_steering_ = SteeringLimiter( + get_node()->get_parameter("steering.min_position").as_double(), + get_node()->get_parameter("steering.max_position").as_double(), + get_node()->get_parameter("steering.min_velocity").as_double(), + get_node()->get_parameter("steering.max_velocity").as_double(), + get_node()->get_parameter("steering.min_acceleration").as_double(), + get_node()->get_parameter("steering.max_acceleration").as_double()); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error configuring steering limiter: %s", e.what()); + return CallbackReturn::ERROR; + } + + if (!reset()) + { + return CallbackReturn::ERROR; + } + + const TwistStamped empty_twist; + received_velocity_msg_ptr_.set(std::make_shared(empty_twist)); + + // Fill last two commands with default constructed commands + const AckermannDrive empty_ackermann_drive; + previous_commands_.emplace(empty_ackermann_drive); + previous_commands_.emplace(empty_ackermann_drive); + + // 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( + 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_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; + + 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; + } + + // Create odom reset service + reset_odom_service_ = get_node()->create_service( + DEFAULT_RESET_ODOM_SERVICE, std::bind( + &TricycleController::reset_odometry, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3)); + + 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; +} + +void TricycleController::reset_odometry( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*req*/, + std::shared_ptr /*res*/) +{ + odometry_.resetOdometry(); + RCLCPP_INFO(get_node()->get_logger(), "Odometry successfully reset"); +} + +bool TricycleController::reset() +{ + odometry_.resetOdometry(); + + // release the old queue + std::queue empty_ackermann_drive; + std::swap(previous_commands_, empty_ackermann_drive); + + 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); + steering_joint_[0].position_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_prefix_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 auto & interface) + { + return interface.get_prefix_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_prefix_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 auto & interface) + { + return interface.get_prefix_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 * wheelbase / Vx); +} + +std::tuple TricycleController::twist_to_ackermann(double Vx, double theta_dot) +{ + // using naming convention in http://users.isr.ist.utl.pt/~mir/cadeiras/robmovel/Kinematics.pdf + double alpha, Ws; + + if (Vx == 0 && theta_dot != 0) + { // is spin action + alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2; + Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius; + return std::make_tuple(alpha, Ws); + } + + 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/config/test_tricycle_controller.yaml b/tricycle_controller/test/config/test_tricycle_controller.yaml new file mode 100644 index 0000000000..d81d722219 --- /dev/null +++ b/tricycle_controller/test/config/test_tricycle_controller.yaml @@ -0,0 +1,20 @@ + +test_tricycle_controller: + ros__parameters: + traction_joint_name: traction_joint + steering_joint_name: steering_joint + wheel_radius: 0.125 + wheelbase: 1.252 + use_stamped_vel: false + enable_odom_tf: false + use_sim_time: false + pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + odom_only_twist: true + publish_ackermann_command: true + traction: + max_acceleration: 5.0 + max_deceleration: 8.0 + steering: + max_position: 1.57 # pi/2 + max_velocity: 1.0 \ No newline at end of file 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..088cddbabe --- /dev/null +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -0,0 +1,43 @@ +// 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 + */ + +#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/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp new file mode 100644 index 0000000000..a1f09ddaf8 --- /dev/null +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -0,0 +1,350 @@ +// 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 + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tricycle_controller/tricycle_controller.hpp" + +using CallbackReturn = controller_interface::CallbackReturn; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::LoanedCommandInterface; +using hardware_interface::LoanedStateInterface; +using lifecycle_msgs::msg::State; +using testing::SizeIs; + +class TestableTricycleController : public tricycle_controller::TricycleController +{ +public: + using TricycleController::TricycleController; + std::shared_ptr getLastReceivedTwist() + { + std::shared_ptr ret; + received_velocity_msg_ptr_.get(ret); + return ret; + } + + /** + * @brief wait_for_twist block until a new twist is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + * + * @return true if new twist msg was received, false if timeout + */ + bool wait_for_twist( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds(500)) + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(velocity_command_subscriber_); + + if (wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready) + { + executor.spin_some(); + return true; + } + return false; + } +}; + +class TestTricycleController : public ::testing::Test +{ +protected: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + void SetUp() override + { + controller_ = std::make_unique(); + pub_node = std::make_shared("velocity_publisher"); + velocity_publisher = pub_node->create_publisher( + controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } + + /// Publish velocity msgs + /** + * linear - magnitude of the linear command in the geometry_msgs::twist message + * angular - the magnitude of the angular command in geometry_msgs::twist message + */ + void publish(double linear, double angular) + { + int wait_count = 0; + auto topic = velocity_publisher->get_topic_name(); + while (pub_node->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + geometry_msgs::msg::TwistStamped velocity_message; + velocity_message.header.stamp = pub_node->get_clock()->now(); + velocity_message.twist.linear.x = linear; + velocity_message.twist.angular.z = angular; + velocity_publisher->publish(velocity_message); + } + + /// \brief wait for the subscriber and publisher to completely setup + void waitForSetup() + { + constexpr std::chrono::seconds TIMEOUT{2}; + auto clock = pub_node->get_clock(); + auto start = clock->now(); + while (velocity_publisher->get_subscription_count() <= 0) + { + if ((clock->now() - start) > TIMEOUT) + { + FAIL(); + } + rclcpp::spin_some(pub_node); + } + } + + void assignResources() + { + std::vector state_ifs; + state_ifs.emplace_back(steering_joint_pos_state_); + state_ifs.emplace_back(traction_joint_vel_state_); + + std::vector command_ifs; + command_ifs.emplace_back(steering_joint_pos_cmd_); + command_ifs.emplace_back(traction_joint_vel_cmd_); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + const std::string controller_name = "test_tricycle_controller"; + std::unique_ptr controller_; + + const std::string traction_joint_name = "traction_joint"; + const std::string steering_joint_name = "steering_joint"; + double position_ = 0.1; + double velocity_ = 0.2; + + hardware_interface::StateInterface steering_joint_pos_state_{ + steering_joint_name, HW_IF_POSITION, &position_}; + + hardware_interface::StateInterface traction_joint_vel_state_{ + traction_joint_name, HW_IF_VELOCITY, &velocity_}; + + hardware_interface::CommandInterface steering_joint_pos_cmd_{ + steering_joint_name, HW_IF_POSITION, &position_}; + + hardware_interface::CommandInterface traction_joint_vel_cmd_{ + traction_joint_name, HW_IF_VELOCITY, &velocity_}; + + rclcpp::Node::SharedPtr pub_node; + rclcpp::Publisher::SharedPtr velocity_publisher; +}; + +TEST_F(TestTricycleController, configure_fails_without_parameters) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, configure_fails_if_only_traction_or_steering_side_defined) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(std::string()))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(std::string()))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, configure_succeeds_when_joints_are_specified) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestTricycleController, activate_fails_without_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR); +} + +TEST_F(TestTricycleController, activate_succeeds_with_resources_assigned) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + // We implicitly test that by default position feedback is required + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); + assignResources(); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); +} + +TEST_F(TestTricycleController, cleanup) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 1.2)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 0.12)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + auto state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + assignResources(); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + waitForSetup(); + + // send msg + const double linear = 1.0; + const double angular = 1.0; + publish(linear, angular); + controller_->wait_for_twist(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->deactivate(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + + // should be stopped + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + + executor.cancel(); +} + +TEST_F(TestTricycleController, correct_initialization_using_parameters) +{ + const auto ret = controller_->init(controller_name); + ASSERT_EQ(ret, controller_interface::return_type::OK); + + controller_->get_node()->set_parameter( + rclcpp::Parameter("traction_joint_name", rclcpp::ParameterValue(traction_joint_name))); + controller_->get_node()->set_parameter( + rclcpp::Parameter("steering_joint_name", rclcpp::ParameterValue(steering_joint_name))); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheelbase", 0.4)); + controller_->get_node()->set_parameter(rclcpp::Parameter("wheel_radius", 1.0)); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + auto state = controller_->get_node()->configure(); + assignResources(); + + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + + state = controller_->get_node()->activate(); + ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); + + // send msg + const double linear = 1.0; + const double angular = 0.0; + publish(linear, angular); + // wait for msg is be published to the system + ASSERT_TRUE(controller_->wait_for_twist(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + + // deactivated + // wait so controller process the second point when deactivated + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + state = controller_->get_node()->deactivate(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + + // cleanup + state = controller_->get_node()->cleanup(); + ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + + state = controller_->get_node()->configure(); + ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); + executor.cancel(); +} 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. + + +