Skip to content

Commit

Permalink
Implement new ~/controller_state message (#578)
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed May 2, 2023
1 parent 4e8f2f7 commit c01b121
Show file tree
Hide file tree
Showing 5 changed files with 348 additions and 44 deletions.
2 changes: 1 addition & 1 deletion joint_trajectory_controller/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,7 @@ ROS2 interface of the controller
~/joint_trajectory (input topic) [trajectory_msgs::msg::JointTrajectory]
Topic for commanding the controller.

~/state (output topic) [control_msgs::msg::JointTrajectoryControllerState]
~/controller_state (output topic) [control_msgs::msg::JointTrajectoryControllerState]
Topic publishing internal states.

~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory]
Expand Down
6 changes: 6 additions & 0 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

// Preallocate variables used in the realtime update() function
trajectory_msgs::msg::JointTrajectoryPoint state_current_;
trajectory_msgs::msg::JointTrajectoryPoint command_current_;
trajectory_msgs::msg::JointTrajectoryPoint state_desired_;
trajectory_msgs::msg::JointTrajectoryPoint state_error_;

Expand Down Expand Up @@ -186,6 +187,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState;
using StatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
using StatePublisherPtr = std::unique_ptr<StatePublisher>;
rclcpp::Publisher<ControllerStateMsg>::SharedPtr publisher_legacy_;
StatePublisherPtr state_publisher_legacy_;
rclcpp::Publisher<ControllerStateMsg>::SharedPtr publisher_;
StatePublisherPtr state_publisher_;

Expand Down Expand Up @@ -255,13 +258,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
void read_state_from_hardware(JointTrajectoryPoint & state);

bool read_state_from_command_interfaces(JointTrajectoryPoint & state);
bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands);

private:
bool contains_interface_type(
const std::vector<std::string> & interface_type_list, const std::string & interface_type);

void resize_joint_trajectory_point(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
void resize_joint_trajectory_point_command(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
};

} // namespace joint_trajectory_controller
Expand Down
196 changes: 183 additions & 13 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -470,6 +470,81 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
return has_values;
}

bool JointTrajectoryController::read_commands_from_command_interfaces(
JointTrajectoryPoint & commands)
{
bool has_values = true;

auto assign_point_from_interface =
[&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
{
for (size_t index = 0; index < dof_; ++index)
{
trajectory_point_interface[index] = joint_interface[index].get().get_value();
}
};

auto interface_has_values = [](const auto & joint_interface)
{
return std::find_if(
joint_interface.begin(), joint_interface.end(),
[](const auto & interface)
{ return std::isnan(interface.get().get_value()); }) == joint_interface.end();
};

// Assign values from the command interfaces as command.
if (has_position_command_interface_)
{
if (interface_has_values(joint_command_interface_[0]))
{
assign_point_from_interface(commands.positions, joint_command_interface_[0]);
}
else
{
commands.positions.clear();
has_values = false;
}
}
if (has_velocity_command_interface_)
{
if (interface_has_values(joint_command_interface_[1]))
{
assign_point_from_interface(commands.velocities, joint_command_interface_[1]);
}
else
{
commands.velocities.clear();
has_values = false;
}
}
if (has_acceleration_command_interface_)
{
if (interface_has_values(joint_command_interface_[2]))
{
assign_point_from_interface(commands.accelerations, joint_command_interface_[2]);
}
else
{
commands.accelerations.clear();
has_values = false;
}
}
if (has_effort_command_interface_)
{
if (interface_has_values(joint_command_interface_[3]))
{
assign_point_from_interface(commands.effort, joint_command_interface_[3]);
}
else
{
commands.effort.clear();
has_values = false;
}
}

return has_values;
}

controller_interface::CallbackReturn JointTrajectoryController::on_configure(
const rclcpp_lifecycle::State &)
{
Expand Down Expand Up @@ -686,27 +761,66 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
state_publisher_period_ = rclcpp::Duration::from_seconds(0.0);
}

publisher_ =
publisher_legacy_ =
get_node()->create_publisher<ControllerStateMsg>("~/state", rclcpp::SystemDefaultsQoS());
state_publisher_legacy_ = std::make_unique<StatePublisher>(publisher_legacy_);

state_publisher_legacy_->lock();
state_publisher_legacy_->msg_.joint_names = params_.joints;
state_publisher_legacy_->msg_.desired.positions.resize(dof_);
state_publisher_legacy_->msg_.desired.velocities.resize(dof_);
state_publisher_legacy_->msg_.desired.accelerations.resize(dof_);
state_publisher_legacy_->msg_.actual.positions.resize(dof_);
state_publisher_legacy_->msg_.error.positions.resize(dof_);
if (has_velocity_state_interface_)
{
state_publisher_legacy_->msg_.actual.velocities.resize(dof_);
state_publisher_legacy_->msg_.error.velocities.resize(dof_);
}
if (has_acceleration_state_interface_)
{
state_publisher_legacy_->msg_.actual.accelerations.resize(dof_);
state_publisher_legacy_->msg_.error.accelerations.resize(dof_);
}
state_publisher_legacy_->unlock();

publisher_ = get_node()->create_publisher<ControllerStateMsg>(
"~/controller_state", rclcpp::SystemDefaultsQoS());
state_publisher_ = std::make_unique<StatePublisher>(publisher_);

state_publisher_->lock();
state_publisher_->msg_.joint_names = params_.joints;
state_publisher_->msg_.desired.positions.resize(dof_);
state_publisher_->msg_.desired.velocities.resize(dof_);
state_publisher_->msg_.desired.accelerations.resize(dof_);
state_publisher_->msg_.actual.positions.resize(dof_);
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_);
if (has_velocity_state_interface_)
{
state_publisher_->msg_.actual.velocities.resize(dof_);
state_publisher_->msg_.feedback.velocities.resize(dof_);
state_publisher_->msg_.error.velocities.resize(dof_);
}
if (has_acceleration_state_interface_)
{
state_publisher_->msg_.actual.accelerations.resize(dof_);
state_publisher_->msg_.feedback.accelerations.resize(dof_);
state_publisher_->msg_.error.accelerations.resize(dof_);
}
if (has_position_command_interface_)
{
state_publisher_->msg_.output.positions.resize(dof_);
}
if (has_velocity_command_interface_)
{
state_publisher_->msg_.output.velocities.resize(dof_);
}
if (has_acceleration_command_interface_)
{
state_publisher_->msg_.output.accelerations.resize(dof_);
}
if (has_effort_command_interface_)
{
state_publisher_->msg_.output.effort.resize(dof_);
}
state_publisher_->unlock();

