diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 8336f0fbf3..ddd61fb2a6 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -148,10 +148,31 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } + // Append the tf prefix if there is one + std::string tf_prefix = ""; + if (params_.tf_frame_prefix_enable) + { + tf_prefix = params_.tf_frame_prefix != "" ? params_.tf_frame_prefix + : std::string(get_node()->get_namespace()); + + // Make sure prefix does not start with '/' and always ends with '/' + if (tf_prefix.back() != '/') + { + tf_prefix = tf_prefix + "/"; + } + if (tf_prefix.front() == '/') + { + tf_prefix.erase(0, 1); + } + } + + const auto odom_frame_id = tf_prefix + params_.odom_frame_id; + const auto base_frame_id = tf_prefix + params_.base_frame_id; + rt_odom_state_publisher_->lock(); rt_odom_state_publisher_->msg_.header.stamp = get_node()->now(); - rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; - rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id; + rt_odom_state_publisher_->msg_.header.frame_id = odom_frame_id; + rt_odom_state_publisher_->msg_.child_frame_id = base_frame_id; rt_odom_state_publisher_->msg_.pose.pose.position.z = 0; auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; @@ -182,8 +203,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( rt_tf_odom_state_publisher_->lock(); rt_tf_odom_state_publisher_->msg_.transforms.resize(1); rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now(); - rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id; - rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = odom_frame_id; + rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = base_frame_id; rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0; rt_tf_odom_state_publisher_->unlock(); @@ -207,7 +228,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( controller_state_publisher_->lock(); controller_state_publisher_->msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id; + controller_state_publisher_->msg_.header.frame_id = odom_frame_id; controller_state_publisher_->unlock(); RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 896b731953..f785481dd8 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -113,12 +113,21 @@ mecanum_drive_controller: read_only: false, } + tf_frame_prefix_enable: { + type: bool, + default_value: true, + description: "Enables or disables appending tf_prefix to tf frame id's.", + } + tf_frame_prefix: { + type: string, + default_value: "", + description: "(optional) Prefix to be appended to the tf frames, will be added to odom_frame_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + } base_frame_id: { type: string, default_value: "base_link", description: "Base frame_id set to value of base_frame_id.", read_only: false, - } odom_frame_id: { type: string, diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index c764c6c063..658cd3b2fa 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -1,40 +1,41 @@ -test_mecanum_drive_controller: - ros__parameters: - reference_timeout: 0.9 - - front_left_wheel_command_joint_name: "front_left_wheel_joint" - front_right_wheel_command_joint_name: "front_right_wheel_joint" - rear_right_wheel_command_joint_name: "back_right_wheel_joint" - rear_left_wheel_command_joint_name: "back_left_wheel_joint" - - kinematics: - base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } - wheels_radius: 0.5 - sum_of_robot_center_projection_on_X_Y_axis: 1.0 - - base_frame_id: "base_link" - odom_frame_id: "odom" - enable_odom_tf: true - twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - - -test_mecanum_drive_controller_with_rotation: - ros__parameters: - reference_timeout: 5.0 - - front_left_wheel_command_joint_name: "front_left_wheel_joint" - front_right_wheel_command_joint_name: "front_right_wheel_joint" - rear_right_wheel_command_joint_name: "rear_right_wheel_joint" - rear_left_wheel_command_joint_name: "rear_left_wheel_joint" - - kinematics: - base_frame_offset: { x: 1.0, y: 2.0, theta: 3.0 } - wheels_radius: 0.05 - sum_of_robot_center_projection_on_X_Y_axis: 0.2925 - - base_frame_id: "base_link" - odom_frame_id: "odom" - enable_odom_tf: true - pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] +/**: + test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.9 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + + + test_mecanum_drive_controller_with_rotation: + ros__parameters: + reference_timeout: 5.0 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "rear_right_wheel_joint" + rear_left_wheel_command_joint_name: "rear_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 1.0, y: 2.0, theta: 3.0 } + wheels_radius: 0.05 + sum_of_robot_center_projection_on_X_Y_axis: 0.2925 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceding_params.yaml index c4e5bb391a..72030202b9 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceding_params.yaml @@ -1,24 +1,25 @@ -test_mecanum_drive_controller: - ros__parameters: - reference_timeout: 0.1 +/**: + test_mecanum_drive_controller: + ros__parameters: + reference_timeout: 0.1 - front_left_wheel_command_joint_name: "front_left_wheel_joint" - front_right_wheel_command_joint_name: "front_right_wheel_joint" - rear_right_wheel_command_joint_name: "back_right_wheel_joint" - rear_left_wheel_command_joint_name: "back_left_wheel_joint" + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" - front_left_wheel_state_joint_name: "state_front_left_wheel_joint" - front_right_wheel_state_joint_name: "state_front_right_wheel_joint" - rear_right_wheel_state_joint_name: "state_back_right_wheel_joint" - rear_left_wheel_state_joint_name: "state_back_left_wheel_joint" + front_left_wheel_state_joint_name: "state_front_left_wheel_joint" + front_right_wheel_state_joint_name: "state_front_right_wheel_joint" + rear_right_wheel_state_joint_name: "state_back_right_wheel_joint" + rear_left_wheel_state_joint_name: "state_back_left_wheel_joint" - kinematics: - base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } - wheels_radius: 0.5 - sum_of_robot_center_projection_on_X_Y_axis: 1.0 + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 - base_frame_id: "base_link" - odom_frame_id: "odom" - enable_odom_tf: true - twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 00c1f5a74b..31c6c36737 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -118,6 +118,166 @@ TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_ex } } +TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(false)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the + frame + * id's */ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); +} + +TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace) +{ + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the + frame + * id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(false)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options, test_namespace); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + /* tf_frame_prefix_enable is false so no modifications to the frame id's */ + ASSERT_EQ(test_odom_frame_id, odom_id); + ASSERT_EQ(test_base_frame_id, base_link_id); +} + +TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = "test_prefix"; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options, test_namespace); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + + /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the + frame + * id's instead of the namespace*/ + ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id); + ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id); +} + +TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_namespace) +{ + std::string test_namespace = "/test_namespace"; + std::string odom_id = "odom"; + std::string base_link_id = "base_link"; + std::string frame_prefix = ""; + + auto node_options = controller_->define_custom_node_options(); + node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true)); + node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)); + node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id)); + node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id)); + + SetUpController("test_mecanum_drive_controller", node_options, test_namespace); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + std::string test_odom_frame_id = odometry_message.header.frame_id; + std::string test_base_frame_id = odometry_message.child_frame_id; + std::string ns_prefix = test_namespace.erase(0, 1) + "/"; + /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to + the + * frame id's */ + ASSERT_EQ(test_odom_frame_id, ns_prefix + odom_id); + ASSERT_EQ(test_base_frame_id, ns_prefix + base_link_id); +} + TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset) { SetUpController(); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 7f87824262..7d42c96c74 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -129,6 +129,16 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD size_t get_rear_right_wheel_index() { return WheelIndex::REAR_RIGHT; } size_t get_rear_left_wheel_index() { return WheelIndex::REAR_LEFT; } + + /** + * @brief Used to get the odometry message to verify its contents + * + * @return Copy of odometry msg from rt_odom_state_publisher_ object + */ + nav_msgs::msg::Odometry get_rt_odom_state_publisher_msg() + { + return rt_odom_state_publisher_->msg_; + } }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -161,12 +171,14 @@ class MecanumDriveControllerFixture : public ::testing::Test void TearDown() { controller_.reset(nullptr); } protected: - void SetUpController(const std::string controller_name = "test_mecanum_drive_controller") + void SetUpController( + const std::string controller_name = "test_mecanum_drive_controller", + const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions(), const std::string ns = "") { const auto urdf = ""; - const auto ns = ""; + ASSERT_EQ( - controller_->init(controller_name, urdf, 0, ns, controller_->define_custom_node_options()), + controller_->init(controller_name, urdf, 0, ns, node_options), controller_interface::return_type::OK); std::vector command_ifs;