diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index 6ad0e9485f..66f09c5f09 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(ackermann_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index a8a8832fce..b2c10e0ba5 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(admittance_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 5d662c1fec..7118e9a44d 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(bicycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index b944ff1e88..436832c523 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(diff_drive_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt index ad97690655..eae8642fb6 100644 --- a/effort_controllers/CMakeLists.txt +++ b/effort_controllers/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(effort_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 2af5500e21..38c984192e 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(force_torque_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/forward_command_controller/CMakeLists.txt b/forward_command_controller/CMakeLists.txt index 15ffe09750..4885c69c8a 100644 --- a/forward_command_controller/CMakeLists.txt +++ b/forward_command_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(forward_command_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/gripper_controllers/CMakeLists.txt b/gripper_controllers/CMakeLists.txt index 8edaaf6386..676062f7a3 100644 --- a/gripper_controllers/CMakeLists.txt +++ b/gripper_controllers/CMakeLists.txt @@ -7,7 +7,7 @@ if(APPLE OR WIN32) endif() if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 3b09d9e72e..6782012c65 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(imu_sensor_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index cadc96b4e3..5c383897cc 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(joint_state_broadcaster LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 88bb7891a3..5cddb1bf4d 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(joint_trajectory_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wconversion) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 6a34185437..6fe2146802 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -193,6 +193,7 @@ Further information Trajectory Representation joint_trajectory_controller Parameters + rqt_joint_trajectory_controller <../../rqt_joint_trajectory_controller/doc/userdoc.rst> .. rubric:: Footnote diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 3cc4293219..b9097b1ce3 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -16,10 +16,9 @@ #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_ #include +#include // for std::reference_wrapper #include -#include #include -#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" @@ -30,15 +29,15 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" +#include "joint_trajectory_controller/trajectory.hpp" #include "joint_trajectory_controller/visibility_control.h" #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp_action/server.hpp" -#include "rclcpp_action/types.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "realtime_tools/realtime_server_goal_handle.h" @@ -50,19 +49,8 @@ using namespace std::chrono_literals; // NOLINT -namespace rclcpp_action -{ -template -class ServerGoalHandle; -} // namespace rclcpp_action -namespace rclcpp_lifecycle -{ -class State; -} // namespace rclcpp_lifecycle - namespace joint_trajectory_controller { -class Trajectory; class JointTrajectoryController : public controller_interface::ControllerInterface { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 9ddf57ac3b..c46b1c297f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -30,9 +30,6 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ -#include -#include -#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 75932fb141..138ddc5f82 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -14,12 +14,10 @@ #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include #include #include #include -#include -#include + #include #include @@ -33,15 +31,11 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/event_handler.hpp" #include "rclcpp/logging.hpp" -#include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" #include "rclcpp_action/create_server.hpp" #include "rclcpp_action/server_goal_handle.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "std_msgs/msg/header.hpp" namespace joint_trajectory_controller { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 7181a08a16..8c0e3a0069 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -30,7 +29,6 @@ #include "builtin_interfaces/msg/time.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -44,9 +42,6 @@ #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "std_msgs/msg/header.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 332d72bf07..f98fd3e286 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,7 +25,6 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include "joint_trajectory_controller/trajectory.hpp" namespace { diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 81cbe6f006..15e903222a 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(pid_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 373e941d96..05fee986dd 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -171,10 +171,10 @@ controller_interface::CallbackReturn PidController::on_configure( if (params_.use_external_measured_states) { auto measured_state_callback = - [&](const std::shared_ptr msg) -> void + [&](const std::shared_ptr state_msg) -> void { // TODO(destogl): Sort the input values based on joint and interface names - measured_state_.writeFromNonRT(msg); + measured_state_.writeFromNonRT(state_msg); }; measured_state_subscriber_ = get_node()->create_subscription( "~/measured_state", subscribers_qos, measured_state_callback); diff --git a/position_controllers/CMakeLists.txt b/position_controllers/CMakeLists.txt index 54a4f94656..18f3cb313a 100644 --- a/position_controllers/CMakeLists.txt +++ b/position_controllers/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(position_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index 00da395db5..d7002d1b17 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png b/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png new file mode 100644 index 0000000000..cb86f4620b Binary files /dev/null and b/rqt_joint_trajectory_controller/doc/rqt_joint_trajectory_controller.png differ diff --git a/rqt_joint_trajectory_controller/doc/userdoc.rst b/rqt_joint_trajectory_controller/doc/userdoc.rst new file mode 100644 index 0000000000..197a7afb30 --- /dev/null +++ b/rqt_joint_trajectory_controller/doc/userdoc.rst @@ -0,0 +1,12 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/rqt_joint_trajectory_controller/doc/userdoc.rst + +.. _rqt_joint_trajectory_controller_userdoc: + +rqt_joint_trajectory_controller +=============================== + +rqt_joint_trajectory_controller is a GUI plugin for rqt that allows to command a joint_trajectory_controller. + +.. image:: rqt_joint_trajectory_controller.png + :width: 400 + :alt: rqt_joint_trajectory_controller diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index b64ea204a8..88f24d304c 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(steering_controllers_library LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies @@ -67,6 +68,9 @@ if(BUILD_TESTING) controller_interface hardware_interface ) + ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp) + target_link_libraries(test_steering_odometry steering_controllers_library) + endif() install( diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index bf254bfcfa..e2ced036ff 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -270,8 +270,8 @@ std::tuple, std::vector> SteeringOdometry::get_comma double denominator_first_member = 2 * wheelbase_ * std::cos(alpha); double denominator_second_member = wheel_track_ * std::sin(alpha); - double alpha_r = std::atan2(numerator, denominator_first_member - denominator_second_member); - double alpha_l = std::atan2(numerator, denominator_first_member + denominator_second_member); + double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); + double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member); steering_commands = {alpha_r, alpha_l}; } return std::make_tuple(traction_commands, steering_commands); diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp new file mode 100644 index 0000000000..173c76baef --- /dev/null +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -0,0 +1,110 @@ +// Copyright (c) 2023, Virtual Vehicle Research GmbH +// +// 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. + +#include "gmock/gmock.h" + +#include "steering_controllers_library/steering_odometry.hpp" + +TEST(TestSteeringOdometry, initialize) +{ + EXPECT_NO_THROW(steering_odometry::SteeringOdometry()); + + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 3.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + EXPECT_DOUBLE_EQ(odom.get_heading(), 0.); + EXPECT_DOUBLE_EQ(odom.get_x(), 0.); + EXPECT_DOUBLE_EQ(odom.get_y(), 0.); +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(2., 0., 0.5); + EXPECT_DOUBLE_EQ(odom.get_linear(), 2.); + EXPECT_DOUBLE_EQ(odom.get_x(), 1.); + EXPECT_DOUBLE_EQ(odom.get_y(), 0.); +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 1., 1.); + EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); + EXPECT_DOUBLE_EQ(odom.get_angular(), 1.); + + EXPECT_GT(odom.get_x(), 0); // pos x + EXPECT_GT(odom.get_y(), 0); // pos y, ie. left +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., -1., 1.); + EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); + EXPECT_DOUBLE_EQ(odom.get_angular(), -1.); + EXPECT_GT(odom.get_x(), 0); // pos x + EXPECT_LT(odom.get_y(), 0); // neg y ie. right +} + +TEST(TestSteeringOdometry, ackermann_back_kin_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0.); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], cmd1[1]); // no steering + EXPECT_EQ(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, ackermann_back_kin_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], cmd1[1]); // right (outer) < left (inner) + EXPECT_GT(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, ackermann_back_kin_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); +} diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index 291f08ad9f..4f6d064dc8 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(tricycle_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 4c56d68185..02c9453ace 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(tricycle_steering_controller LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() # find dependencies diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index 3642ae1cb9..a39cd162fd 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.16) project(velocity_controllers LANGUAGES CXX) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic -Wconversion) + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) endif() set(THIS_PACKAGE_INCLUDE_DEPENDS