Skip to content

cmdvel_to_ackermann confused track_width and wheel_base #18

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

Open
JaFeKl wants to merge 1 commit into
base: main
Choose a base branch
from
Open

cmdvel_to_ackermann confused track_width and wheel_base #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
57 changes: 38 additions & 19 deletions humble_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,22 +20,41 @@


def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument("publish_period_ms", default_value="20", description="publishing dt in milliseconds"),
DeclareLaunchArgument("track_width", default_value="0.24", description="wheel separation distance (m)"),
DeclareLaunchArgument("acceleration", default_value="0.0", description="acceleration, 0 means change speed as quickly as possible (ms^-2)"),
DeclareLaunchArgument("steering_velocity", default_value="0.0", description="delta steering angle, 0 means change angle as quickly as possible (radians/s)"),

Node(
package='cmdvel_to_ackermann',
executable='cmdvel_to_ackermann.py',
name='cmdvel_to_ackermann',
output="screen",
parameters=[{
'publish_period_ms': LaunchConfiguration('publish_period_ms'),
'track_width': LaunchConfiguration('track_width'),
'acceleration': LaunchConfiguration('acceleration'),
'steering_velocity': LaunchConfiguration('steering_velocity')
}]
),
])
return LaunchDescription(
[
DeclareLaunchArgument(
"publish_period_ms",
default_value="20",
description="publishing dt in milliseconds",
),
DeclareLaunchArgument(
"wheel_base",
default_value="0.24",
description="front-to-rear axle distance (m)",
),
DeclareLaunchArgument(
"acceleration",
default_value="0.0",
description="acceleration, 0 means change speed as quickly as possible (ms^-2)",
),
DeclareLaunchArgument(
"steering_velocity",
default_value="0.0",
description="delta steering angle, 0 means change angle as quickly as possible (radians/s)",
),
Node(
package="cmdvel_to_ackermann",
executable="cmdvel_to_ackermann.py",
name="cmdvel_to_ackermann",
output="screen",
parameters=[
{
"publish_period_ms": LaunchConfiguration("publish_period_ms"),
"wheel_base": LaunchConfiguration("wheel_base"),
"acceleration": LaunchConfiguration("acceleration"),
"steering_velocity": LaunchConfiguration("steering_velocity"),
}
],
),
]
)
53 changes: 30 additions & 23 deletions humble_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,52 +24,59 @@


class CmdvelToAckermann(Node):
"""Subscribe to the Twist message and converts it to AckermannDrive message and publish.
"""
"""Subscribe to the Twist message and converts it to AckermannDrive message and publish."""

def __init__(self):
super().__init__('cmdvel_to_ackermann')

self.declare_parameter('publish_period_ms', 20)
self.declare_parameter('track_width', 0.24)
self.declare_parameter('acceleration', 0.0)
self.declare_parameter('steering_velocity', 0.0)

self._cmd_vel_subscription = self.create_subscription(Twist, '/cmd_vel',
self._cmd_vel_callback, 1)
self._ackermann_publisher = self.create_publisher(AckermannDriveStamped, '/ackermann_cmd',
1)
publish_period_ms = self.get_parameter(
'publish_period_ms').value / 1000
super().__init__("cmdvel_to_ackermann")

self.declare_parameter("publish_period_ms", 20)
self.declare_parameter("wheel_base", 0.24)
self.declare_parameter("acceleration", 0.0)
self.declare_parameter("steering_velocity", 0.0)

self._cmd_vel_subscription = self.create_subscription(
Twist, "/cmd_vel", self._cmd_vel_callback, 1
)
self._ackermann_publisher = self.create_publisher(
AckermannDriveStamped, "/ackermann_cmd", 1
)
publish_period_ms = self.get_parameter("publish_period_ms").value / 1000
self.create_timer(publish_period_ms, self._timer_callback)
self.track_width = self.get_parameter("track_width").value
self.wheel_base = self.get_parameter("wheel_base").value
self.acceleration = self.get_parameter("acceleration").value
self.steering_velocity = self.get_parameter("steering_velocity").value

self.get_logger().info(f"track_width: {self.track_width}")
if self.wheel_base <= 0.0:
self.get_logger().warn(
f"Invalid wheel_base {self.wheel_base}. Falling back to 0.24 m."
)
self.wheel_base = 0.24

self.get_logger().info(f"wheel_base: {self.wheel_base}")
self.get_logger().info(f"acceleration: {self.acceleration}")
self.get_logger().info(f"steering_velocity: {self.steering_velocity}")
self._ackermann_msg = None


def _convert_trans_rot_vel_to_steering_angle(self, v, omega) -> float:
if omega == 0 or v == 0:
if omega != 0:
self.get_logger().warn(
f'Invalid command for ackermann drive with zero vel {v} but non zero '
f'omega {omega}')
f"Invalid command for ackermann drive with zero vel {v} but non zero "
f"omega {omega}"
)
return 0.0

turning_radius = v / omega
return math.atan(self.track_width / turning_radius)
return math.atan(self.wheel_base / turning_radius)

def _cmd_vel_callback(self, msg):
self._ackermann_msg = AckermannDriveStamped()
self._ackermann_msg.header.stamp = self.get_clock().now().to_msg()
# Conversion logic (simplified example)
self._ackermann_msg.drive.speed = msg.linear.x
steering_angle = self._convert_trans_rot_vel_to_steering_angle(
self._ackermann_msg.drive.speed, msg.angular.z)
self._ackermann_msg.drive.speed, msg.angular.z
)
self._ackermann_msg.drive.steering_angle = steering_angle
self._ackermann_msg.drive.acceleration = self.acceleration
self._ackermann_msg.drive.steering_angle_velocity = self.steering_velocity
Expand All @@ -87,5 +94,5 @@ def main(args=None):
rclpy.shutdown()


