diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 061f64b8db..8aece3791d 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -37,11 +37,9 @@ void reset_controller_reference_msg(trajectory_msgs::msg::JointTrajectoryPoint & } // called from RT control loop -void reset_wrench_msg( - geometry_msgs::msg::WrenchStamped & msg, - const std::shared_ptr & node) +void reset_wrench_msg(geometry_msgs::msg::WrenchStamped & msg) { - msg.header.stamp = node->now(); + msg.header.stamp = rclcpp::Time(0); msg.wrench = geometry_msgs::msg::Wrench(); } @@ -403,7 +401,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( } } reset_controller_reference_msg(joint_command_msg_); - reset_wrench_msg(wrench_command_msg_, get_node()); + reset_wrench_msg(wrench_command_msg_); // Use current joint_state as a default reference last_reference_ = joint_state_; @@ -519,7 +517,7 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( admittance_->reset(num_joints_); reset_controller_reference_msg(joint_command_msg_); - reset_wrench_msg(wrench_command_msg_, get_node()); + reset_wrench_msg(wrench_command_msg_); return CallbackReturn::SUCCESS; } diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 1d2b3836fc..f201724221 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -659,7 +659,7 @@ void DiffDriveController::reset_buffers() // Fill RealtimeBox with NaNs so it will contain a known value // but still indicate that no command has yet been sent. - command_msg_.header.stamp = get_node()->now(); + command_msg_.header.stamp = rclcpp::Time(0); command_msg_.twist.linear.x = std::numeric_limits::quiet_NaN(); command_msg_.twist.linear.y = std::numeric_limits::quiet_NaN(); command_msg_.twist.linear.z = std::numeric_limits::quiet_NaN(); diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 8ee414d7a0..ac3169251f 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -34,10 +34,9 @@ void print_interface(const rclcpp::Logger & logger, const T & command_interfaces } // called from RT control loop -void reset_controller_reference_msg( - gpio_controllers::CmdType & msg, const std::shared_ptr & node) +void reset_controller_reference_msg(gpio_controllers::CmdType & msg) { - msg.header.stamp = node->now(); + msg.header.stamp = rclcpp::Time(0); msg.interface_groups.clear(); msg.interface_values.clear(); } @@ -147,7 +146,7 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State initialize_gpio_state_msg(); // Set default value in command - reset_controller_reference_msg(gpio_commands_, get_node()); + reset_controller_reference_msg(gpio_commands_); rt_command_.try_set(gpio_commands_); RCLCPP_INFO(get_node()->get_logger(), "activate successful"); return CallbackReturn::SUCCESS; @@ -156,7 +155,7 @@ CallbackReturn GpioCommandController::on_activate(const rclcpp_lifecycle::State CallbackReturn GpioCommandController::on_deactivate(const rclcpp_lifecycle::State &) { // Set default value in command - reset_controller_reference_msg(gpio_commands_, get_node()); + reset_controller_reference_msg(gpio_commands_); rt_command_.try_set(gpio_commands_); return CallbackReturn::SUCCESS; } diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 2023e3b99a..10d559f779 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -33,10 +33,9 @@ using ControllerReferenceMsg = mecanum_drive_controller::MecanumDriveController::ControllerReferenceMsg; // called from RT control loop -void reset_controller_reference_msg( - ControllerReferenceMsg & msg, const std::shared_ptr & node) +void reset_controller_reference_msg(ControllerReferenceMsg & msg) { - msg.header.stamp = node->now(); + msg.header.stamp = rclcpp::Time(0); msg.twist.linear.x = std::numeric_limits::quiet_NaN(); msg.twist.linear.y = std::numeric_limits::quiet_NaN(); msg.twist.linear.z = std::numeric_limits::quiet_NaN(); @@ -130,7 +129,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( "~/reference", subscribers_qos, std::bind(&MecanumDriveController::reference_callback, this, std::placeholders::_1)); - reset_controller_reference_msg(current_ref_, get_node()); + reset_controller_reference_msg(current_ref_); input_ref_.set(current_ref_); // deprecation warning if tf_frame_prefix_enable set to non-default value @@ -297,7 +296,7 @@ void MecanumDriveController::reference_callback(const std::shared_ptrwait_for_commands(executor); reference = controller_->input_ref_.get(); - ASSERT_EQ(old_timestamp.sec, reference.header.stamp.sec); + EXPECT_GT(reference.header.stamp.sec, old_timestamp.sec); EXPECT_FALSE(std::isnan(reference.twist.linear.x)); EXPECT_FALSE(std::isnan(reference.twist.angular.z)); EXPECT_EQ(reference.twist.linear.x, 1.5); diff --git a/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp b/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp index ae0e5785bc..38de23631f 100644 --- a/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp +++ b/omni_wheel_drive_controller/src/omni_wheel_drive_controller.cpp @@ -566,7 +566,7 @@ void OmniWheelDriveController::reset_buffers() // Fill RealtimeBox with NaNs so it will contain a known value // but still indicate that no command has yet been sent. - command_msg_.header.stamp = get_node()->now(); + command_msg_.header.stamp = rclcpp::Time(0); command_msg_.twist.linear.x = std::numeric_limits::quiet_NaN(); command_msg_.twist.linear.y = std::numeric_limits::quiet_NaN(); command_msg_.twist.linear.z = std::numeric_limits::quiet_NaN(); diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 32997db6c1..f56179378c 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -14,6 +14,7 @@ #include "steering_controllers_library/steering_controllers_library.hpp" +#include #include #include #include @@ -31,10 +32,9 @@ using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; // called from RT control loop -void reset_controller_reference_msg( - ControllerTwistReferenceMsg & msg, const std::shared_ptr & node) +void reset_controller_reference_msg(ControllerTwistReferenceMsg & msg) { - msg.header.stamp = node->now(); + msg.header.stamp = rclcpp::Time(0); msg.twist.linear.x = std::numeric_limits::quiet_NaN(); msg.twist.linear.y = std::numeric_limits::quiet_NaN(); msg.twist.linear.z = std::numeric_limits::quiet_NaN(); @@ -263,7 +263,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - reset_controller_reference_msg(current_ref_, get_node()); + reset_controller_reference_msg(current_ref_); input_ref_.set(current_ref_); try @@ -470,7 +470,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( { // Try to set default value in command. // If this fails, then another command will be received soon anyways. - reset_controller_reference_msg(current_ref_, get_node()); + reset_controller_reference_msg(current_ref_); input_ref_.try_set(current_ref_); return controller_interface::CallbackReturn::SUCCESS; @@ -727,10 +727,28 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c controller_state_msg_.steering_angle_command.push_back(command_interface_value_op.value()); } } - controller_state_publisher_->try_publish(controller_state_msg_); } + // store current ref (for open loop odometry) and update odometry + if (std::isfinite(reference_interfaces_[0])) + { + last_linear_velocity_ = reference_interfaces_[0]; + } + else + { + last_linear_velocity_ = 0.0; + } + if (std::isfinite(reference_interfaces_[1])) + { + last_angular_velocity_ = reference_interfaces_[1]; + } + else + { + last_angular_velocity_ = 0.0; + } + update_odometry(period); + reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); @@ -763,7 +781,7 @@ bool SteeringControllersLibrary::reset() { odometry_.set_odometry(0.0, 0.0, 0.0); - reset_controller_reference_msg(current_ref_, get_node()); + reset_controller_reference_msg(current_ref_); input_ref_.set(current_ref_); last_linear_velocity_ = std::numeric_limits::quiet_NaN(); diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 627e5c2e6c..275e44c136 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -426,6 +426,8 @@ TEST_F(SteeringControllersLibraryTest, odometry_set_service) for (int i = 0; i < 10; ++i) move_robot(1.0, 0.0, 0.0); ASSERT_GT(controller_->odometry_.get_x(), 0.0); + move_robot(0.0, 0.0, 0.0); + // 2. Call the odometry set service auto set_request = std::make_shared(); auto set_response = std::make_shared();