Skip to content

Commit

Permalink
[JTC] Implement new ~/controller_state message (#557)
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed Apr 28, 2023
1 parent cf27231 commit edd494c
Show file tree
Hide file tree
Showing 5 changed files with 251 additions and 74 deletions.
2 changes: 1 addition & 1 deletion joint_trajectory_controller/doc/userdoc.rst
View file
Expand Up @@ -230,7 +230,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 with the update-rate of the controller manager.

~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory]
Expand Down
4 changes: 4 additions & 0 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Expand Up @@ -127,6 +127,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 @@ -256,6 +257,7 @@ 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);

void query_state_service(
const std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Request> request,
Expand All @@ -267,6 +269,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa

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
146 changes: 132 additions & 14 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Expand Up @@ -467,6 +467,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;
}

void JointTrajectoryController::query_state_service(
const std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Request> request,
std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Response> response)
Expand Down Expand Up @@ -706,27 +781,44 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
"~/joint_trajectory", rclcpp::SystemDefaultsQoS(),
std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1));

publisher_ =
get_node()->create_publisher<ControllerStateMsg>("~/state", rclcpp::SystemDefaultsQoS());
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();

// action server configuration
Expand All @@ -749,6 +841,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 @@ -929,21 +1022,25 @@ void JointTrajectoryController::publish_state(
if (state_publisher_->trylock())
{
state_publisher_->msg_.header.stamp = 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 @@ -1315,6 +1412,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

0 comments on commit edd494c

Please sign in to comment.