Skip to content

Commit

Permalink
adding stamped option for loopbacks im (#4637)
Browse files Browse the repository at this point in the history
Signed-off-by: Steve Macenski <[email protected]>
  • Loading branch information
SteveMacenski committed Aug 23, 2024
1 parent 1c64e62 commit dc3cd26
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 4 deletions.
1 change: 1 addition & 0 deletions nav2_loopback_sim/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py # Nav2 integrated na
- `odom_frame_id`: The odom frame to use (default `odom`)
- `map_frame_id`: The map frame to use (default `map`)
- `scan_frame_id`: The can frame to use to publish a scan to keep the collision monitor fed and happy (default `base_scan` for TB3, `rplidar_link` for TB4)
- `enable_stamped_cmd_vel`: Whether cmd_vel is stamped or unstamped (i.e. Twist or TwistStamped). Default `false` for `Twist`.

### Topics

Expand Down
24 changes: 20 additions & 4 deletions nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@

import math

from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistStamped
from geometry_msgs.msg import Quaternion, TransformStamped, Vector3
from nav2_simple_commander.line_iterator import LineIterator
from nav_msgs.msg import Odometry
Expand Down Expand Up @@ -73,6 +73,9 @@ def __init__(self):
self.declare_parameter('scan_frame_id', 'base_scan')
self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value

self.declare_parameter('enable_stamped_cmd_vel', False)
use_stamped = self.get_parameter('enable_stamped_cmd_vel').get_parameter_value().bool_value

self.declare_parameter('scan_publish_dur', 0.1)
self.scan_publish_dur = self.get_parameter(
'scan_publish_dur').get_parameter_value().double_value
Expand All @@ -93,9 +96,14 @@ def __init__(self):
self.initial_pose_sub = self.create_subscription(
PoseWithCovarianceStamped,
'initialpose', self.initialPoseCallback, 10)
self.cmd_vel_sub = self.create_subscription(
Twist,
'cmd_vel', self.cmdVelCallback, 10)
if not use_stamped:
self.cmd_vel_sub = self.create_subscription(
Twist,
'cmd_vel', self.cmdVelCallback, 10)
else:
self.cmd_vel_sub = self.create_subscription(
TwistStamped,
'cmd_vel', self.cmdVelStampedCallback, 10)
self.odom_pub = self.create_publisher(Odometry, 'odom', 10)

sensor_qos = QoSProfile(
Expand Down Expand Up @@ -139,6 +147,14 @@ def cmdVelCallback(self, msg):
self.curr_cmd_vel = msg
self.curr_cmd_vel_time = self.get_clock().now()

def cmdVelStampedCallback(self, msg):
self.debug('Received cmd_vel')
if self.initial_pose is None:
# Don't consider velocities before the initial pose is set
return
self.curr_cmd_vel = msg.twist
self.curr_cmd_vel_time = rclpy.time.Time.from_msg(msg.header.stamp)

def initialPoseCallback(self, msg):
self.info('Received initial pose!')
if self.initial_pose is None:
Expand Down

0 comments on commit dc3cd26

Please sign in to comment.