diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 47c9fd514a..51da1fc848 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -130,6 +130,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt input_joint_command_; realtime_tools::RealtimeThreadSafeBox input_wrench_command_; std::unique_ptr> state_publisher_; + ControllerStateMsg state_msg_; // save the last commands in case of unable to get value from box trajectory_msgs::msg::JointTrajectoryPoint joint_command_msg_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index e19daefb3e..13e9e47163 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -336,9 +336,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( std::make_unique>(s_publisher_); // Initialize state message - state_publisher_->lock(); - state_publisher_->msg_ = admittance_->get_controller_state(); - state_publisher_->unlock(); + state_msg_ = admittance_->get_controller_state(); // Initialize FTS semantic semantic_component force_torque_sensor_ = std::make_unique( @@ -492,9 +490,11 @@ controller_interface::return_type AdmittanceController::update_and_write_command write_state_to_hardware(reference_admittance_); // Publish controller state - state_publisher_->lock(); - state_publisher_->msg_ = admittance_->get_controller_state(); - state_publisher_->unlockAndPublish(); + if (state_publisher_) + { + state_msg_ = admittance_->get_controller_state(); + state_publisher_->try_publish(state_msg_); + } return controller_interface::return_type::OK; } diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index 0ca1d023ec..fca6e30039 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -113,11 +113,13 @@ class DiffDriveController : public controller_interface::ChainableControllerInte std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; + nav_msgs::msg::Odometry odometry_message_; std::shared_ptr> odometry_transform_publisher_ = nullptr; std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; + tf2_msgs::msg::TFMessage odometry_transform_message_; bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; @@ -136,6 +138,7 @@ class DiffDriveController : public controller_interface::ChainableControllerInte std::shared_ptr> limited_velocity_publisher_ = nullptr; std::shared_ptr> realtime_limited_velocity_publisher_ = nullptr; + TwistStamped limited_velocity_message_; rclcpp::Time previous_update_timestamp_{0}; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index cb4f2edcee..e49fce44b5 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -231,24 +231,23 @@ controller_interface::return_type DiffDriveController::update_and_write_commands if (should_publish) { - if (realtime_odometry_publisher_->trylock()) + if (realtime_odometry_publisher_) { - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = time; - odometry_message.pose.pose.position.x = odometry_.getX(); - odometry_message.pose.pose.position.y = odometry_.getY(); - odometry_message.pose.pose.orientation.x = orientation.x(); - odometry_message.pose.pose.orientation.y = orientation.y(); - odometry_message.pose.pose.orientation.z = orientation.z(); - odometry_message.pose.pose.orientation.w = orientation.w(); - odometry_message.twist.twist.linear.x = odometry_.getLinear(); - odometry_message.twist.twist.angular.z = odometry_.getAngular(); - realtime_odometry_publisher_->unlockAndPublish(); + odometry_message_.header.stamp = time; + odometry_message_.pose.pose.position.x = odometry_.getX(); + odometry_message_.pose.pose.position.y = odometry_.getY(); + odometry_message_.pose.pose.orientation.x = orientation.x(); + odometry_message_.pose.pose.orientation.y = orientation.y(); + odometry_message_.pose.pose.orientation.z = orientation.z(); + odometry_message_.pose.pose.orientation.w = orientation.w(); + odometry_message_.twist.twist.linear.x = odometry_.getLinear(); + odometry_message_.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->try_publish(odometry_message_); } - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_) { - auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + auto & transform = odometry_transform_message_.transforms.front(); transform.header.stamp = time; transform.transform.translation.x = odometry_.getX(); transform.transform.translation.y = odometry_.getY(); @@ -256,7 +255,7 @@ controller_interface::return_type DiffDriveController::update_and_write_commands transform.transform.rotation.y = orientation.y(); transform.transform.rotation.z = orientation.z(); transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->unlockAndPublish(); + realtime_odometry_transform_publisher_->try_publish(odometry_transform_message_); } } @@ -271,17 +270,16 @@ controller_interface::return_type DiffDriveController::update_and_write_commands previous_two_commands_.push({{linear_command, angular_command}}); // Publish limited velocity - if (publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock()) - { - auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_; - limited_velocity_command.header.stamp = time; - limited_velocity_command.twist.linear.x = linear_command; - limited_velocity_command.twist.linear.y = 0.0; - limited_velocity_command.twist.linear.z = 0.0; - limited_velocity_command.twist.angular.x = 0.0; - limited_velocity_command.twist.angular.y = 0.0; - limited_velocity_command.twist.angular.z = angular_command; - realtime_limited_velocity_publisher_->unlockAndPublish(); + if (publish_limited_velocity_ && realtime_limited_velocity_publisher_) + { + limited_velocity_message_.header.stamp = time; + limited_velocity_message_.twist.linear.x = linear_command; + limited_velocity_message_.twist.linear.y = 0.0; + limited_velocity_message_.twist.linear.z = 0.0; + limited_velocity_message_.twist.angular.x = 0.0; + limited_velocity_message_.twist.angular.y = 0.0; + limited_velocity_message_.twist.angular.z = angular_command; + realtime_limited_velocity_publisher_->try_publish(limited_velocity_message_); } // Compute wheels velocities: @@ -469,10 +467,9 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( odometry_transform_publisher_); // keeping track of odom and base_link transforms only - auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; - odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = base_frame_id; + odometry_transform_message_.transforms.resize(1); + odometry_transform_message_.transforms.front().header.frame_id = odom_frame_id; + odometry_transform_message_.transforms.front().child_frame_id = base_frame_id; previous_update_timestamp_ = get_node()->get_clock()->now(); return controller_interface::CallbackReturn::SUCCESS; diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index 0c8ce438f1..64debae25f 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -71,6 +71,7 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr using StatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr sensor_state_publisher_; std::unique_ptr realtime_publisher_; + geometry_msgs::msg::WrenchStamped sensor_state_message_; }; } // namespace force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 6ddd94ca72..0185493ade 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -102,9 +102,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_publisher_->unlock(); + sensor_state_message_.header.frame_id = params_.frame_id; RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -146,13 +144,13 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write { param_listener_->try_get_params(params_); - if (realtime_publisher_ && realtime_publisher_->trylock()) + if (realtime_publisher_) { - realtime_publisher_->msg_.header.stamp = time; - force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); - this->apply_sensor_offset(params_, realtime_publisher_->msg_); - this->apply_sensor_multiplier(params_, realtime_publisher_->msg_); - realtime_publisher_->unlockAndPublish(); + sensor_state_message_.header.stamp = time; + force_torque_sensor_->get_values_as_message(sensor_state_message_.wrench); + this->apply_sensor_offset(params_, sensor_state_message_); + this->apply_sensor_multiplier(params_, sensor_state_message_); + realtime_publisher_->try_publish(sensor_state_message_); } return controller_interface::return_type::OK; diff --git a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp index f7d75e1c20..561abcce91 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_command_controller.hpp @@ -100,6 +100,7 @@ class GpioCommandController : public controller_interface::ControllerInterface std::shared_ptr> gpio_state_publisher_{}; std::shared_ptr> realtime_gpio_state_publisher_{}; + StateType gpio_state_msg_; std::shared_ptr param_listener_{}; gpio_command_controller_parameters::Params params_; diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp index 34222928c3..f2768f07cf 100644 --- a/gpio_controllers/src/gpio_command_controller.cpp +++ b/gpio_controllers/src/gpio_command_controller.cpp @@ -252,19 +252,18 @@ void GpioCommandController::store_state_interface_types() void GpioCommandController::initialize_gpio_state_msg() { - auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; - gpio_state_msg.header.stamp = get_node()->now(); - gpio_state_msg.interface_groups.resize(params_.gpios.size()); - gpio_state_msg.interface_values.resize(params_.gpios.size()); + gpio_state_msg_.header.stamp = get_node()->now(); + gpio_state_msg_.interface_groups.resize(params_.gpios.size()); + gpio_state_msg_.interface_values.resize(params_.gpios.size()); for (std::size_t gpio_index = 0; gpio_index < params_.gpios.size(); ++gpio_index) { const auto gpio_name = params_.gpios[gpio_index]; - gpio_state_msg.interface_groups[gpio_index] = gpio_name; - gpio_state_msg.interface_values[gpio_index].interface_names = + gpio_state_msg_.interface_groups[gpio_index] = gpio_name; + gpio_state_msg_.interface_values[gpio_index].interface_names = get_gpios_state_interfaces_names(gpio_name); - gpio_state_msg.interface_values[gpio_index].values = std::vector( - gpio_state_msg.interface_values[gpio_index].interface_names.size(), + gpio_state_msg_.interface_values[gpio_index].values = std::vector( + gpio_state_msg_.interface_values[gpio_index].interface_names.size(), std::numeric_limits::quiet_NaN()); } } @@ -391,24 +390,23 @@ void GpioCommandController::apply_command( void GpioCommandController::update_gpios_states() { - if (!realtime_gpio_state_publisher_ || !realtime_gpio_state_publisher_->trylock()) + if (!realtime_gpio_state_publisher_) { return; } - auto & gpio_state_msg = realtime_gpio_state_publisher_->msg_; - gpio_state_msg.header.stamp = get_node()->now(); - for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg.interface_groups.size(); + gpio_state_msg_.header.stamp = get_node()->now(); + for (std::size_t gpio_index = 0; gpio_index < gpio_state_msg_.interface_groups.size(); ++gpio_index) { for (std::size_t interface_index = 0; - interface_index < gpio_state_msg.interface_values[gpio_index].interface_names.size(); + interface_index < gpio_state_msg_.interface_values[gpio_index].interface_names.size(); ++interface_index) { - apply_state_value(gpio_state_msg, gpio_index, interface_index); + apply_state_value(gpio_state_msg_, gpio_index, interface_index); } } - realtime_gpio_state_publisher_->unlockAndPublish(); + realtime_gpio_state_publisher_->try_publish(gpio_state_msg_); } void GpioCommandController::apply_state_value( diff --git a/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp b/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp index 35c88073aa..2ef100af25 100644 --- a/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp +++ b/gps_sensor_broadcaster/include/gps_sensor_broadcaster/gps_sensor_broadcaster.hpp @@ -57,8 +57,11 @@ class GPSSensorBroadcaster : public controller_interface::ControllerInterface callback_return_type setup_publisher(); GPSSensorVariant gps_sensor_; + rclcpp::Publisher::SharedPtr sensor_state_publisher_; std::unique_ptr realtime_publisher_; + sensor_msgs::msg::NavSatFix state_message_; + std::shared_ptr param_listener_{}; gps_sensor_broadcaster::Params params_; std::vector state_names_; diff --git a/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp b/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp index 182eb0e87d..655364a6f6 100644 --- a/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp +++ b/gps_sensor_broadcaster/src/gps_sensor_broadcaster.cpp @@ -78,10 +78,8 @@ callback_return_type GPSSensorBroadcaster::setup_publisher() sensor_state_publisher_ = get_node()->create_publisher( "~/gps/fix", rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique(sensor_state_publisher_); - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; + state_message_.header.frame_id = params_.frame_id; setup_covariance(); - realtime_publisher_->unlock(); return callback_return_type::SUCCESS; } @@ -98,14 +96,14 @@ void GPSSensorBroadcaster::setup_covariance() { if (params_.read_covariance_from_interface) { - realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_DIAGONAL_KNOWN; + state_message_.position_covariance_type = COVARIANCE_TYPE_DIAGONAL_KNOWN; return; } for (size_t i = 0; i < COVARIANCE_3x3_SIZE; ++i) { - realtime_publisher_->msg_.position_covariance[i] = params_.static_position_covariance[i]; - realtime_publisher_->msg_.position_covariance_type = COVARIANCE_TYPE_KNOWN; + state_message_.position_covariance[i] = params_.static_position_covariance[i]; + state_message_.position_covariance_type = COVARIANCE_TYPE_KNOWN; } } @@ -148,19 +146,19 @@ callback_return_type GPSSensorBroadcaster::on_deactivate(const rclcpp_lifecycle: controller_interface::return_type GPSSensorBroadcaster::update( const rclcpp::Time &, const rclcpp::Duration &) { - if (realtime_publisher_ && realtime_publisher_->trylock()) + if (realtime_publisher_) { - realtime_publisher_->msg_.header.stamp = get_node()->now(); + state_message_.header.stamp = get_node()->now(); std::visit( Visitor{ [this](auto & sensor) { sensor_msgs::msg::NavSatFix message; - sensor.get_values_as_message(realtime_publisher_->msg_); + sensor.get_values_as_message(state_message_); }, [](std::monostate &) {}}, gps_sensor_); - realtime_publisher_->unlockAndPublish(); + realtime_publisher_->try_publish(state_message_); } return controller_interface::return_type::OK; } diff --git a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp index 01045b7510..3beea03605 100644 --- a/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/include/imu_sensor_broadcaster/imu_sensor_broadcaster.hpp @@ -61,6 +61,7 @@ class IMUSensorBroadcaster : public controller_interface::ControllerInterface using StatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr sensor_state_publisher_; std::unique_ptr realtime_publisher_; + sensor_msgs::msg::Imu state_message_; }; } // namespace imu_sensor_broadcaster diff --git a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp index 04a28e5368..8a41657fef 100644 --- a/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp @@ -62,18 +62,15 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure( return CallbackReturn::ERROR; } - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; + state_message_.header.frame_id = params_.frame_id; // convert double vector to fixed-size array in the message for (size_t i = 0; i < 9; ++i) { - realtime_publisher_->msg_.orientation_covariance[i] = params_.static_covariance_orientation[i]; - realtime_publisher_->msg_.angular_velocity_covariance[i] = - params_.static_covariance_angular_velocity[i]; - realtime_publisher_->msg_.linear_acceleration_covariance[i] = + state_message_.orientation_covariance[i] = params_.static_covariance_orientation[i]; + state_message_.angular_velocity_covariance[i] = params_.static_covariance_angular_velocity[i]; + state_message_.linear_acceleration_covariance[i] = params_.static_covariance_linear_acceleration[i]; } - realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; @@ -113,11 +110,11 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_deactivate( controller_interface::return_type IMUSensorBroadcaster::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - if (realtime_publisher_ && realtime_publisher_->trylock()) + if (realtime_publisher_) { - realtime_publisher_->msg_.header.stamp = time; - imu_sensor_->get_values_as_message(realtime_publisher_->msg_); - realtime_publisher_->unlockAndPublish(); + state_message_.header.stamp = time; + imu_sensor_->get_values_as_message(state_message_); + realtime_publisher_->try_publish(state_message_); } return controller_interface::return_type::OK; diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 85d2b93765..05d2334d0b 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -105,6 +105,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface std::shared_ptr> joint_state_publisher_; std::shared_ptr> realtime_joint_state_publisher_; + sensor_msgs::msg::JointState joint_state_msg_; // For the DynamicJointState format, we use a map to buffer values in for easier lookup // This allows to preserve whatever order or names/interfaces were initialized. @@ -113,6 +114,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface dynamic_joint_state_publisher_; std::shared_ptr> realtime_dynamic_joint_state_publisher_; + control_msgs::msg::DynamicJointState dynamic_joint_state_msg_; urdf::Model model_; bool is_model_loaded_ = false; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index b88971a3a6..bdcc1d1c82 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -175,11 +175,10 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( (params_.joints.empty() ? model_.joints_.size() : params_.joints.size()) + params_.extra_joints.size(); joint_names_.reserve(max_joints_size); - auto & joint_state_msg = realtime_joint_state_publisher_->msg_; - joint_state_msg.name.reserve(max_joints_size); - joint_state_msg.position.reserve(max_joints_size); - joint_state_msg.velocity.reserve(max_joints_size); - joint_state_msg.effort.reserve(max_joints_size); + joint_state_msg_.name.reserve(max_joints_size); + joint_state_msg_.position.reserve(max_joints_size); + joint_state_msg_.velocity.reserve(max_joints_size); + joint_state_msg_.effort.reserve(max_joints_size); frame_id_ = params_.frame_id; if (frame_id_.empty()) @@ -314,16 +313,15 @@ void JointStateBroadcaster::init_joint_state_msg() { const size_t num_joints = joint_names_.size(); - /// @note joint_state_msg publishes position, velocity and effort for all joints, + /// @note joint_state_msg_ publishes position, velocity and effort for all joints, /// with at least one of these interfaces, the rest are omitted from this message // default initialization for joint state message - auto & joint_state_msg = realtime_joint_state_publisher_->msg_; - joint_state_msg.header.frame_id = frame_id_; - joint_state_msg.name = joint_names_; - joint_state_msg.position.resize(num_joints, kUninitializedValue); - joint_state_msg.velocity.resize(num_joints, kUninitializedValue); - joint_state_msg.effort.resize(num_joints, kUninitializedValue); + joint_state_msg_.header.frame_id = frame_id_; + joint_state_msg_.name = joint_names_; + joint_state_msg_.position.resize(num_joints, kUninitializedValue); + joint_state_msg_.velocity.resize(num_joints, kUninitializedValue); + joint_state_msg_.effort.resize(num_joints, kUninitializedValue); // save joint state data auto get_address = @@ -352,36 +350,36 @@ void JointStateBroadcaster::init_joint_state_msg() void JointStateBroadcaster::init_dynamic_joint_state_msg() { - auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_; - dynamic_joint_state_msg.header.frame_id = frame_id_; - dynamic_joint_state_msg.joint_names.clear(); - dynamic_joint_state_msg.interface_values.clear(); + dynamic_joint_state_msg_.header.frame_id = frame_id_; + dynamic_joint_state_msg_.joint_names.clear(); + dynamic_joint_state_msg_.interface_values.clear(); for (const auto & name_ifv : name_if_value_mapping_) { const auto & name = name_ifv.first; const auto & interfaces_and_values = name_ifv.second; - dynamic_joint_state_msg.joint_names.push_back(name); + dynamic_joint_state_msg_.joint_names.push_back(name); control_msgs::msg::InterfaceValue if_value; for (const auto & interface_and_value : interfaces_and_values) { if_value.interface_names.emplace_back(interface_and_value.first); if_value.values.emplace_back(kUninitializedValue); } - dynamic_joint_state_msg.interface_values.emplace_back(if_value); + dynamic_joint_state_msg_.interface_values.emplace_back(if_value); } // save dynamic joint state data dynamic_joint_states_data_.clear(); - const auto & msg = realtime_dynamic_joint_state_publisher_->msg_; - for (auto ji = 0u; ji < msg.joint_names.size(); ++ji) + for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji) { dynamic_joint_states_data_.push_back(std::vector()); - const auto & name = msg.joint_names[ji]; + const auto & name = dynamic_joint_state_msg_.joint_names[ji]; - for (auto ii = 0u; ii < msg.interface_values[ji].interface_names.size(); ++ii) + for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size(); + ++ii) { - const auto & interface_name = msg.interface_values[ji].interface_names[ii]; + const auto & interface_name = + dynamic_joint_state_msg_.interface_values[ji].interface_names[ii]; dynamic_joint_states_data_[ji].push_back(&name_if_value_mapping_[name][interface_name]); } } @@ -421,34 +419,33 @@ controller_interface::return_type JointStateBroadcaster::update( } } - if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock()) + if (realtime_joint_state_publisher_) { - auto & joint_state_msg = realtime_joint_state_publisher_->msg_; - - joint_state_msg.header.stamp = time; + joint_state_msg_.header.stamp = time; // update joint state message and dynamic joint state message for (size_t i = 0; i < joint_names_.size(); ++i) { - joint_state_msg.position[i] = joint_states_data_[i].position_; - joint_state_msg.velocity[i] = joint_states_data_[i].velocity_; - joint_state_msg.effort[i] = joint_states_data_[i].effort_; + joint_state_msg_.position[i] = joint_states_data_[i].position_; + joint_state_msg_.velocity[i] = joint_states_data_[i].velocity_; + joint_state_msg_.effort[i] = joint_states_data_[i].effort_; } - realtime_joint_state_publisher_->unlockAndPublish(); + realtime_joint_state_publisher_->try_publish(joint_state_msg_); } - if (realtime_dynamic_joint_state_publisher_ && realtime_dynamic_joint_state_publisher_->trylock()) + if (realtime_dynamic_joint_state_publisher_) { - auto & msg = realtime_dynamic_joint_state_publisher_->msg_; - msg.header.stamp = time; - for (auto ji = 0u; ji < msg.joint_names.size(); ++ji) + dynamic_joint_state_msg_.header.stamp = time; + for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji) { - for (auto ii = 0u; ii < msg.interface_values[ji].interface_names.size(); ++ii) + for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size(); + ++ii) { - msg.interface_values[ji].values[ii] = *dynamic_joint_states_data_[ji][ii]; + dynamic_joint_state_msg_.interface_values[ji].values[ii] = + *dynamic_joint_states_data_[ji][ii]; } } - realtime_dynamic_joint_state_publisher_->unlockAndPublish(); + realtime_dynamic_joint_state_publisher_->try_publish(dynamic_joint_state_msg_); } return controller_interface::return_type::OK; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index fc3d430a02..8946f4ea55 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -177,7 +177,7 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); @@ -185,8 +185,7 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -229,7 +228,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); @@ -237,8 +236,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -276,7 +274,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & new_joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(new_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); @@ -284,8 +282,7 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces ASSERT_THAT(new_joint_state_msg.effort, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); // dynamic joint state initialized - const auto & new_dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & new_dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(new_dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); ASSERT_THAT( @@ -328,7 +325,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); @@ -336,8 +333,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -380,15 +376,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); @@ -435,15 +430,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDes ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized and it will have the data of all the interfaces - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); @@ -489,15 +483,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized and it will have the data of all the interfaces - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); @@ -542,15 +535,14 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized and it will have the data of all the interfaces - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); @@ -585,7 +577,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); @@ -593,8 +585,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -636,7 +627,7 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); @@ -652,8 +643,7 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -702,7 +692,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); @@ -714,8 +704,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -780,7 +769,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); @@ -794,8 +783,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -829,7 +817,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, SizeIs(0)); ASSERT_THAT(joint_state_msg.position, SizeIs(0)); @@ -837,8 +825,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) ASSERT_THAT(joint_state_msg.effort, SizeIs(0)); // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -876,7 +863,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); @@ -892,8 +879,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -938,8 +924,7 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate) } // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); @@ -1259,7 +1244,7 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) const size_t NUM_JOINTS = all_joint_names.size(); // joint state initialized - const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + const auto & joint_state_msg = state_broadcaster_->joint_state_msg_; ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(joint_state_msg.name, ElementsAreArray(all_joint_names)); ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); @@ -1267,8 +1252,7 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest) ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); // dynamic joint state initialized - const auto & dynamic_joint_state_msg = - state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + const auto & dynamic_joint_state_msg = state_broadcaster_->dynamic_joint_state_msg_; ASSERT_EQ(dynamic_joint_state_msg.header.frame_id, frame_id_); ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); } 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 7bea9552a8..05ecd43efc 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 @@ -182,6 +182,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa using StatePublisherPtr = std::unique_ptr; rclcpp::Publisher::SharedPtr publisher_; StatePublisherPtr state_publisher_; + ControllerStateMsg state_msg_; using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a55f6e2a9d..900dcf434e 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -946,42 +946,39 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "~/controller_state", rclcpp::SystemDefaultsQoS()); state_publisher_ = std::make_unique(publisher_); - state_publisher_->lock(); - state_publisher_->msg_.joint_names = params_.joints; - state_publisher_->msg_.reference.positions.resize(dof_); - state_publisher_->msg_.reference.velocities.resize(dof_); - state_publisher_->msg_.reference.accelerations.resize(dof_); - state_publisher_->msg_.feedback.positions.resize(dof_); - state_publisher_->msg_.error.positions.resize(dof_); + state_msg_.joint_names = params_.joints; + state_msg_.reference.positions.resize(dof_); + state_msg_.reference.velocities.resize(dof_); + state_msg_.reference.accelerations.resize(dof_); + state_msg_.feedback.positions.resize(dof_); + state_msg_.error.positions.resize(dof_); if (has_velocity_state_interface_) { - state_publisher_->msg_.feedback.velocities.resize(dof_); - state_publisher_->msg_.error.velocities.resize(dof_); + state_msg_.feedback.velocities.resize(dof_); + state_msg_.error.velocities.resize(dof_); } if (has_acceleration_state_interface_) { - state_publisher_->msg_.feedback.accelerations.resize(dof_); - state_publisher_->msg_.error.accelerations.resize(dof_); + state_msg_.feedback.accelerations.resize(dof_); + state_msg_.error.accelerations.resize(dof_); } if (has_position_command_interface_) { - state_publisher_->msg_.output.positions.resize(dof_); + state_msg_.output.positions.resize(dof_); } if (has_velocity_command_interface_) { - state_publisher_->msg_.output.velocities.resize(dof_); + state_msg_.output.velocities.resize(dof_); } if (has_acceleration_command_interface_) { - state_publisher_->msg_.output.accelerations.resize(dof_); + state_msg_.output.accelerations.resize(dof_); } if (has_effort_command_interface_) { - state_publisher_->msg_.output.effort.resize(dof_); + state_msg_.output.effort.resize(dof_); } - state_publisher_->unlock(); - // action server configuration if (params_.allow_partial_joints_goal) { @@ -1303,31 +1300,31 @@ void JointTrajectoryController::publish_state( const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error) { - if (state_publisher_->trylock()) - { - state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.reference.positions = desired_state.positions; - state_publisher_->msg_.reference.velocities = desired_state.velocities; - state_publisher_->msg_.reference.accelerations = desired_state.accelerations; - state_publisher_->msg_.feedback.positions = current_state.positions; - state_publisher_->msg_.error.positions = state_error.positions; + if (state_publisher_) + { + state_msg_.header.stamp = time; + state_msg_.reference.positions = desired_state.positions; + state_msg_.reference.velocities = desired_state.velocities; + state_msg_.reference.accelerations = desired_state.accelerations; + state_msg_.feedback.positions = current_state.positions; + state_msg_.error.positions = state_error.positions; if (has_velocity_state_interface_) { - state_publisher_->msg_.feedback.velocities = current_state.velocities; - state_publisher_->msg_.error.velocities = state_error.velocities; + state_msg_.feedback.velocities = current_state.velocities; + state_msg_.error.velocities = state_error.velocities; } if (has_acceleration_state_interface_) { - state_publisher_->msg_.feedback.accelerations = current_state.accelerations; - state_publisher_->msg_.error.accelerations = state_error.accelerations; + state_msg_.feedback.accelerations = current_state.accelerations; + state_msg_.error.accelerations = state_error.accelerations; } if (read_commands_from_command_interfaces(command_current_)) { - state_publisher_->msg_.output = command_current_; + state_msg_.output = command_current_; } - state_publisher_->msg_.speed_scaling_factor = scaling_factor_; + state_msg_.speed_scaling_factor = scaling_factor_; - state_publisher_->unlockAndPublish(); + state_publisher_->try_publish(state_msg_); } } diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index c0edc8c5a2..58b6de04b7 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -122,14 +122,17 @@ class MecanumDriveController : public controller_interface::ChainableControllerI using OdomStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr odom_s_publisher_; std::unique_ptr rt_odom_state_publisher_; + OdomStateMsg odom_state_msg_; using TfStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; std::unique_ptr rt_tf_odom_state_publisher_; + TfStateMsg tf_odom_state_msg_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr controller_s_publisher_; std::unique_ptr controller_state_publisher_; + ControllerStateMsg controller_state_msg_; // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index c320b258be..e468c8577b 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -167,14 +167,13 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( 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 = 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 & pose_covariance = rt_odom_state_publisher_->msg_.pose.covariance; - auto & twist_covariance = rt_odom_state_publisher_->msg_.twist.covariance; + odom_state_msg_.header.stamp = get_node()->now(); + odom_state_msg_.header.frame_id = odom_frame_id; + odom_state_msg_.child_frame_id = base_frame_id; + odom_state_msg_.pose.pose.position.z = 0; + + auto & pose_covariance = odom_state_msg_.pose.covariance; + auto & twist_covariance = odom_state_msg_.twist.covariance; constexpr size_t NUM_DIMENSIONS = 6; for (size_t index = 0; index < 6; ++index) { @@ -182,7 +181,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( pose_covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; twist_covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } - rt_odom_state_publisher_->unlock(); try { @@ -199,13 +197,11 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - 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 = 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(); + tf_odom_state_msg_.transforms.resize(1); + tf_odom_state_msg_.transforms[0].header.stamp = get_node()->now(); + tf_odom_state_msg_.transforms[0].header.frame_id = odom_frame_id; + tf_odom_state_msg_.transforms[0].child_frame_id = base_frame_id; + tf_odom_state_msg_.transforms[0].transform.translation.z = 0.0; try { @@ -225,10 +221,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - controller_state_publisher_->lock(); - controller_state_publisher_->msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.header.frame_id = odom_frame_id; - controller_state_publisher_->unlock(); + controller_state_msg_.header.stamp = get_node()->now(); + controller_state_msg_.header.frame_id = odom_frame_id; RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); @@ -524,42 +518,41 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma orientation.setRPY(0.0, 0.0, odometry_.getRz()); // Populate odom message and publish - if (rt_odom_state_publisher_->trylock()) + if (rt_odom_state_publisher_) { - rt_odom_state_publisher_->msg_.header.stamp = time; - rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.getX(); - rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.getY(); - rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); - rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.getVx(); - rt_odom_state_publisher_->msg_.twist.twist.linear.y = odometry_.getVy(); - rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.getWz(); - rt_odom_state_publisher_->unlockAndPublish(); + odom_state_msg_.header.stamp = time; + odom_state_msg_.pose.pose.position.x = odometry_.getX(); + odom_state_msg_.pose.pose.position.y = odometry_.getY(); + odom_state_msg_.pose.pose.orientation = tf2::toMsg(orientation); + odom_state_msg_.twist.twist.linear.x = odometry_.getVx(); + odom_state_msg_.twist.twist.linear.y = odometry_.getVy(); + odom_state_msg_.twist.twist.angular.z = odometry_.getWz(); + rt_odom_state_publisher_->try_publish(odom_state_msg_); } // Publish tf /odom frame - if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_) { - rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = odometry_.getX(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = odometry_.getY(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = - tf2::toMsg(orientation); - rt_tf_odom_state_publisher_->unlockAndPublish(); + tf_odom_state_msg_.transforms.front().header.stamp = time; + tf_odom_state_msg_.transforms.front().transform.translation.x = odometry_.getX(); + tf_odom_state_msg_.transforms.front().transform.translation.y = odometry_.getY(); + tf_odom_state_msg_.transforms.front().transform.rotation = tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->try_publish(tf_odom_state_msg_); } - if (controller_state_publisher_->trylock()) + if (controller_state_publisher_) { - controller_state_publisher_->msg_.header.stamp = get_node()->now(); + controller_state_msg_.header.stamp = get_node()->now(); - controller_state_publisher_->msg_.front_left_wheel_velocity = wheel_front_left_state_vel; - controller_state_publisher_->msg_.front_right_wheel_velocity = wheel_front_right_state_vel; - controller_state_publisher_->msg_.back_right_wheel_velocity = wheel_rear_right_state_vel; - controller_state_publisher_->msg_.back_left_wheel_velocity = wheel_rear_left_state_vel; + controller_state_msg_.front_left_wheel_velocity = wheel_front_left_state_vel; + controller_state_msg_.front_right_wheel_velocity = wheel_front_right_state_vel; + controller_state_msg_.back_right_wheel_velocity = wheel_rear_right_state_vel; + controller_state_msg_.back_left_wheel_velocity = wheel_rear_left_state_vel; - controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0]; - controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1]; - controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2]; - controller_state_publisher_->unlockAndPublish(); + controller_state_msg_.reference_velocity.linear.x = reference_interfaces_[0]; + controller_state_msg_.reference_velocity.linear.y = reference_interfaces_[1]; + controller_state_msg_.reference_velocity.angular.z = reference_interfaces_[2]; + controller_state_publisher_->try_publish(controller_state_msg_); } reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index c1466bf6f3..a1ec5fdba9 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -141,7 +141,7 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_na ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + auto odometry_message = controller_->odom_state_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 */ @@ -176,7 +176,7 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_nam ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + auto odometry_message = controller_->odom_state_msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; @@ -203,7 +203,7 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_na ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + auto odometry_message = controller_->odom_state_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 @@ -231,7 +231,7 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_set_n ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + auto odometry_message = controller_->odom_state_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 */ @@ -257,7 +257,7 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_set_na ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + auto odometry_message = controller_->odom_state_msg_; std::string test_odom_frame_id = odometry_message.header.frame_id; std::string test_base_frame_id = odometry_message.child_frame_id; @@ -285,7 +285,7 @@ TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_n ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto odometry_message = controller_->get_rt_odom_state_publisher_msg(); + auto odometry_message = controller_->odom_state_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) + "/"; diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 7d42c96c74..4d07dee7b2 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -84,6 +84,13 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD when_ref_timeout_zero_for_reference_callback_expect_reference_msg_being_used_only_once); FRIEND_TEST(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_set_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_set_namespace); + FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_namespace); + public: controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override @@ -129,16 +136,6 @@ 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 diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 12007297c0..2b08b9dc15 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -92,6 +92,7 @@ class PidController : public controller_interface::ChainableControllerInterface rclcpp::Publisher::SharedPtr s_publisher_; std::unique_ptr state_publisher_; + ControllerStateMsg state_msg_; // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 25cc136ab1..0831951aab 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -241,13 +241,11 @@ controller_interface::CallbackReturn PidController::on_configure( } // Reserve memory in state publisher - state_publisher_->lock(); - state_publisher_->msg_.dof_states.resize(reference_and_state_dof_names_.size()); + state_msg_.dof_states.resize(reference_and_state_dof_names_.size()); for (size_t i = 0; i < reference_and_state_dof_names_.size(); ++i) { - state_publisher_->msg_.dof_states[i].name = reference_and_state_dof_names_[i]; + state_msg_.dof_states[i].name = reference_and_state_dof_names_[i]; } - state_publisher_->unlock(); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -562,31 +560,30 @@ controller_interface::return_type PidController::update_and_write_commands( } } - if (state_publisher_ && state_publisher_->trylock()) + if (state_publisher_) { - state_publisher_->msg_.header.stamp = time; + state_msg_.header.stamp = time; for (size_t i = 0; i < dof_; ++i) { - state_publisher_->msg_.dof_states[i].reference = reference_interfaces_[i]; - state_publisher_->msg_.dof_states[i].feedback = measured_state_values_[i]; + state_msg_.dof_states[i].reference = reference_interfaces_[i]; + state_msg_.dof_states[i].feedback = measured_state_values_[i]; if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { - state_publisher_->msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i]; + state_msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i]; } - state_publisher_->msg_.dof_states[i].error = - reference_interfaces_[i] - measured_state_values_[i]; + state_msg_.dof_states[i].error = reference_interfaces_[i] - measured_state_values_[i]; if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) { // for continuous angles the error is normalized between -pimsg_.dof_states[i].error = + state_msg_.dof_states[i].error = angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]); } if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) { - state_publisher_->msg_.dof_states[i].error_dot = + state_msg_.dof_states[i].error_dot = reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i]; } - state_publisher_->msg_.dof_states[i].time_step = period.seconds(); + state_msg_.dof_states[i].time_step = period.seconds(); // Command can store the old calculated values. This should be obvious because at least one // another value is NaN. const auto command_interface_value_op = command_interfaces_[i].get_optional(); @@ -599,10 +596,10 @@ controller_interface::return_type PidController::update_and_write_commands( } else { - state_publisher_->msg_.dof_states[i].output = command_interface_value_op.value(); + state_msg_.dof_states[i].output = command_interface_value_op.value(); } } - state_publisher_->unlockAndPublish(); + state_publisher_->try_publish(state_msg_); } return controller_interface::return_type::OK; diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index f40e459509..8dbfcf4025 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -62,10 +62,12 @@ class PoseBroadcaster : public controller_interface::ControllerInterface rclcpp::Publisher::SharedPtr pose_publisher_; std::unique_ptr> realtime_publisher_; + geometry_msgs::msg::PoseStamped pose_msg_; rclcpp::Publisher::SharedPtr tf_publisher_; std::unique_ptr> realtime_tf_publisher_; + tf2_msgs::msg::TFMessage tf_msg_; }; } // namespace pose_broadcaster diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index a018d87ce3..8880a58d5b 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -105,17 +105,13 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( } // Initialize pose message - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_publisher_->unlock(); + pose_msg_.header.frame_id = params_.frame_id; // Initialize tf message if tf publishing is enabled if (realtime_tf_publisher_) { - realtime_tf_publisher_->lock(); - - realtime_tf_publisher_->msg_.transforms.resize(1); - auto & tf_transform = realtime_tf_publisher_->msg_.transforms.front(); + tf_msg_.transforms.resize(1); + auto & tf_transform = tf_msg_.transforms.front(); tf_transform.header.frame_id = params_.frame_id; if (params_.tf.child_frame_id.empty()) { @@ -125,8 +121,6 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( { tf_transform.child_frame_id = params_.tf.child_frame_id; } - - realtime_tf_publisher_->unlock(); } return controller_interface::CallbackReturn::SUCCESS; @@ -152,11 +146,11 @@ controller_interface::return_type PoseBroadcaster::update( geometry_msgs::msg::Pose pose; pose_sensor_->get_values_as_message(pose); - if (realtime_publisher_ && realtime_publisher_->trylock()) + if (realtime_publisher_) { - realtime_publisher_->msg_.header.stamp = time; - realtime_publisher_->msg_.pose = pose; - realtime_publisher_->unlockAndPublish(); + pose_msg_.header.stamp = time; + pose_msg_.pose = pose; + realtime_publisher_->try_publish(pose_msg_); } if (!is_pose_valid(pose)) { @@ -166,9 +160,9 @@ controller_interface::return_type PoseBroadcaster::update( pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); } - else if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + else if (realtime_tf_publisher_) { - auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; + auto & tf_transform = tf_msg_.transforms[0]; tf_transform.header.stamp = time; tf_transform.transform.translation.x = pose.position.x; @@ -180,7 +174,7 @@ controller_interface::return_type PoseBroadcaster::update( tf_transform.transform.rotation.z = pose.orientation.z; tf_transform.transform.rotation.w = pose.orientation.w; - realtime_tf_publisher_->unlockAndPublish(); + realtime_tf_publisher_->try_publish(tf_msg_); } return controller_interface::return_type::OK; diff --git a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp index 65a1d3092f..75862d0815 100644 --- a/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/include/range_sensor_broadcaster/range_sensor_broadcaster.hpp @@ -60,6 +60,7 @@ class RangeSensorBroadcaster : public controller_interface::ControllerInterface using StatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr sensor_state_publisher_; std::unique_ptr realtime_publisher_; + sensor_msgs::msg::Range range_msg_; }; } // namespace range_sensor_broadcaster diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp index 4ff817b2d3..80a8c26cf0 100644 --- a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp @@ -73,17 +73,15 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure( return CallbackReturn::ERROR; } - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = params_.frame_id; - realtime_publisher_->msg_.radiation_type = static_cast(params_.radiation_type); - realtime_publisher_->msg_.field_of_view = static_cast(params_.field_of_view); - realtime_publisher_->msg_.min_range = static_cast(params_.min_range); - realtime_publisher_->msg_.max_range = static_cast(params_.max_range); + range_msg_.header.frame_id = params_.frame_id; + range_msg_.radiation_type = static_cast(params_.radiation_type); + range_msg_.field_of_view = static_cast(params_.field_of_view); + range_msg_.min_range = static_cast(params_.min_range); + range_msg_.max_range = static_cast(params_.max_range); // \note The versions conditioning is added here to support the source-compatibility with Humble #if SENSOR_MSGS_VERSION_MAJOR >= 5 - realtime_publisher_->msg_.variance = params_.variance; + range_msg_.variance = params_.variance; #endif - realtime_publisher_->unlock(); RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; @@ -123,11 +121,11 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_deactivate( controller_interface::return_type RangeSensorBroadcaster::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - if (realtime_publisher_ && realtime_publisher_->trylock()) + if (realtime_publisher_) { - realtime_publisher_->msg_.header.stamp = time; - range_sensor_->get_values_as_message(realtime_publisher_->msg_); - realtime_publisher_->unlockAndPublish(); + range_msg_.header.stamp = time; + range_sensor_->get_values_as_message(range_msg_); + realtime_publisher_->try_publish(range_msg_); } return controller_interface::return_type::OK; diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 83057652a5..6deabe8237 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -96,7 +96,9 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl rclcpp::Publisher::SharedPtr tf_odom_s_publisher_; std::unique_ptr rt_odom_state_publisher_; + ControllerStateMsgOdom odom_state_msg_; std::unique_ptr rt_tf_odom_state_publisher_; + ControllerStateMsgTf tf_odom_state_msg_; // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; @@ -111,6 +113,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl using ControllerStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr controller_s_publisher_; std::unique_ptr controller_state_publisher_; + SteeringControllerStateMsg controller_state_msg_; // name constants for state interfaces size_t nr_state_itfs_; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index be67eb9cdd..dd57971f30 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -275,13 +275,12 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } - 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_.pose.pose.position.z = 0; + odom_state_msg_.header.stamp = get_node()->now(); + odom_state_msg_.header.frame_id = params_.odom_frame_id; + odom_state_msg_.child_frame_id = params_.base_frame_id; + odom_state_msg_.pose.pose.position.z = 0; - auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance; + auto & covariance = odom_state_msg_.twist.covariance; constexpr size_t NUM_DIMENSIONS = 6; for (size_t index = 0; index < 6; ++index) { @@ -290,7 +289,6 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } - rt_odom_state_publisher_->unlock(); try { @@ -308,13 +306,11 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } - 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].transform.translation.z = 0.0; - rt_tf_odom_state_publisher_->unlock(); + tf_odom_state_msg_.transforms.resize(1); + tf_odom_state_msg_.transforms[0].header.stamp = get_node()->now(); + tf_odom_state_msg_.transforms[0].header.frame_id = params_.odom_frame_id; + tf_odom_state_msg_.transforms[0].child_frame_id = params_.base_frame_id; + tf_odom_state_msg_.transforms[0].transform.translation.z = 0.0; try { @@ -332,10 +328,8 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::ERROR; } - 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_->unlock(); + controller_state_msg_.header.stamp = get_node()->now(); + controller_state_msg_.header.frame_id = params_.odom_frame_id; RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -566,38 +560,35 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c orientation.setRPY(0.0, 0.0, odometry_.get_heading()); // Populate odom message and publish - if (rt_odom_state_publisher_->trylock()) + if (rt_odom_state_publisher_) { - rt_odom_state_publisher_->msg_.header.stamp = time; - rt_odom_state_publisher_->msg_.pose.pose.position.x = odometry_.get_x(); - rt_odom_state_publisher_->msg_.pose.pose.position.y = odometry_.get_y(); - rt_odom_state_publisher_->msg_.pose.pose.orientation = tf2::toMsg(orientation); - rt_odom_state_publisher_->msg_.twist.twist.linear.x = odometry_.get_linear(); - rt_odom_state_publisher_->msg_.twist.twist.angular.z = odometry_.get_angular(); - rt_odom_state_publisher_->unlockAndPublish(); + odom_state_msg_.header.stamp = time; + odom_state_msg_.pose.pose.position.x = odometry_.get_x(); + odom_state_msg_.pose.pose.position.y = odometry_.get_y(); + odom_state_msg_.pose.pose.orientation = tf2::toMsg(orientation); + odom_state_msg_.twist.twist.linear.x = odometry_.get_linear(); + odom_state_msg_.twist.twist.angular.z = odometry_.get_angular(); + rt_odom_state_publisher_->try_publish(odom_state_msg_); } // Publish tf /odom frame - if (params_.enable_odom_tf && rt_tf_odom_state_publisher_->trylock()) + if (params_.enable_odom_tf && rt_tf_odom_state_publisher_) { - rt_tf_odom_state_publisher_->msg_.transforms.front().header.stamp = time; - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.x = - odometry_.get_x(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.translation.y = - odometry_.get_y(); - rt_tf_odom_state_publisher_->msg_.transforms.front().transform.rotation = - tf2::toMsg(orientation); - rt_tf_odom_state_publisher_->unlockAndPublish(); + tf_odom_state_msg_.transforms.front().header.stamp = time; + tf_odom_state_msg_.transforms.front().transform.translation.x = odometry_.get_x(); + tf_odom_state_msg_.transforms.front().transform.translation.y = odometry_.get_y(); + tf_odom_state_msg_.transforms.front().transform.rotation = tf2::toMsg(orientation); + rt_tf_odom_state_publisher_->try_publish(tf_odom_state_msg_); } - if (controller_state_publisher_->trylock()) + if (controller_state_publisher_) { - controller_state_publisher_->msg_.header.stamp = time; - controller_state_publisher_->msg_.traction_wheels_position.clear(); - controller_state_publisher_->msg_.traction_wheels_velocity.clear(); - controller_state_publisher_->msg_.traction_command.clear(); - controller_state_publisher_->msg_.steer_positions.clear(); - controller_state_publisher_->msg_.steering_angle_command.clear(); + controller_state_msg_.header.stamp = time; + controller_state_msg_.traction_wheels_position.clear(); + controller_state_msg_.traction_wheels_velocity.clear(); + controller_state_msg_.traction_command.clear(); + controller_state_msg_.steer_positions.clear(); + controller_state_msg_.steering_angle_command.clear(); auto number_of_traction_wheels = params_.traction_joints_names.size(); auto number_of_steering_wheels = params_.steering_joints_names.size(); @@ -614,7 +605,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { - controller_state_publisher_->msg_.traction_wheels_position.push_back( + controller_state_msg_.traction_wheels_position.push_back( position_state_interface_op.value()); } } @@ -628,7 +619,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { - controller_state_publisher_->msg_.traction_wheels_velocity.push_back( + controller_state_msg_.traction_wheels_velocity.push_back( velocity_state_interface_op.value()); } } @@ -640,8 +631,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { - controller_state_publisher_->msg_.traction_command.push_back( - velocity_command_interface_op.value()); + controller_state_msg_.traction_command.push_back(velocity_command_interface_op.value()); } } @@ -661,14 +651,12 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c } else { - controller_state_publisher_->msg_.steer_positions.push_back( - state_interface_value_op.value()); - controller_state_publisher_->msg_.steering_angle_command.push_back( - command_interface_value_op.value()); + controller_state_msg_.steer_positions.push_back(state_interface_value_op.value()); + controller_state_msg_.steering_angle_command.push_back(command_interface_value_op.value()); } } - controller_state_publisher_->unlockAndPublish(); + controller_state_publisher_->try_publish(controller_state_msg_); } reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 60590012b9..004f7d867b 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -106,17 +106,20 @@ class TricycleController : public controller_interface::ControllerInterface std::shared_ptr> ackermann_command_publisher_ = nullptr; std::shared_ptr> realtime_ackermann_command_publisher_ = nullptr; + AckermannDrive ackermann_command_msg_; Odometry odometry_; std::shared_ptr> odometry_publisher_ = nullptr; std::shared_ptr> realtime_odometry_publisher_ = nullptr; + nav_msgs::msg::Odometry odometry_msg_; std::shared_ptr> odometry_transform_publisher_ = nullptr; std::shared_ptr> realtime_odometry_transform_publisher_ = nullptr; + tf2_msgs::msg::TFMessage tf_msg_; // Timeout to consider cmd_vel commands old std::chrono::milliseconds cmd_vel_timeout_{500}; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 380a70cf37..aef62e7b0f 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -141,27 +141,26 @@ controller_interface::return_type TricycleController::update( tf2::Quaternion orientation; orientation.setRPY(0.0, 0.0, odometry_.getHeading()); - if (realtime_odometry_publisher_->trylock()) + if (realtime_odometry_publisher_) { - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.stamp = time; + odometry_msg_.header.stamp = time; if (!params_.odom_only_twist) { - odometry_message.pose.pose.position.x = odometry_.getX(); - odometry_message.pose.pose.position.y = odometry_.getY(); - odometry_message.pose.pose.orientation.x = orientation.x(); - odometry_message.pose.pose.orientation.y = orientation.y(); - odometry_message.pose.pose.orientation.z = orientation.z(); - odometry_message.pose.pose.orientation.w = orientation.w(); + odometry_msg_.pose.pose.position.x = odometry_.getX(); + odometry_msg_.pose.pose.position.y = odometry_.getY(); + odometry_msg_.pose.pose.orientation.x = orientation.x(); + odometry_msg_.pose.pose.orientation.y = orientation.y(); + odometry_msg_.pose.pose.orientation.z = orientation.z(); + odometry_msg_.pose.pose.orientation.w = orientation.w(); } - odometry_message.twist.twist.linear.x = odometry_.getLinear(); - odometry_message.twist.twist.angular.z = odometry_.getAngular(); - realtime_odometry_publisher_->unlockAndPublish(); + odometry_msg_.twist.twist.linear.x = odometry_.getLinear(); + odometry_msg_.twist.twist.angular.z = odometry_.getAngular(); + realtime_odometry_publisher_->try_publish(odometry_msg_); } - if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock()) + if (params_.enable_odom_tf && realtime_odometry_transform_publisher_) { - auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front(); + auto & transform = tf_msg_.transforms.front(); transform.header.stamp = time; transform.transform.translation.x = odometry_.getX(); transform.transform.translation.y = odometry_.getY(); @@ -169,7 +168,7 @@ controller_interface::return_type TricycleController::update( transform.transform.rotation.y = orientation.y(); transform.transform.rotation.z = orientation.z(); transform.transform.rotation.w = orientation.w(); - realtime_odometry_transform_publisher_->unlockAndPublish(); + realtime_odometry_transform_publisher_->try_publish(tf_msg_); } // Compute wheel velocity and angle @@ -212,14 +211,13 @@ controller_interface::return_type TricycleController::update( previous_commands_.emplace(ackermann_command); // Publish ackermann command - if (params_.publish_ackermann_command && realtime_ackermann_command_publisher_->trylock()) + if (params_.publish_ackermann_command && realtime_ackermann_command_publisher_) { - auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_; // speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel // speed (rad/s) - realtime_ackermann_command.speed = static_cast(Ws_write); - realtime_ackermann_command.steering_angle = static_cast(alpha_write); - realtime_ackermann_command_publisher_->unlockAndPublish(); + ackermann_command_msg_.speed = static_cast(Ws_write); + ackermann_command_msg_.steering_angle = static_cast(alpha_write); + realtime_ackermann_command_publisher_->try_publish(ackermann_command_msg_); } if (!traction_joint_[0].velocity_command.get().set_value(Ws_write)) @@ -332,12 +330,11 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & std::make_shared>( odometry_publisher_); - auto & odometry_message = realtime_odometry_publisher_->msg_; - odometry_message.header.frame_id = params_.odom_frame_id; - odometry_message.child_frame_id = params_.base_frame_id; + odometry_msg_.header.frame_id = params_.odom_frame_id; + odometry_msg_.child_frame_id = params_.base_frame_id; // initialize odom values zeros - odometry_message.twist = + odometry_msg_.twist = geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL); constexpr size_t NUM_DIMENSIONS = 6; @@ -345,8 +342,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & { // 0, 7, 14, 21, 28, 35 const size_t diagonal_index = NUM_DIMENSIONS * index + index; - odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; - odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; + odometry_msg_.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index]; + odometry_msg_.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index]; } // initialize transform publisher and message @@ -359,10 +356,9 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_transform_publisher_); // keeping track of odom and base_link transforms only - auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_; - odometry_transform_message.transforms.resize(1); - odometry_transform_message.transforms.front().header.frame_id = params_.odom_frame_id; - odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id; + tf_msg_.transforms.resize(1); + tf_msg_.transforms.front().header.frame_id = params_.odom_frame_id; + tf_msg_.transforms.front().child_frame_id = params_.base_frame_id; } // Create odom reset service