if __name__ == '__main__':
if __name__ == "__main__":
main()
57 changes: 38 additions & 19 deletions jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,22 +20,41 @@


def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument("publish_period_ms", default_value="20", description="publishing dt in milliseconds"),
DeclareLaunchArgument("track_width", default_value="0.24", description="wheel separation distance (m)"),
DeclareLaunchArgument("acceleration", default_value="0.0", description="acceleration, 0 means change speed as quickly as possible (ms^-2)"),
DeclareLaunchArgument("steering_velocity", default_value="0.0", description="delta steering angle, 0 means change angle as quickly as possible (radians/s)"),

Node(
package='cmdvel_to_ackermann',
executable='cmdvel_to_ackermann.py',
name='cmdvel_to_ackermann',
output="screen",
parameters=[{
'publish_period_ms': LaunchConfiguration('publish_period_ms'),
'track_width': LaunchConfiguration('track_width'),
'acceleration': LaunchConfiguration('acceleration'),
'steering_velocity': LaunchConfiguration('steering_velocity')
}]
),
])
return LaunchDescription(
[
DeclareLaunchArgument(
"publish_period_ms",
default_value="20",
description="publishing dt in milliseconds",
),
DeclareLaunchArgument(
"wheel_base",
default_value="0.24",
description="front-to-rear axle distance (m)",
),
DeclareLaunchArgument(
"acceleration",
default_value="0.0",
description="acceleration, 0 means change speed as quickly as possible (ms^-2)",
),
DeclareLaunchArgument(
"steering_velocity",
default_value="0.0",
description="delta steering angle, 0 means change angle as quickly as possible (radians/s)",
),
Node(
package="cmdvel_to_ackermann",
executable="cmdvel_to_ackermann.py",
name="cmdvel_to_ackermann",
output="screen",
parameters=[
{
"publish_period_ms": LaunchConfiguration("publish_period_ms"),
"wheel_base": LaunchConfiguration("wheel_base"),
"acceleration": LaunchConfiguration("acceleration"),
"steering_velocity": LaunchConfiguration("steering_velocity"),
}
],
),
]
)
53 changes: 30 additions & 23 deletions jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,52 +24,59 @@


class CmdvelToAckermann(Node):
"""Subscribe to the Twist message and converts it to AckermannDrive message and publish.
"""
"""Subscribe to the Twist message and converts it to AckermannDrive message and publish."""

def __init__(self):
super().__init__('cmdvel_to_ackermann')

self.declare_parameter('publish_period_ms', 20)
self.declare_parameter('track_width', 0.24)
self.declare_parameter('acceleration', 0.0)
self.declare_parameter('steering_velocity', 0.0)

self._cmd_vel_subscription = self.create_subscription(Twist, '/cmd_vel',
self._cmd_vel_callback, 1)
self._ackermann_publisher = self.create_publisher(AckermannDriveStamped, '/ackermann_cmd',
1)
publish_period_ms = self.get_parameter(
'publish_period_ms').value / 1000
super().__init__("cmdvel_to_ackermann")

self.declare_parameter("publish_period_ms", 20)
self.declare_parameter("wheel_base", 0.24)
self.declare_parameter("acceleration", 0.0)
self.declare_parameter("steering_velocity", 0.0)

self._cmd_vel_subscription = self.create_subscription(
Twist, "/cmd_vel", self._cmd_vel_callback, 1
)
self._ackermann_publisher = self.create_publisher(
AckermannDriveStamped, "/ackermann_cmd", 1
)
publish_period_ms = self.get_parameter("publish_period_ms").value / 1000
self.create_timer(publish_period_ms, self._timer_callback)
self.track_width = self.get_parameter("track_width").value
self.wheel_base = self.get_parameter("wheel_base").value
self.acceleration = self.get_parameter("acceleration").value
self.steering_velocity = self.get_parameter("steering_velocity").value

self.get_logger().info(f"track_width: {self.track_width}")
if self.wheel_base <= 0.0:
self.get_logger().warn(
f"Invalid wheel_base {self.wheel_base}. Falling back to 0.24 m."
)
self.wheel_base = 0.24

self.get_logger().info(f"wheel_base: {self.wheel_base}")
self.get_logger().info(f"acceleration: {self.acceleration}")
self.get_logger().info(f"steering_velocity: {self.steering_velocity}")
self._ackermann_msg = None


def _convert_trans_rot_vel_to_steering_angle(self, v, omega) -> float:
if omega == 0 or v == 0:
if omega != 0:
self.get_logger().warn(
f'Invalid command for ackermann drive with zero vel {v} but non zero '
f'omega {omega}')
f"Invalid command for ackermann drive with zero vel {v} but non zero "
f"omega {omega}"
)
return 0.0

turning_radius = v / omega
return math.atan(self.track_width / turning_radius)
return math.atan(self.wheel_base / turning_radius)

def _cmd_vel_callback(self, msg):
self._ackermann_msg = AckermannDriveStamped()
self._ackermann_msg.header.stamp = self.get_clock().now().to_msg()
# Conversion logic (simplified example)
self._ackermann_msg.drive.speed = msg.linear.x
steering_angle = self._convert_trans_rot_vel_to_steering_angle(
self._ackermann_msg.drive.speed, msg.angular.z)
self._ackermann_msg.drive.speed, msg.angular.z
)
self._ackermann_msg.drive.steering_angle = steering_angle
self._ackermann_msg.drive.acceleration = self.acceleration
self._ackermann_msg.drive.steering_angle_velocity = self.steering_velocity
Expand All @@ -87,5 +94,5 @@ def main(args=None):
rclpy.shutdown()


if __name__ == '__main__':
if __name__ == "__main__":
main()