Skip to content

Commit 2e311c2

Browse files
SteveMacenskiSteveMacenski
committed
twist add stamps and properly propogated
1 parent da53aa2 commit 2e311c2

6 files changed

Lines changed: 17 additions & 9 deletions

File tree

‎nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -127,6 +127,8 @@ class DriveOnHeading : public TimedBehavior<ActionT>
127127
}
128128

129129
auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
130+
cmd_vel->header.stamp = this->clock_->now();
131+
cmd_vel->header.frame_id = this->robot_base_frame_;
130132
cmd_vel->twist.linear.y = 0.0;
131133
cmd_vel->twist.angular.z = 0.0;
132134
cmd_vel->twist.linear.x = command_speed_;

‎nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -291,8 +291,8 @@ class TimedBehavior : public nav2_core::Behavior
291291
void stopRobot()
292292
{
293293
auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
294-
cmd_vel->header.frame_id = "base_link";
295-
cmd_vel->header.stamp = rclcpp::Time();
294+
cmd_vel->header.frame_id = robot_base_frame_;
295+
cmd_vel->header.stamp = clock_->now();
296296
cmd_vel->twist.linear.x = 0.0;
297297
cmd_vel->twist.linear.y = 0.0;
298298
cmd_vel->twist.angular.z = 0.0;

‎nav2_behaviors/plugins/spin.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -139,6 +139,8 @@ ResultStatus Spin::onCycleUpdate()
139139
vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
140140

141141
auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
142+
cmd_vel->header.frame_id = robot_base_frame_;
143+
cmd_vel->header.stamp = clock_->now();
142144
cmd_vel->twist.angular.z = copysign(vel, cmd_yaw_);
143145

144146
geometry_msgs::msg::Pose2D pose2d;

‎nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -104,8 +104,9 @@ class CollisionMonitor : public nav2_util::LifecycleNode
104104
* @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds,
105105
* quit to publish 0-velocity.
106106
* @param robot_action Robot action to publish
107+
* @param header TwistStamped header to use
107108
*/
108-
void publishVelocity(const Action & robot_action);
109+
void publishVelocity(const Action & robot_action, const std_msgs::msg::Header & header);
109110

110111
/**
111112
* @brief Supporting routine obtaining all ROS-parameters
@@ -149,8 +150,9 @@ class CollisionMonitor : public nav2_util::LifecycleNode
149150
/**
150151
* @brief Main processing routine
151152
* @param cmd_vel_in Input desired robot velocity
153+
* @param header Twist header
152154
*/
153-
void process(const Velocity & cmd_vel_in);
155+
void process(const Velocity & cmd_vel_in, const std_msgs::msg::Header & header);
154156

155157
/**
156158
* @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type

‎nav2_collision_monitor/src/collision_monitor_node.cpp‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -191,7 +191,7 @@ void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped:
191191
return;
192192
}
193193

194-
process({msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z});
194+
process({msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z}, msg->header);
195195
}
196196

197197
void CollisionMonitor::cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::SharedPtr msg)
@@ -201,7 +201,8 @@ void CollisionMonitor::cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::Shar
201201
cmdVelInCallbackStamped(twist_stamped);
202202
}
203203

204-
void CollisionMonitor::publishVelocity(const Action & robot_action)
204+
void CollisionMonitor::publishVelocity(
205+
const Action & robot_action, const std_msgs::msg::Header & header)
205206
{
206207
if (robot_action.req_vel.isZero()) {
207208
if (!robot_action_prev_.req_vel.isZero()) {
@@ -215,7 +216,7 @@ void CollisionMonitor::publishVelocity(const Action & robot_action)
215216
}
216217

217218
auto cmd_vel_out_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
218-
cmd_vel_out_msg->header.stamp = this->now();
219+
cmd_vel_out_msg->header = header;
219220
cmd_vel_out_msg->twist.linear.x = robot_action.req_vel.x;
220221
cmd_vel_out_msg->twist.linear.y = robot_action.req_vel.y;
221222
cmd_vel_out_msg->twist.angular.z = robot_action.req_vel.tw;
@@ -389,7 +390,7 @@ bool CollisionMonitor::configureSources(
389390
return true;
390391
}
391392

392-
void CollisionMonitor::process(const Velocity & cmd_vel_in)
393+
void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg::Header & header)
393394
{
394395
// Current timestamp for all inner routines prolongation
395396
rclcpp::Time curr_time = this->now();
@@ -483,7 +484,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in)
483484
}
484485

485486
// Publish required robot velocity
486-
publishVelocity(robot_action);
487+
publishVelocity(robot_action, header);
487488

488489
// Publish polygons for better visualization
489490
publishPolygons();

‎nav2_velocity_smoother/src/velocity_smoother.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -289,6 +289,7 @@ void VelocitySmoother::smootherTimer()
289289
}
290290

291291
auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
292+
cmd_vel->header = command_->header;
292293

293294
// Check for velocity timeout. If nothing received, publish zeros to apply deceleration
294295
if (now() - last_command_time_ > velocity_timeout_) {

0 commit comments

Comments
 (0)