diff --git a/humble_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py b/humble_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py index 90d85f5..2a2a9dd 100644 --- a/humble_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py +++ b/humble_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py @@ -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"), + } + ], + ), + ] + ) diff --git a/humble_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py b/humble_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py index 988de94..fda66f3 100755 --- a/humble_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py +++ b/humble_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py @@ -24,44 +24,50 @@ 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() @@ -69,7 +75,8 @@ def _cmd_vel_callback(self, 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 @@ -87,5 +94,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py b/jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py index 90d85f5..2a2a9dd 100644 --- a/jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py +++ b/jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/launch/cmdvel_to_ackermann.launch.py @@ -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"), + } + ], + ), + ] + ) diff --git a/jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py b/jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py index 988de94..fda66f3 100755 --- a/jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py +++ b/jazzy_ws/src/ackermann_control/cmdvel_to_ackermann/scripts/cmdvel_to_ackermann.py @@ -24,44 +24,50 @@ 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() @@ -69,7 +75,8 @@ def _cmd_vel_callback(self, 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 @@ -87,5 +94,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main()