Merge branch 'master' into jtc/parse_urdf

joint_trajectory_controller/doc/userdoc.rst

@@ -193,6 +193,7 @@ Further information
 
    Trajectory Representation <trajectory.rst>
    joint_trajectory_controller Parameters <parameters.rst>
+   rqt_joint_trajectory_controller <../../rqt_joint_trajectory_controller/doc/userdoc.rst>
 
 
 .. rubric:: Footnote

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -16,10 +16,9 @@
 #define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_
 
 #include <chrono>
+#include <functional>  // for std::reference_wrapper
 #include <memory>
-#include <mutex>
 #include <string>
-#include <utility>
 #include <vector>
 
 #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"
@@ -51,19 +50,8 @@
 
 using namespace std::chrono_literals;  // NOLINT
 
-namespace rclcpp_action
-{
-template <typename ActionT>
-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
 {

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 <cassert>
-#include <cmath>
-#include <string>
 #include <vector>
 
 #include "control_msgs/action/follow_joint_trajectory.hpp"

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -14,12 +14,10 @@
 
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 
-#include <stddef.h>
 #include <chrono>
 #include <functional>
 #include <memory>
-#include <ostream>
-#include <ratio>
+
 #include <string>
 #include <vector>
 
@@ -33,13 +31,10 @@
 #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"
 #include "urdf/model.h"

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -14,7 +14,6 @@
 
 #include <stddef.h>
 
-#include <array>
 #include <chrono>
 #include <cmath>
 #include <future>
@@ -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"
 

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
 {

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

steering_controllers_library/CMakeLists.txt

@@ -67,6 +67,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(

steering_controllers_library/src/steering_odometry.cpp

@@ -270,8 +270,8 @@ std::tuple<std::vector<double>, std::vector<double>> 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);

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);
+}