last_state_publish_time_ = get_node()->now();
Expand All @@ -731,6 +845,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1));

resize_joint_trajectory_point(state_current_, dof_);
resize_joint_trajectory_point_command(command_current_, dof_);
resize_joint_trajectory_point(state_desired_, dof_);
resize_joint_trajectory_point(state_error_, dof_);
resize_joint_trajectory_point(last_commanded_state_, dof_);
Expand Down Expand Up @@ -915,25 +1030,59 @@ void JointTrajectoryController::publish_state(
return;
}

if (state_publisher_legacy_ && state_publisher_legacy_->trylock())
{
last_state_publish_time_ = get_node()->now();
state_publisher_legacy_->msg_.header.stamp = last_state_publish_time_;
state_publisher_legacy_->msg_.desired.positions = desired_state.positions;
state_publisher_legacy_->msg_.desired.velocities = desired_state.velocities;
state_publisher_legacy_->msg_.desired.accelerations = desired_state.accelerations;
state_publisher_legacy_->msg_.actual.positions = current_state.positions;
state_publisher_legacy_->msg_.error.positions = state_error.positions;
if (has_velocity_state_interface_)
{
state_publisher_legacy_->msg_.actual.velocities = current_state.velocities;
state_publisher_legacy_->msg_.error.velocities = state_error.velocities;
}
if (has_acceleration_state_interface_)
{
state_publisher_legacy_->msg_.actual.accelerations = current_state.accelerations;
state_publisher_legacy_->msg_.error.accelerations = state_error.accelerations;
}

state_publisher_legacy_->unlockAndPublish();

if (publisher_legacy_->get_subscription_count())
{
RCLCPP_WARN_THROTTLE(
get_node()->get_logger(), *get_node()->get_clock(), 1000,
"Subscription to deprecated ~/state topic. Use ~/controller_state instead.");
}
}

if (state_publisher_ && state_publisher_->trylock())
{
last_state_publish_time_ = get_node()->now();
state_publisher_->msg_.header.stamp = last_state_publish_time_;
state_publisher_->msg_.desired.positions = desired_state.positions;
state_publisher_->msg_.desired.velocities = desired_state.velocities;
state_publisher_->msg_.desired.accelerations = desired_state.accelerations;
state_publisher_->msg_.actual.positions = current_state.positions;
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 (has_velocity_state_interface_)
{
state_publisher_->msg_.actual.velocities = current_state.velocities;
state_publisher_->msg_.feedback.velocities = current_state.velocities;
state_publisher_->msg_.error.velocities = state_error.velocities;
}
if (has_acceleration_state_interface_)
{
state_publisher_->msg_.actual.accelerations = current_state.accelerations;
state_publisher_->msg_.feedback.accelerations = current_state.accelerations;
state_publisher_->msg_.error.accelerations = state_error.accelerations;
}
if (read_commands_from_command_interfaces(command_current_))
{
state_publisher_->msg_.output = command_current_;
}

state_publisher_->unlockAndPublish();
}
Expand Down Expand Up @@ -1305,6 +1454,27 @@ void JointTrajectoryController::resize_joint_trajectory_point(
}
}

void JointTrajectoryController::resize_joint_trajectory_point_command(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size)
{
if (has_position_command_interface_)
{
point.positions.resize(size, 0.0);
}
if (has_velocity_command_interface_)
{
point.velocities.resize(size, 0.0);
}
if (has_acceleration_command_interface_)
{
point.accelerations.resize(size, 0.0);
}
if (has_effort_command_interface_)
{
point.effort.resize(size, 0.0);
}
}

} // namespace joint_trajectory_controller

#include "pluginlib/class_list_macros.hpp"
Expand Down
Loading

0 comments on commit c01b121

Please sign in to comment.