Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(controller-server): publish_zero_velocity parameter jazzy backport #4687

Merged
merged 2 commits into from
Sep 25, 2024
Merged

feat(controller-server): publish_zero_velocity parameter jazzy backport #4687

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f72b994
feat(controller-server): `publish_zero_velocity` parameter (#4675)
reinzor Sep 24, 2024
d976db1
Update nav2_controller/src/controller_server.cpp
reinzor Sep 25, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 25 additions & 11 deletions nav2_controller/src/controller_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options)

declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0));
declare_parameter("use_realtime_priority", rclcpp::ParameterValue(false));
declare_parameter("publish_zero_velocity", rclcpp::ParameterValue(true));

// The costmap node is used in the implementation of the controller
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
Expand Down Expand Up @@ -292,7 +293,18 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
*/
costmap_ros_->deactivate();

publishZeroVelocity();
// Always publish a zero velocity when deactivating the controller server
geometry_msgs::msg::TwistStamped velocity;
reinzor marked this conversation as resolved.
Show resolved Hide resolved
velocity.twist.angular.x = 0;
velocity.twist.angular.y = 0;
velocity.twist.angular.z = 0;
velocity.twist.linear.x = 0;
velocity.twist.linear.y = 0;
velocity.twist.linear.z = 0;
velocity.header.frame_id = costmap_ros_->getBaseFrameID();
velocity.header.stamp = now();
publishVelocity(velocity);

vel_publisher_->on_deactivate();

remove_on_set_parameters_callback(dyn_params_handler_.get());
Expand Down Expand Up @@ -719,16 +731,18 @@ void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped &

void ControllerServer::publishZeroVelocity()
{
geometry_msgs::msg::TwistStamped velocity;
velocity.twist.angular.x = 0;
velocity.twist.angular.y = 0;
velocity.twist.angular.z = 0;
velocity.twist.linear.x = 0;
velocity.twist.linear.y = 0;
velocity.twist.linear.z = 0;
velocity.header.frame_id = costmap_ros_->getBaseFrameID();
velocity.header.stamp = now();
publishVelocity(velocity);
if (get_parameter("publish_zero_velocity").as_bool()) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You can store the variable in the class, that's OK

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

According to https://www.ros.org/reps/rep-0009.html:

You cannot...
  Add new non-static data members, even if they are protected or private

Also the get_parameter is a map lookup so should be fast.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I suppose I haven't enforced that part of ABI, but perhaps I should be -- it would just make direct backporting any new feature virtually impossible since they would likely have some parameter set tied to them. I enforce API stability and behavior stability (i.e. not changing defaults or fundamentally changing the behavior on a module mid-distribution), but I think there's an argument that I should by enforcing stricter ABI as well. Though, its never come up and no one's had an issue.

This is fine.

geometry_msgs::msg::TwistStamped velocity;
velocity.twist.angular.x = 0;
velocity.twist.angular.y = 0;
velocity.twist.angular.z = 0;
velocity.twist.linear.x = 0;
velocity.twist.linear.y = 0;
velocity.twist.linear.z = 0;
velocity.header.frame_id = costmap_ros_->getBaseFrameID();
velocity.header.stamp = now();
publishVelocity(velocity);
}

// Reset the state of the controllers after the task has ended
ControllerMap::iterator it;
Expand Down
Loading