diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index efb6b41e20..0cc5706dd2 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -161,10 +161,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; @@ -195,8 +216,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(); @@ -220,7 +241,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 3cbe51e1f2..ce4ae886cb 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -113,17 +113,28 @@ mecanum_drive_controller: read_only: false, } +<<<<<<< HEAD use_stamped_vel: { type: bool, default_value: true, description: "Use stamp from input velocity message to calculate how old the command actually is.", +======= + tf_frame_prefix_enable: { + type: bool, + default_value: true, + description: "Enables or disables appending ``tf_frame_prefix`` to tf frame id's. See its parameter description for more information.", + } + 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.", +>>>>>>> 97dd175 (Add tf_frame_prefix parameters to mecanum_drive_controller (#1680)) } 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 c4bd712d3d..b80230b5ee 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -116,6 +116,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 ce500fbbbe..0be9636928 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,9 +171,19 @@ 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 = "") { +<<<<<<< HEAD ASSERT_EQ(controller_->init(controller_name), controller_interface::return_type::OK); +======= + const auto urdf = ""; + + ASSERT_EQ( + controller_->init(controller_name, urdf, 0, ns, node_options), + controller_interface::return_type::OK); +>>>>>>> 97dd175 (Add tf_frame_prefix parameters to mecanum_drive_controller (#1680)) std::vector command_ifs; command_itfs_.reserve(joint_command_values_.size());