diff --git a/.github/workflows/linter,yml b/.github/workflows/linter,yml deleted file mode 100644 index 2066d6f..0000000 --- a/.github/workflows/linter,yml +++ /dev/null @@ -1,23 +0,0 @@ -name: Lint -on: - pull_request: - push: - workflow_dispatch: - -jobs: - ament_lint_general: - name: ament_${{ matrix.linter }} - runs-on: ubuntu-latest - container: - image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest - strategy: - fail-fast: false - matrix: - linter: [xmllint, cpplint, uncrustify, pep257, flake8] - steps: - - uses: actions/checkout@v4 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - linter: ${{ matrix.linter }} - distribution: rolling - package-name: "*" \ No newline at end of file diff --git a/.github/workflows/linter.yml b/.github/workflows/linter.yml index 64c243e..7e783ab 100644 --- a/.github/workflows/linter.yml +++ b/.github/workflows/linter.yml @@ -2,6 +2,8 @@ name: Lint on: pull_request: push: + branches: + - autonav_2.0 workflow_dispatch: jobs: @@ -20,4 +22,4 @@ jobs: with: linter: ${{ matrix.linter }} distribution: rolling - package-name: "*" + package-name: "autonav_bringup autonav_controller autonav_description autonav_firmware autonav_localization autonav_navigation autonav_perception autonav_utils" diff --git a/autonav_bringup/launch/autonav_bringup.launch.py b/autonav_bringup/launch/autonav_bringup.launch.py index c379ba6..d7bb02b 100644 --- a/autonav_bringup/launch/autonav_bringup.launch.py +++ b/autonav_bringup/launch/autonav_bringup.launch.py @@ -1,8 +1,16 @@ import os -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction, IncludeLaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + RegisterEventHandler, + TimerAction, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessStart +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( Command, FindExecutable, @@ -11,88 +19,83 @@ ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch_ros.parameter_descriptions import ParameterValue -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessStart -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): - autonav_description_dir = get_package_share_directory("autonav_description") - autonav_localization_dir = get_package_share_directory("autonav_localization") + autonav_description_dir = get_package_share_directory('autonav_description') + autonav_localization_dir = get_package_share_directory('autonav_localization') pkg_bno055 = get_package_share_directory('bno055') pkg_ydlidar = get_package_share_directory('ydlidar_ros2_driver') use_rviz_arg = DeclareLaunchArgument( - "rviz", - default_value="False", + 'rviz', + default_value='False', ) - rviz = LaunchConfiguration("rviz") + rviz = LaunchConfiguration('rviz') robot_description_content = Command( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', PathJoinSubstitution( [ - FindPackageShare("autonav_description"), - "urdf", - "autonav.xacro", + FindPackageShare('autonav_description'), + 'urdf', + 'autonav.xacro', ] ), ] ) - robot_description = {"robot_description": robot_description_content} + robot_description = {'robot_description': robot_description_content} robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", + package='robot_state_publisher', + executable='robot_state_publisher', parameters=[robot_description], ) rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - arguments=["-d", os.path.join(autonav_description_dir, "rviz", "display.rviz")], + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', os.path.join(autonav_description_dir, 'rviz', 'display.rviz')], condition=IfCondition(rviz), ) imu = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_bno055, 'launch', 'bno055.launch.py'), + os.path.join(pkg_bno055, 'launch', 'bno055.launch.py'), ) - ) - + ) + lidar = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_ydlidar, 'launch', 'ydlidar_launch.py'), + os.path.join(pkg_ydlidar, 'launch', 'ydlidar_launch.py'), ) ) controller_params_file = os.path.join( - get_package_share_directory("autonav_controller"), - "config", - "autonav_controllers.yaml", + get_package_share_directory('autonav_controller'), + 'config', + 'autonav_controllers.yaml', ) controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", + package='controller_manager', + executable='ros2_control_node', parameters=[robot_description, controller_params_file], ) delayed_controller_manager = TimerAction(period=3.0, actions=[controller_manager]) autonav_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["autonav_controller", "--controller-manager", "/controller_manager"], + package='controller_manager', + executable='spawner', + arguments=['autonav_controller', '--controller-manager', '/controller_manager'], remappings=[ - ("/autonav_controller/cmd_vel_unstamped", "/autonav_controller/cmd_vel"), + ('/autonav_controller/cmd_vel_unstamped', '/autonav_controller/cmd_vel'), ], ) @@ -104,12 +107,12 @@ def generate_launch_description(): ) joint_broad_spawner = Node( - package="controller_manager", - executable="spawner", + package='controller_manager', + executable='spawner', arguments=[ - "joint_state_broadcaster", - "--controller-manager", - "/controller_manager", + 'joint_state_broadcaster', + '--controller-manager', + '/controller_manager', ], ) @@ -120,27 +123,21 @@ def generate_launch_description(): ) ) - pid_controller = Node( - package="autonav_controller", - executable="pid_controller.py", - output="screen", - ) - lidar_republisher = Node( - package="autonav_controller", - executable="lidar_republisher.py", - output="screen", + package='autonav_controller', + executable='lidar_republisher.py', + output='screen', ) velocity_republisher = Node( - package="autonav_controller", - executable="cmd_vel_republisher.py", - output="screen", + package='autonav_controller', + executable='cmd_vel_republisher.py', + output='screen', ) ekf = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(autonav_localization_dir, 'launch', 'ekf.launch.py'), + os.path.join(autonav_localization_dir, 'launch', 'ekf.launch.py'), ) ) @@ -152,7 +149,6 @@ def generate_launch_description(): delayed_controller_manager, delayed_diff_drive_spawner, delayed_joint_broad_spawner, - # pid_controller, imu, lidar, ekf, diff --git a/autonav_bringup/launch/view_rviz.launch.py b/autonav_bringup/launch/view_rviz.launch.py index c04a6f2..f014a3d 100644 --- a/autonav_bringup/launch/view_rviz.launch.py +++ b/autonav_bringup/launch/view_rviz.launch.py @@ -1,38 +1,29 @@ import os -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from launch_ros.parameter_descriptions import ParameterValue -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessStart +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node def generate_launch_description(): - autonav_description_dir = get_package_share_directory("autonav_description") + autonav_description_dir = get_package_share_directory('autonav_description') use_rviz_arg = DeclareLaunchArgument( - "rviz", - default_value="True", + 'rviz', + default_value='True', ) - rviz = LaunchConfiguration("rviz") + rviz = LaunchConfiguration('rviz') rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - arguments=["-d", os.path.join(autonav_description_dir, "rviz", "display.rviz")], + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', os.path.join(autonav_description_dir, 'rviz', 'display.rviz')], condition=IfCondition(rviz), ) diff --git a/autonav_bringup/launch/view_rviz_cartographer.launch.py b/autonav_bringup/launch/view_rviz_cartographer.launch.py index 4c0a057..512b4a1 100644 --- a/autonav_bringup/launch/view_rviz_cartographer.launch.py +++ b/autonav_bringup/launch/view_rviz_cartographer.launch.py @@ -1,13 +1,12 @@ #!/usr/bin/python3 -import os from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch_ros.substitutions import FindPackageShare +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): @@ -17,7 +16,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( - name='rviz', + name='rviz', default_value='true', description='Run rviz' ), @@ -28,6 +27,6 @@ def generate_launch_description(): name='rviz2', output='screen', arguments=['-d', rviz_config_path], - condition=IfCondition(LaunchConfiguration("rviz")), + condition=IfCondition(LaunchConfiguration('rviz')), ) - ]) \ No newline at end of file + ]) diff --git a/autonav_bringup/launch/view_rviz_navigation.launch.py b/autonav_bringup/launch/view_rviz_navigation.launch.py index a137f17..4f1f218 100644 --- a/autonav_bringup/launch/view_rviz_navigation.launch.py +++ b/autonav_bringup/launch/view_rviz_navigation.launch.py @@ -1,13 +1,12 @@ #!/usr/bin/python3 -import os from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch_ros.substitutions import FindPackageShare +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): @@ -17,7 +16,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( - name='rviz', + name='rviz', default_value='true', description='Run rviz' ), @@ -28,6 +27,6 @@ def generate_launch_description(): name='rviz2', output='screen', arguments=['-d', rviz_config_path], - condition=IfCondition(LaunchConfiguration("rviz")), + condition=IfCondition(LaunchConfiguration('rviz')), ) - ]) \ No newline at end of file + ]) diff --git a/autonav_bringup/launch/view_rviz_slam.launch.py b/autonav_bringup/launch/view_rviz_slam.launch.py index f882b3a..2e0cd94 100644 --- a/autonav_bringup/launch/view_rviz_slam.launch.py +++ b/autonav_bringup/launch/view_rviz_slam.launch.py @@ -1,13 +1,12 @@ #!/usr/bin/python3 -import os from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch_ros.substitutions import FindPackageShare +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): @@ -17,7 +16,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( - name='rviz', + name='rviz', default_value='true', description='Run rviz' ), @@ -28,6 +27,6 @@ def generate_launch_description(): name='rviz2', output='screen', arguments=['-d', rviz_config_path], - condition=IfCondition(LaunchConfiguration("rviz")), + condition=IfCondition(LaunchConfiguration('rviz')), ) - ]) \ No newline at end of file + ]) diff --git a/autonav_controller/autonav_controller/cmd_vel_republisher.py b/autonav_controller/autonav_controller/cmd_vel_republisher.py index f856e2f..593cd72 100755 --- a/autonav_controller/autonav_controller/cmd_vel_republisher.py +++ b/autonav_controller/autonav_controller/cmd_vel_republisher.py @@ -1,10 +1,12 @@ #!/usr/bin/env python3 +from geometry_msgs.msg import Twist import rclpy from rclpy.node import Node -from geometry_msgs.msg import Twist + class VelocityRelay(Node): + def __init__(self): super().__init__('cmd_vel_republisher') self.subscription = self.create_subscription( @@ -21,7 +23,8 @@ def __init__(self): def listener_callback(self, msg): self.publisher.publish(msg) - # self.get_logger().info('Relaying velocity command: "%s"' % msg) + # self.get_logger().info('Relaying velocity command: '%s'' % msg) + def main(args=None): rclpy.init(args=args) @@ -30,5 +33,6 @@ def main(args=None): velocity_relay.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/autonav_controller/autonav_controller/control.py b/autonav_controller/autonav_controller/control.py index 682a897..cf24f1e 100755 --- a/autonav_controller/autonav_controller/control.py +++ b/autonav_controller/autonav_controller/control.py @@ -1,17 +1,19 @@ #! /usr/bin/python3 -import rclpy -from rclpy.node import Node -from rclpy.timer import Timer from geometry_msgs.msg import Twist from pynput import keyboard +import rclpy +from rclpy.node import Node + class TeleopPublisher(Node): + def __init__(self): super().__init__('teleop_publisher') self.publisher_ = self.create_publisher(Twist, 'autonav_controller/cmd_vel_unstamped', 10) self.twist = Twist() - self.listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release, suppress=True) + self.listener = keyboard.Listener( + on_press=self.on_press, on_release=self.on_release, suppress=True) self.listener.start() self.timer = self.create_timer(0.05, self.timer_callback) @@ -28,14 +30,21 @@ def on_press(self, key): self.twist.angular.z = -1.0 def on_release(self, key): - if key in [keyboard.KeyCode.from_char('w'), keyboard.KeyCode.from_char('W'), keyboard.KeyCode.from_char('s'), keyboard.KeyCode.from_char('S')]: + if key in [ + keyboard.KeyCode.from_char('w'), keyboard.KeyCode.from_char('W'), + keyboard.KeyCode.from_char('s'), keyboard.KeyCode.from_char('S') + ]: self.twist.linear.x = 0.0 - elif key in [keyboard.KeyCode.from_char('a'), keyboard.KeyCode.from_char('A'), keyboard.KeyCode.from_char('d'), keyboard.KeyCode.from_char('D')]: + elif key in [ + keyboard.KeyCode.from_char('a'), keyboard.KeyCode.from_char('A'), + keyboard.KeyCode.from_char('d'), keyboard.KeyCode.from_char('D') + ]: self.twist.angular.z = 0.0 def timer_callback(self): self.publisher_.publish(self.twist) + def main(args=None): rclpy.init(args=args) teleop_publisher = TeleopPublisher() @@ -45,7 +54,6 @@ def main(args=None): teleop_publisher.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() - - diff --git a/autonav_controller/autonav_controller/lidar_republisher.py b/autonav_controller/autonav_controller/lidar_republisher.py index 622d03e..b58941a 100755 --- a/autonav_controller/autonav_controller/lidar_republisher.py +++ b/autonav_controller/autonav_controller/lidar_republisher.py @@ -4,7 +4,9 @@ from rclpy.node import Node from sensor_msgs.msg import LaserScan + class LidarRelay(Node): + def __init__(self): super().__init__('lidar_relay') self.subscription = self.create_subscription( @@ -17,6 +19,7 @@ def __init__(self): def listener_callback(self, msg): self.publisher.publish(msg) + def main(args=None): rclpy.init(args=args) lidar_relay = LidarRelay() @@ -29,5 +32,6 @@ def main(args=None): lidar_relay.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/autonav_controller/autonav_controller/odom_logger.py b/autonav_controller/autonav_controller/odom_logger.py index 1cf0575..a41def2 100755 --- a/autonav_controller/autonav_controller/odom_logger.py +++ b/autonav_controller/autonav_controller/odom_logger.py @@ -1,11 +1,13 @@ #!/usr/bin/env python3 +from nav_msgs.msg import Odometry import rclpy from rclpy.node import Node -from nav_msgs.msg import Odometry import tf_transformations + class OdomLogger(Node): + def __init__(self): super().__init__('odom_logger') self.subscription = self.create_subscription( @@ -19,9 +21,11 @@ def odom_callback(self, msg): position = msg.pose.pose.position orientation_q = msg.pose.pose.orientation orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] - (roll, pitch, yaw) = tf_transformations.euler_from_quaternion(orientation_list) + yaw = tf_transformations.euler_from_quaternion(orientation_list)[2] + + self.get_logger().info( + 'Position: ({:.2f}, {:.2f}), Theta: {:.2f}'.format(position.x, position.y, yaw)) - self.get_logger().info('Position: ({:.2f}, {:.2f}), Theta: {:.2f}'.format(position.x, position.y, yaw)) def main(args=None): rclpy.init(args=args) @@ -30,5 +34,6 @@ def main(args=None): node.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/autonav_controller/autonav_controller/pid_controller.py b/autonav_controller/autonav_controller/pid_controller.py index 2071bdb..3a48356 100755 --- a/autonav_controller/autonav_controller/pid_controller.py +++ b/autonav_controller/autonav_controller/pid_controller.py @@ -1,30 +1,32 @@ #! /usr/bin/python3 -import rclpy -from rclpy.node import Node -from std_msgs.msg import Int64MultiArray, Int32, Float64MultiArray import time + import numpy as np +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray, Int32, Int64MultiArray class MotorFeedbackListener(Node): + def __init__(self): - super().__init__("motor_controller") + super().__init__('motor_controller') self.feedback_sub = self.create_subscription( - Int64MultiArray, "/motor/feedback", self.feedback_callback, 10 + Int64MultiArray, '/motor/feedback', self.feedback_callback, 10 ) self.controller_cmd_sub = self.create_subscription( - Float64MultiArray, "/motor/cmd", self.controller_cmd_callback, 10 + Float64MultiArray, '/motor/cmd', self.controller_cmd_callback, 10 ) self.left_pub = self.create_publisher( - Int32, "/motor/left_cmd", 10 + Int32, '/motor/left_cmd', 10 ) self.right_pub = self.create_publisher( - Int32, "/motor/right_cmd", 10 + Int32, '/motor/right_cmd', 10 ) self.timer = self.create_timer( 0.2, self.cmd_callback - ) + ) self.last_time = time.time() self.last_left_counts = 0 self.last_right_counts = 0 @@ -41,10 +43,7 @@ def __init__(self): def controller_cmd_callback(self, msg): self.right_motor_cmd = (msg.data[1] * 60.0) / (2 * np.pi) self.left_motor_cmd = (msg.data[0] * 60.0) / (2 * np.pi) - # self.get_logger().info( - # f'Left`: {round(self.left_motor_cmd , 1)}, Right`: {round(self.right_motor_cmd , 1)}' - # ) - + def get_right_motor_cmd(self): return self.right_motor_cmd @@ -72,7 +71,7 @@ def feedback_callback(self, msg): # self.get_logger().info( # f'{round(self.left_motor_rpm, 1)}, {round(self.right_motor_rpm, 1)}' # ) - + def cmd_callback(self): left = 0 right = 0 @@ -93,13 +92,13 @@ def cmd_callback(self): self.right_motor_cmd_msg.data = right self.left_pub.publish(self.left_motor_cmd_msg) self.right_pub.publish(self.right_motor_cmd_msg) - + def get_left_motor_rpm(self): return self.left_motor_rpm def get_right_motor_rpm(self): return self.right_motor_rpm - + def set_left_motor_rpm(self, cmd_pwm): self.cmd_left_motor_pwm = cmd_pwm @@ -111,8 +110,10 @@ def calculate_rpm(self, counts_delta, elapsed_time): rpm = (counts_delta / counts_per_revolution) / (elapsed_time / 60.0) return rpm + class PIDController: - def __init__(self, kp, ki, kd, setpoint, sample_time=10, output_limits=None, proportional_on_error=True): + + def __init__(self, kp, ki, kd, setpoint, sample_time=10, proportional_on_error=True): self.kp = kp self.ki = ki self.kd = kd @@ -134,7 +135,6 @@ def set_const(self, Kp, Ki, Kd, Setpoint): def compute(self, feedback): current_time = time.time() - elapsed_time = current_time - self.last_time input_val = feedback error = self.setpoint - input_val @@ -179,6 +179,7 @@ def compute(self, feedback): kd = 0.001 setpoint = 0.0 + def main(args=None): global left_pid, right_pid rclpy.init(args=args) @@ -195,17 +196,18 @@ def main(args=None): right = right_pid.compute(motor_feedback_listener.get_right_motor_rpm()) motor_feedback_listener.set_left_motor_rpm(left) motor_feedback_listener.set_right_motor_rpm(right) - + try: rclpy.spin_once(motor_feedback_listener) - except: + except Exception: break - + try: motor_feedback_listener.destroy_node() rclpy.shutdown() - except: + except Exception: pass -if __name__ == "__main__": + +if __name__ == '__main__': main() diff --git a/autonav_controller/autonav_controller/tune_pid.py b/autonav_controller/autonav_controller/tune_pid.py index b51e5ca..b521458 100755 --- a/autonav_controller/autonav_controller/tune_pid.py +++ b/autonav_controller/autonav_controller/tune_pid.py @@ -1,35 +1,38 @@ #! /usr/bin/python3 -import rclpy -from rclpy.node import Node -from std_msgs.msg import Int64MultiArray, Int32 +import threading import time import tkinter as tk -import threading + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32, Int64MultiArray kp = 0 ki = 0 kd = 0 setpoint = 0 + class MotorFeedbackListener(Node): + def __init__(self): - super().__init__("motor_controller") + super().__init__('motor_controller') self.subscription = self.create_subscription( - Int64MultiArray, "/motor/feedback", self.callback, 10 + Int64MultiArray, '/motor/feedback', self.callback, 10 ) self.left_pub = self.create_publisher( - Int32, "/motor/left_cmd", 10 + Int32, '/motor/left_cmd', 10 ) self.right_pub = self.create_publisher( - Int32, "/motor/right_cmd", 10 + Int32, '/motor/right_cmd', 10 ) self.timer = self.create_timer( 0.2, self.cmd_callback - ) + ) self.last_time = time.time() self.last_left_counts = 0 - self.last_right_counts = 0 + self.last_right_counts = 0 self.total_counts_per_revolution = 151 self.left_motor_rpm = 0 self.right_motor_rpm = 0 @@ -59,7 +62,7 @@ def callback(self, msg): self.get_logger().info( f'Left: {round(self.left_motor_rpm, 1)}, Right: {round(self.right_motor_rpm, 1)}' ) - + def cmd_callback(self): left = 0 right = 0 @@ -86,7 +89,7 @@ def get_left_motor_rpm(self): def get_right_motor_rpm(self): return self.right_motor_rpm - + def set_left_motor_rpm(self, cmd_pwm): self.cmd_left_motor_rpm = cmd_pwm @@ -98,8 +101,10 @@ def calculate_rpm(self, counts_delta, elapsed_time): rpm = (counts_delta / counts_per_revolution) / (elapsed_time / 60.0) return rpm + class PIDController: - def __init__(self, kp, ki, kd, setpoint, sample_time=10, output_limits=None, proportional_on_error=True): + + def __init__(self, kp, ki, kd, setpoint, sample_time=10, proportional_on_error=True): self.kp = kp self.ki = ki self.kd = kd @@ -121,7 +126,6 @@ def set_const(self, Kp, Ki, Kd, Setpoint): def compute(self, feedback): current_time = time.time() - elapsed_time = current_time - self.last_time input_val = feedback error = self.setpoint - input_val @@ -156,51 +160,50 @@ def compute(self, feedback): return output - class PIDGUI: + def __init__(self, master): self.master = master - self.master.title("PID Controller GUI") + self.master.title('PID Controller GUI') self.default_kp = 0.71 self.default_ki = 0.352 self.default_kd = 0.0857 self.default_setpoint = 30.0 - self.kp_label = tk.Label(master, text="Enter Kp:") + self.kp_label = tk.Label(master, text='Enter Kp:') self.kp_label.pack() self.kp_entry = tk.Entry(master) self.kp_entry.insert(tk.END, str(self.default_kp)) self.kp_entry.pack() - self.ki_label = tk.Label(master, text="Enter Ki:") + self.ki_label = tk.Label(master, text='Enter Ki:') self.ki_label.pack() self.ki_entry = tk.Entry(master) self.ki_entry.insert(tk.END, str(self.default_ki)) self.ki_entry.pack() - self.kd_label = tk.Label(master, text="Enter Kd:") + self.kd_label = tk.Label(master, text='Enter Kd:') self.kd_label.pack() self.kd_entry = tk.Entry(master) self.kd_entry.insert(tk.END, str(self.default_kd)) self.kd_entry.pack() - self.setpoint_label = tk.Label(master, text="Enter Setpoint:") + self.setpoint_label = tk.Label(master, text='Enter Setpoint:') self.setpoint_label.pack() self.setpoint_entry = tk.Entry(master) self.setpoint_entry.insert(tk.END, str(self.default_setpoint)) self.setpoint_entry.pack() - self.set_button = tk.Button(master, text="Set", command=self.print_values) + self.set_button = tk.Button(master, text='Set', command=self.print_values) self.set_button.pack() def print_values(self): try: - kp = float(self.kp_entry.get()) ki = float(self.ki_entry.get()) kd = float(self.kd_entry.get()) @@ -208,9 +211,10 @@ def print_values(self): left_pid.set_const(kp, ki, kd, setpoint) right_pid.set_const(kp, ki, kd, setpoint) - print(f"Kp: {kp}, Ki: {ki}, Kd: {kd}, Setpoint: {setpoint}") + print(f'Kp: {kp}, Ki: {ki}, Kd: {kd}, Setpoint: {setpoint}') except ValueError: - print("Invalid input. Please enter numeric values.") + print('Invalid input. Please enter numeric values.') + left_pid = None right_pid = None @@ -219,6 +223,7 @@ def print_values(self): kd = 0 setpoint = 0 + def main(args=None): global left_pid, right_pid rclpy.init(args=args) @@ -232,17 +237,18 @@ def main(args=None): right = right_pid.compute(motor_feedback_listener.get_right_motor_rpm()) motor_feedback_listener.set_left_motor_rpm(left) motor_feedback_listener.set_right_motor_rpm(right) - # print(kp, ki, kd, setpoint, left, right) rclpy.spin_once(motor_feedback_listener) rclpy.spin(motor_feedback_listener) motor_feedback_listener.destroy_node() rclpy.shutdown() + def run_gui(): root = tk.Tk() - pid_gui = PIDGUI(root) + PIDGUI(root) root.mainloop() -if __name__ == "__main__": + +if __name__ == '__main__': main() diff --git a/autonav_controller/launch/autonav_bringup.launch.py b/autonav_controller/launch/autonav_bringup.launch.py index ad515de..e53c59d 100644 --- a/autonav_controller/launch/autonav_bringup.launch.py +++ b/autonav_controller/launch/autonav_bringup.launch.py @@ -1,8 +1,10 @@ import os -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction +from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessStart from launch.substitutions import ( Command, FindExecutable, @@ -11,70 +13,66 @@ ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch_ros.parameter_descriptions import ParameterValue -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessStart -from launch.conditions import IfCondition def generate_launch_description(): - autonav_description_dir = get_package_share_directory("autonav_description") + autonav_description_dir = get_package_share_directory('autonav_description') use_rviz_arg = DeclareLaunchArgument( - "rviz", - default_value="False", + 'rviz', + default_value='False', ) - rviz = LaunchConfiguration("rviz") + rviz = LaunchConfiguration('rviz') robot_description_content = Command( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', PathJoinSubstitution( [ - FindPackageShare("autonav_description"), - "urdf", - "autonav.xacro", + FindPackageShare('autonav_description'), + 'urdf', + 'autonav.xacro', ] ), ] ) - robot_description = {"robot_description": robot_description_content} + robot_description = {'robot_description': robot_description_content} robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", + package='robot_state_publisher', + executable='robot_state_publisher', parameters=[robot_description], ) rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - arguments=["-d", os.path.join(autonav_description_dir, "rviz", "display.rviz")], + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', os.path.join(autonav_description_dir, 'rviz', 'display.rviz')], condition=IfCondition(rviz), ) controller_params_file = os.path.join( - get_package_share_directory("autonav_controller"), - "config", - "autonav_controllers.yaml", + get_package_share_directory('autonav_controller'), + 'config', + 'autonav_controllers.yaml', ) controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", + package='controller_manager', + executable='ros2_control_node', parameters=[robot_description, controller_params_file], ) delayed_controller_manager = TimerAction(period=3.0, actions=[controller_manager]) autonav_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["autonav_controller", "--controller-manager", "/controller_manager"], + package='controller_manager', + executable='spawner', + arguments=['autonav_controller', '--controller-manager', '/controller_manager'], ) delayed_diff_drive_spawner = RegisterEventHandler( @@ -85,12 +83,12 @@ def generate_launch_description(): ) joint_broad_spawner = Node( - package="controller_manager", - executable="spawner", + package='controller_manager', + executable='spawner', arguments=[ - "joint_state_broadcaster", - "--controller-manager", - "/controller_manager", + 'joint_state_broadcaster', + '--controller-manager', + '/controller_manager', ], ) @@ -102,8 +100,8 @@ def generate_launch_description(): ) pid_controller = Node( - package="autonav_controller", - executable="pid_controller.py", + package='autonav_controller', + executable='pid_controller.py', output='screen', ) diff --git a/autonav_controller/launch/controller.launch.py b/autonav_controller/launch/controller.launch.py index 3ba297e..816ba38 100644 --- a/autonav_controller/launch/controller.launch.py +++ b/autonav_controller/launch/controller.launch.py @@ -1,33 +1,29 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration -from launch.conditions import UnlessCondition, IfCondition def generate_launch_description(): joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", + package='controller_manager', + executable='spawner', arguments=[ - "joint_state_broadcaster", - "--controller-manager", - "/controller_manager", + 'joint_state_broadcaster', + '--controller-manager', + '/controller_manager', ], ) wheel_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["autonav_controller", - "--controller-manager", - "/controller_manager" + package='controller_manager', + executable='spawner', + arguments=[ + 'autonav_controller', + '--controller-manager', + '/controller_manager', ], ) - return LaunchDescription( - [ - joint_state_broadcaster_spawner, - wheel_controller_spawner, - ] - ) \ No newline at end of file + return LaunchDescription([ + joint_state_broadcaster_spawner, + wheel_controller_spawner, + ]) diff --git a/autonav_controller/launch/joystick_teleop.launch.py b/autonav_controller/launch/joystick_teleop.launch.py index 8c7d697..9aaebb3 100644 --- a/autonav_controller/launch/joystick_teleop.launch.py +++ b/autonav_controller/launch/joystick_teleop.launch.py @@ -1,36 +1,37 @@ +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node -import os -from ament_index_python.packages import get_package_share_directory - def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') - joy_params = os.path.join(get_package_share_directory('autonav_controller'),'config','joystick.yaml') + joy_params = os.path.join(get_package_share_directory( + 'autonav_controller'), 'config', 'joystick.yaml') joy_node = Node( - package='joy', - executable='joy_node', - parameters=[joy_params, {'use_sim_time': use_sim_time}], - ) + package='joy', + executable='joy_node', + parameters=[joy_params, {'use_sim_time': use_sim_time}], + ) teleop_node = Node( - package='teleop_twist_joy', - executable='teleop_node', - name='teleop_node', - parameters=[joy_params, {'use_sim_time': use_sim_time}], - remappings=[('/cmd_vel','/autonav_controller/cmd_vel_unstamped')] - ) - + package='teleop_twist_joy', + executable='teleop_node', + name='teleop_node', + parameters=[joy_params, {'use_sim_time': use_sim_time}], + remappings=[('/cmd_vel', '/autonav_controller/cmd_vel_unstamped')] + ) + return LaunchDescription([ DeclareLaunchArgument( 'use_sim_time', default_value='false', description='Use sim time if true'), joy_node, - teleop_node - ]) \ No newline at end of file + teleop_node + ]) diff --git a/autonav_description/launch/autonav_bringup.launch.py b/autonav_description/launch/autonav_bringup.launch.py deleted file mode 100644 index ad515de..0000000 --- a/autonav_description/launch/autonav_bringup.launch.py +++ /dev/null @@ -1,120 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from launch_ros.parameter_descriptions import ParameterValue -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessStart -from launch.conditions import IfCondition - - -def generate_launch_description(): - autonav_description_dir = get_package_share_directory("autonav_description") - - use_rviz_arg = DeclareLaunchArgument( - "rviz", - default_value="False", - ) - - rviz = LaunchConfiguration("rviz") - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("autonav_description"), - "urdf", - "autonav.xacro", - ] - ), - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - parameters=[robot_description], - ) - - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - arguments=["-d", os.path.join(autonav_description_dir, "rviz", "display.rviz")], - condition=IfCondition(rviz), - ) - - controller_params_file = os.path.join( - get_package_share_directory("autonav_controller"), - "config", - "autonav_controllers.yaml", - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, controller_params_file], - ) - - delayed_controller_manager = TimerAction(period=3.0, actions=[controller_manager]) - - autonav_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["autonav_controller", "--controller-manager", "/controller_manager"], - ) - - delayed_diff_drive_spawner = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=controller_manager, - on_start=[autonav_spawner], - ) - ) - - joint_broad_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_state_broadcaster", - "--controller-manager", - "/controller_manager", - ], - ) - - delayed_joint_broad_spawner = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=controller_manager, - on_start=[joint_broad_spawner], - ) - ) - - pid_controller = Node( - package="autonav_controller", - executable="pid_controller.py", - output='screen', - ) - - return LaunchDescription( - [ - use_rviz_arg, - robot_state_publisher_node, - rviz_node, - delayed_controller_manager, - delayed_diff_drive_spawner, - delayed_joint_broad_spawner, - pid_controller, - ] - ) diff --git a/autonav_description/launch/display.launch.py b/autonav_description/launch/display.launch.py index ec3352c..bb4bbe1 100644 --- a/autonav_description/launch/display.launch.py +++ b/autonav_description/launch/display.launch.py @@ -1,60 +1,55 @@ import os -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction +from launch.actions import TimerAction from launch.substitutions import ( Command, FindExecutable, - LaunchConfiguration, PathJoinSubstitution, ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch_ros.parameter_descriptions import ParameterValue def generate_launch_description(): - autonav_description_dir = get_package_share_directory("autonav_description") + autonav_description_dir = get_package_share_directory('autonav_description') robot_description_content = Command( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', PathJoinSubstitution( [ - FindPackageShare("autonav_description"), - "urdf", - "autonav.xacro", + FindPackageShare('autonav_description'), + 'urdf', + 'autonav.xacro', ] ), ] ) - robot_description = {"robot_description": robot_description_content} + robot_description = {'robot_description': robot_description_content} robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", + package='robot_state_publisher', + executable='robot_state_publisher', parameters=[robot_description], ) - joint_state_publisher_gui_node = Node( - package="joint_state_publisher_gui", executable="joint_state_publisher_gui" - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - arguments=["-d", os.path.join(autonav_description_dir, "rviz", "display.rviz")], + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', os.path.join(autonav_description_dir, 'rviz', 'display.rviz')], ) - controller_params_file = os.path.join(get_package_share_directory('autonav_controller'),'config','autonav_controllers.yaml') + controller_params_file = os.path.join(get_package_share_directory( + 'autonav_controller'), 'config', 'autonav_controllers.yaml') controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", + package='controller_manager', + executable='ros2_control_node', parameters=[robot_description, controller_params_file] ) @@ -63,7 +58,6 @@ def generate_launch_description(): return LaunchDescription( [ - # joint_state_publisher_gui_node, robot_state_publisher_node, rviz_node, delayed_controller_manager, diff --git a/autonav_description/launch/gazebo.launch.py b/autonav_description/launch/gazebo.launch.py index 197ed6c..96a36d6 100644 --- a/autonav_description/launch/gazebo.launch.py +++ b/autonav_description/launch/gazebo.launch.py @@ -1,76 +1,72 @@ import os -from ament_index_python.packages import get_package_share_directory, get_package_prefix +from ament_index_python.packages import get_package_prefix, get_package_share_directory from launch import LaunchDescription from launch.actions import ( - DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable, ) +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ( Command, FindExecutable, - LaunchConfiguration, PathJoinSubstitution, ) -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - autonav_description_dir = get_package_share_directory("autonav_description") autonav_description_share = os.path.join( - get_package_prefix("autonav_description"), "share" + get_package_prefix('autonav_description'), 'share' ) - gazebo_ros_dir = get_package_share_directory("gazebo_ros") + gazebo_ros_dir = get_package_share_directory('gazebo_ros') - env_var = SetEnvironmentVariable("GAZEBO_MODEL_PATH", autonav_description_share) + env_var = SetEnvironmentVariable('GAZEBO_MODEL_PATH', autonav_description_share) robot_description_content = Command( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', PathJoinSubstitution( [ - FindPackageShare("autonav_description"), - "urdf", - "autonav.xacro", + FindPackageShare('autonav_description'), + 'urdf', + 'autonav.xacro', ] ), ] ) - robot_description = {"robot_description": robot_description_content} + robot_description = {'robot_description': robot_description_content} robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", + package='robot_state_publisher', + executable='robot_state_publisher', parameters=[robot_description], ) start_gazebo_server = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(gazebo_ros_dir, "launch", "gzserver.launch.py") + os.path.join(gazebo_ros_dir, 'launch', 'gzserver.launch.py') ) ) start_gazebo_client = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(gazebo_ros_dir, "launch", "gzclient.launch.py") + os.path.join(gazebo_ros_dir, 'launch', 'gzclient.launch.py') ) ) spawn_robot = Node( - package="gazebo_ros", - executable="spawn_entity.py", + package='gazebo_ros', + executable='spawn_entity.py', arguments=[ - "-entity", - "autonav", - "-topic", - "robot_description", + '-entity', + 'autonav', + '-topic', + 'robot_description', ], - output="screen", + output='screen', ) return LaunchDescription( diff --git a/autonav_description/launch/robot_description.launch.py b/autonav_description/launch/robot_description.launch.py index d8d9096..f53c3a0 100644 --- a/autonav_description/launch/robot_description.launch.py +++ b/autonav_description/launch/robot_description.launch.py @@ -1,29 +1,27 @@ #!/usr/bin/env python3 from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -def generate_launch_description(): - +def generate_launch_description(): robot_description_content = Command( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', PathJoinSubstitution( [ - FindPackageShare("autonav_description"), - "urdf", - "autonav.xacro", + FindPackageShare('autonav_description'), + 'urdf', + 'autonav.xacro', ] ), ] ) - robot_description = {"robot_description": robot_description_content} + robot_description = {'robot_description': robot_description_content} return LaunchDescription([ Node( @@ -32,4 +30,4 @@ def generate_launch_description(): name='robot_state_publisher', output='screen', parameters=[robot_description]), - ]) \ No newline at end of file + ]) diff --git a/autonav_firmware/CMakeLists.txt b/autonav_firmware/CMakeLists.txt index dbd59cc..3ab31e6 100644 --- a/autonav_firmware/CMakeLists.txt +++ b/autonav_firmware/CMakeLists.txt @@ -43,11 +43,6 @@ include_directories( include ) -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - pluginlib_export_plugin_description_file(hardware_interface autonav_interface.xml) ament_export_include_directories( diff --git a/autonav_firmware/include/autonav_firmware/autonav_interface.hpp b/autonav_firmware/include/autonav_firmware/autonav_interface.hpp index d24847a..d8c3567 100644 --- a/autonav_firmware/include/autonav_firmware/autonav_interface.hpp +++ b/autonav_firmware/include/autonav_firmware/autonav_interface.hpp @@ -1,18 +1,30 @@ -#ifndef AUTONAV_INTERFACE_HPP -#define AUTONAV_INTERFACE_HPP +// Copyright (c) 2024 Jatin Patil +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. -#include -#include -#include -#include +#ifndef AUTONAV_FIRMWARE__AUTONAV_INTERFACE_HPP_ +#define AUTONAV_FIRMWARE__AUTONAV_INTERFACE_HPP_ -#include -#include #include - -#include #include +#include +#include "hardware_interface/system_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "std_msgs/msg/int64_multi_array.hpp" namespace autonav_firmware { @@ -26,15 +38,19 @@ class AutonavInterface : public hardware_interface::SystemInterface virtual ~AutonavInterface(); // Implementing rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface - virtual CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; - virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; // Implementing hardware_interface::SystemInterface - virtual CallbackReturn on_init(const hardware_interface::HardwareInfo &hardware_info) override; - virtual std::vector export_state_interfaces() override; - virtual std::vector export_command_interfaces() override; - virtual hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override; - virtual hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) override; + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + hardware_interface::return_type read( + const rclcpp::Time &, + const rclcpp::Duration &) override; + hardware_interface::return_type write( + const rclcpp::Time &, + const rclcpp::Duration &) override; private: std::shared_ptr node_; @@ -42,7 +58,7 @@ class AutonavInterface : public hardware_interface::SystemInterface // rclcpp::Publisher::SharedPtr left_cmd_publisher_; // rclcpp::Publisher::SharedPtr right_cmd_publisher_; rclcpp::Publisher::SharedPtr cmd_publisher_; - + void processFeedback(const std_msgs::msg::Float64MultiArray::SharedPtr msg); double hw_start_sec_; double hw_stop_sec_; @@ -52,5 +68,4 @@ class AutonavInterface : public hardware_interface::SystemInterface }; } // namespace autonav_firmware - -#endif // AUTONAV_INTERFACE_HPP \ No newline at end of file +#endif // AUTONAV_FIRMWARE__AUTONAV_INTERFACE_HPP_ diff --git a/autonav_firmware/include/autonav_firmware/autonav_interface_backup.hpp b/autonav_firmware/include/autonav_firmware/autonav_interface_backup.hpp deleted file mode 100644 index cc4c386..0000000 --- a/autonav_firmware/include/autonav_firmware/autonav_interface_backup.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef AUTONAV_INTERFACE_HPP -#define AUTONAV_INTERFACE_HPP - -#include -#include -#include -#include -#include - -#include -#include - - -namespace autonav_firmware -{ - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -class AutonavInterface : public hardware_interface::SystemInterface -{ -public: - AutonavInterface(); - virtual ~AutonavInterface(); - - // Implementing rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface - virtual CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; - virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; - - // Implementing hardware_interface::SystemInterface - virtual CallbackReturn on_init(const hardware_interface::HardwareInfo &hardware_info) override; - virtual std::vector export_state_interfaces() override; - virtual std::vector export_command_interfaces() override; - virtual hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override; - virtual hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override; - -private: - double hw_start_sec_; - double hw_stop_sec_; - std::vector hw_commands_; - std::vector hw_positions_; - std::vector hw_velocities_; -}; -} // namespace bumperbot_firmware - - -#endif // AUTONAV_INTERFACE_HPP \ No newline at end of file diff --git a/autonav_firmware/include/autonav_firmware/autonav_interface_serial.hpp b/autonav_firmware/include/autonav_firmware/autonav_interface_serial.hpp index 6a9f451..713af35 100644 --- a/autonav_firmware/include/autonav_firmware/autonav_interface_serial.hpp +++ b/autonav_firmware/include/autonav_firmware/autonav_interface_serial.hpp @@ -1,15 +1,28 @@ -#ifndef AUTONAV_INTERFACE_HPP -#define AUTONAV_INTERFACE_HPP +// Copyright (c) 2024 Jatin Patil +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTONAV_FIRMWARE__AUTONAV_INTERFACE_SERIAL_HPP_ +#define AUTONAV_FIRMWARE__AUTONAV_INTERFACE_SERIAL_HPP_ -#include -#include #include -#include -#include - -#include #include +#include +#include "hardware_interface/system_interface.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace autonav_firmware { @@ -23,15 +36,19 @@ class AutonavInterface : public hardware_interface::SystemInterface virtual ~AutonavInterface(); // Implementing rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface - virtual CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; - virtual CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; // Implementing hardware_interface::SystemInterface - virtual CallbackReturn on_init(const hardware_interface::HardwareInfo &hardware_info) override; - virtual std::vector export_state_interfaces() override; - virtual std::vector export_command_interfaces() override; - virtual hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override; - virtual hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & hardware_info) override; + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + hardware_interface::return_type read( + const rclcpp::Time &, + const rclcpp::Duration &) override; + hardware_interface::return_type write( + const rclcpp::Time &, + const rclcpp::Duration &) override; private: serial::Serial esp_; @@ -43,5 +60,4 @@ class AutonavInterface : public hardware_interface::SystemInterface }; } // namespace autonav_firmware - -#endif // AUTONAV_INTERFACE_HPP \ No newline at end of file +#endif // AUTONAV_FIRMWARE__AUTONAV_INTERFACE_SERIAL_HPP_ diff --git a/autonav_firmware/launch/autonav_bringup.launch.py b/autonav_firmware/launch/autonav_bringup.launch.py deleted file mode 100644 index 4a21590..0000000 --- a/autonav_firmware/launch/autonav_bringup.launch.py +++ /dev/null @@ -1,120 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from launch_ros.parameter_descriptions import ParameterValue -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessStart -from launch.conditions import IfCondition - - -def generate_launch_description(): - autonav_description_dir = get_package_share_directory("autonav_description") - - use_rviz_arg = DeclareLaunchArgument( - "rviz", - default_value="true", - ) - - rviz = LaunchConfiguration("rviz") - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("autonav_description"), - "urdf", - "autonav.xacro", - ] - ), - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - parameters=[robot_description], - ) - - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="screen", - arguments=["-d", os.path.join(autonav_description_dir, "rviz", "display.rviz")], - condition=IfCondition(rviz), - ) - - controller_params_file = os.path.join( - get_package_share_directory("autonav_controller"), - "config", - "autonav_controllers.yaml", - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, controller_params_file], - ) - - delayed_controller_manager = TimerAction(period=3.0, actions=[controller_manager]) - - autonav_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["autonav_controller", "--controller-manager", "/controller_manager"], - ) - - delayed_diff_drive_spawner = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=controller_manager, - on_start=[autonav_spawner], - ) - ) - - joint_broad_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_state_broadcaster", - "--controller-manager", - "/controller_manager", - ], - ) - - delayed_joint_broad_spawner = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=controller_manager, - on_start=[joint_broad_spawner], - ) - ) - - pid_controller = Node( - package="autonav_controller", - executable="pid_controller.py", - output='screen', - ) - - return LaunchDescription( - [ - use_rviz_arg, - robot_state_publisher_node, - rviz_node, - delayed_controller_manager, - delayed_diff_drive_spawner, - delayed_joint_broad_spawner, - # pid_controller, - ] - ) diff --git a/autonav_firmware/src/autonav_interface.cpp b/autonav_firmware/src/autonav_interface.cpp index dce5f94..33cbeec 100644 --- a/autonav_firmware/src/autonav_interface.cpp +++ b/autonav_firmware/src/autonav_interface.cpp @@ -1,6 +1,16 @@ -#include "autonav_firmware/autonav_interface.hpp" -#include -#include +// Copyright (c) 2024 Jatin Patil +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. #include #include @@ -9,12 +19,16 @@ #include #include +#include "autonav_firmware/autonav_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "std_msgs/msg/int64_multi_array.hpp" namespace autonav_firmware { -AutonavInterface::AutonavInterface() : node_(std::make_shared("autonav_interface_node")) +AutonavInterface::AutonavInterface() +: node_(std::make_shared("autonav_interface_node")) { feedback_subscription_ = node_->create_subscription( "/motor/feedback", 10, @@ -51,8 +65,7 @@ hardware_interface::CallbackReturn AutonavInterface::on_init( std::vector AutonavInterface::export_state_interfaces() { std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) - { + for (auto i = 0u; i < info_.joints.size(); i++) { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -65,8 +78,7 @@ std::vector AutonavInterface::export_state_i std::vector AutonavInterface::export_command_interfaces() { std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) - { + for (auto i = 0u; i < info_.joints.size(); i++) { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); } @@ -81,10 +93,8 @@ hardware_interface::CallbackReturn AutonavInterface::on_activate( RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Activating ...please wait..."); // set some default values - for (auto i = 0u; i < hw_positions_.size(); i++) - { - if (std::isnan(hw_positions_[i])) - { + for (auto i = 0u; i < hw_positions_.size(); i++) { + if (std::isnan(hw_positions_[i])) { hw_positions_[i] = 0; hw_velocities_[i] = 0; hw_commands_[i] = 0; @@ -123,7 +133,7 @@ hardware_interface::return_type AutonavInterface::read( return hardware_interface::return_type::OK; } -hardware_interface::return_type autonav_firmware ::AutonavInterface::write( +hardware_interface::return_type autonav_firmware::AutonavInterface::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto cmd_msg = std::make_shared(); @@ -137,5 +147,5 @@ hardware_interface::return_type autonav_firmware ::AutonavInterface::write( } // namespace autonav_firmware #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - autonav_firmware::AutonavInterface, hardware_interface::SystemInterface) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(autonav_firmware::AutonavInterface, + hardware_interface::SystemInterface) diff --git a/autonav_firmware/src/autonav_interface_backup.cpp b/autonav_firmware/src/autonav_interface_backup.cpp deleted file mode 100644 index 3b36e0a..0000000 --- a/autonav_firmware/src/autonav_interface_backup.cpp +++ /dev/null @@ -1,214 +0,0 @@ -#include "autonav_firmware/autonav_interface.hpp" - -#include -#include -#include -#include -#include -#include - -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace autonav_firmware -{ -AutonavInterface::AutonavInterface() -{ -} - -AutonavInterface::~AutonavInterface() -{ -} - -hardware_interface::CallbackReturn AutonavInterface::on_init( - const hardware_interface::HardwareInfo & info) -{ - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; - } - - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code -// hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); -// hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); - // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - - for (const hardware_interface::ComponentInfo & joint : info_.joints) - { - // DiffBotSystem has exactly two states and one command interface on each joint - if (joint.command_interfaces.size() != 1) - { - RCLCPP_FATAL( - rclcpp::get_logger("AutonavInterface"), - "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) - { - RCLCPP_FATAL( - rclcpp::get_logger("AutonavInterface"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.state_interfaces.size() != 2) - { - RCLCPP_FATAL( - rclcpp::get_logger("AutonavInterface"), - "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), - joint.state_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) - { - RCLCPP_FATAL( - rclcpp::get_logger("AutonavInterface"), - "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) - { - RCLCPP_FATAL( - rclcpp::get_logger("AutonavInterface"), - "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), - joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return hardware_interface::CallbackReturn::ERROR; - } - } - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector AutonavInterface::export_state_interfaces() -{ - std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); - } - - return state_interfaces; -} - -std::vector AutonavInterface::export_command_interfaces() -{ - std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); - } - - return command_interfaces; -} - -hardware_interface::CallbackReturn AutonavInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Activating ...please wait..."); - -// for (auto i = 0; i < hw_start_sec_; i++) -// { -// rclcpp::sleep_for(std::chrono::seconds(1)); -// RCLCPP_INFO( -// rclcpp::get_logger("AutonavInterface"), "%.1f seconds left...", hw_start_sec_ - i); -// } - // END: This part here is for exemplary purposes - Please do not copy to your production code - - // set some default values - for (auto i = 0u; i < hw_positions_.size(); i++) - { - if (std::isnan(hw_positions_[i])) - { - hw_positions_[i] = 0; - hw_velocities_[i] = 0; - hw_commands_[i] = 0; - } - } - - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Successfully activated!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn AutonavInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Deactivating ...please wait..."); - -// for (auto i = 0; i < hw_stop_sec_; i++) -// { -// rclcpp::sleep_for(std::chrono::seconds(1)); -// RCLCPP_INFO( -// rclcpp::get_logger("AutonavInterface"), "%.1f seconds left...", hw_stop_sec_ - i); -// } - // END: This part here is for exemplary purposes - Please do not copy to your production code - - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Successfully deactivated!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::return_type AutonavInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & period) -{ - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (std::size_t i = 0; i < hw_velocities_.size(); i++) - { - // Simulate DiffBot wheels's movement as a first-order system - // Update the joint status: this is a revolute joint without any limit. - // Simply integrates - hw_positions_[i] = hw_positions_[i] + period.seconds() * hw_velocities_[i]; - - RCLCPP_INFO( - rclcpp::get_logger("AutonavInterface"), - "Got position state %.5f and velocity state %.5f for '%s'!", hw_positions_[i], - hw_velocities_[i], info_.joints[i].name.c_str()); - } - // END: This part here is for exemplary purposes - Please do not copy to your production code - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type autonav_firmware ::AutonavInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Writing..."); - - for (auto i = 0u; i < hw_commands_.size(); i++) - { - // Simulate sending commands to the hardware - RCLCPP_INFO( - rclcpp::get_logger("AutonavInterface"), "Got command %.5f for '%s'!", hw_commands_[i], - info_.joints[i].name.c_str()); - - hw_velocities_[i] = hw_commands_[i]; - } - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Joints successfully written!"); - // END: This part here is for exemplary purposes - Please do not copy to your production code - - return hardware_interface::return_type::OK; -} - -} // namespace autonav_firmware - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - autonav_firmware::AutonavInterface, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/autonav_firmware/src/autonav_interface_serial.cpp b/autonav_firmware/src/autonav_interface_serial.cpp index ac2de31..5c8811c 100644 --- a/autonav_firmware/src/autonav_interface_serial.cpp +++ b/autonav_firmware/src/autonav_interface_serial.cpp @@ -1,183 +1,174 @@ +// Copyright (c) 2024 Jatin Patil +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + #include "autonav_firmware/autonav_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include + namespace autonav_firmware { - AutonavInterface::AutonavInterface() - { - } +AutonavInterface::AutonavInterface() +{ +} - AutonavInterface::~AutonavInterface() - { - if (esp_.isOpen()) - { - try - { - esp_.close(); - } - catch (...) - { - RCLCPP_FATAL_STREAM(rclcpp::get_logger("AutonavInterface"), "Something went wrong while closing connection with port " << port_); - } +AutonavInterface::~AutonavInterface() +{ + if (esp_.isOpen()) { + try { + esp_.close(); + } catch (...) { + RCLCPP_FATAL_STREAM(rclcpp::get_logger("AutonavInterface"), + "Something went wrong while closing connection with port " << port_); } } +} - hardware_interface::CallbackReturn AutonavInterface::on_init( - const hardware_interface::HardwareInfo &info) +hardware_interface::CallbackReturn AutonavInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; - } - - try - { - port_ = info_.hardware_parameters.at("port"); - } - catch (const std::out_of_range &e) - { - RCLCPP_FATAL(rclcpp::get_logger("AutonavInterface"), "No Serial Port provided! Aborting"); - return CallbackReturn::FAILURE; - } - - hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + return hardware_interface::CallbackReturn::ERROR; + } - return hardware_interface::CallbackReturn::SUCCESS; + try { + port_ = info_.hardware_parameters.at("port"); + } catch (const std::out_of_range & e) { + RCLCPP_FATAL(rclcpp::get_logger("AutonavInterface"), "No Serial Port provided! Aborting"); + return CallbackReturn::FAILURE; } - std::vector AutonavInterface::export_state_interfaces() - { - std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) - { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); - } + hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + return hardware_interface::CallbackReturn::SUCCESS; +} - return state_interfaces; +std::vector AutonavInterface::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i])); } - std::vector AutonavInterface::export_command_interfaces() - { - std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); - } + return state_interfaces; +} - return command_interfaces; +std::vector AutonavInterface::export_command_interfaces() +{ + std::vector command_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); } - hardware_interface::CallbackReturn AutonavInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) - { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Activating ...please wait..."); - - // set some default values - for (auto i = 0u; i < hw_positions_.size(); i++) - { - if (std::isnan(hw_positions_[i])) - { - hw_positions_[i] = 0; - hw_velocities_[i] = 0; - hw_commands_[i] = 0; - } - } + return command_interfaces; +} - try - { - esp_.setPort(port_); - esp_.setBaudrate(115200); - esp_.open(); - } - catch (...) - { - RCLCPP_FATAL_STREAM(rclcpp::get_logger("AutonavInterface"), - "Something went wrong while interacting with port " << port_); - return CallbackReturn::FAILURE; +hardware_interface::CallbackReturn AutonavInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Activating ...please wait..."); + + // set some default values + for (auto i = 0u; i < hw_positions_.size(); i++) { + if (std::isnan(hw_positions_[i])) { + hw_positions_[i] = 0; + hw_velocities_[i] = 0; + hw_commands_[i] = 0; } + } - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Successfully activated!"); - - return hardware_interface::CallbackReturn::SUCCESS; + try { + esp_.setPort(port_); + esp_.setBaudrate(115200); + esp_.open(); + } catch (...) { + RCLCPP_FATAL_STREAM(rclcpp::get_logger("AutonavInterface"), + "Something went wrong while interacting with port " << port_); + return CallbackReturn::FAILURE; } - hardware_interface::CallbackReturn AutonavInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) - { - // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Deactivating ...please wait..."); - - if (esp_.isOpen()) - { - try - { - esp_.close(); - } - catch (...) - { - RCLCPP_FATAL_STREAM(rclcpp::get_logger("AutonavInterface"), "Something went wrong while closing connection with port " << port_); - } - } + RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Successfully activated!"); - RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Successfully deactivated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} - return hardware_interface::CallbackReturn::SUCCESS; - } +hardware_interface::CallbackReturn AutonavInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Deactivating ...please wait..."); - hardware_interface::return_type AutonavInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) - { - // if (esp_.isDataAvailable()) - // { - std::string message; - esp_.readline(message); - size_t commaIndex = message.find(','); - if (commaIndex != std::string::npos) - { - std::string leftPos = message.substr(0, commaIndex); - std::string rightPos = message.substr(commaIndex + 1); - - // Convert the strings to float variables - hw_positions_[1] = std::stof(leftPos) * 2 * M_PI / 1600.0; - hw_positions_[0] = std::stof(rightPos) * 2 * M_PI / 1600.0; + if (esp_.isOpen()) { + try { + esp_.close(); + } catch (...) { + RCLCPP_FATAL_STREAM(rclcpp::get_logger("AutonavInterface"), + "Something went wrong while closing connection with port " << port_); } - // } - return hardware_interface::return_type::OK; } - hardware_interface::return_type autonav_firmware ::AutonavInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) - { - std::stringstream ss; - auto leftCmd = hw_commands_[1] * 1600.0 / (2 * M_PI); - auto rightCmd = hw_commands_[0] * 1600.0 / (2 * M_PI); - ss << leftCmd << "," << rightCmd << "\n"; - try - { - esp_.write(ss.str()); - } - catch (...) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("BumperbotInterface"), "Something went wrong while sending the message " << ss.str() << " to the port " << port_); - return hardware_interface::return_type::ERROR; - } + RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Successfully deactivated!"); - // RCLCPP_INFO(rclcpp::get_logger("AutonavInterface"), "Velocity successfully written!"); + return hardware_interface::CallbackReturn::SUCCESS; +} - return hardware_interface::return_type::OK; +hardware_interface::return_type AutonavInterface::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + std::string message; + esp_.readline(message); + size_t commaIndex = message.find(','); + if (commaIndex != std::string::npos) { + std::string leftPos = message.substr(0, commaIndex); + std::string rightPos = message.substr(commaIndex + 1); + + // Convert the strings to float variables + hw_positions_[1] = std::stof(leftPos) * 2 * M_PI / 1600.0; + hw_positions_[0] = std::stof(rightPos) * 2 * M_PI / 1600.0; } + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type autonav_firmware::AutonavInterface::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + std::stringstream ss; + auto leftCmd = hw_commands_[1] * 1600.0 / (2 * M_PI); + auto rightCmd = hw_commands_[0] * 1600.0 / (2 * M_PI); + ss << leftCmd << "," << rightCmd << "\n"; + try { + esp_.write(ss.str()); + } catch (...) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("BumperbotInterface"), + "Something went wrong while sending the message " << ss.str() << " to the port " << port_); + return hardware_interface::return_type::ERROR; + } + + return hardware_interface::return_type::OK; +} -} // namespace autonav_firmware +} // namespace autonav_firmware #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(autonav_firmware::AutonavInterface, hardware_interface::SystemInterface) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(autonav_firmware::AutonavInterface, + hardware_interface::SystemInterface) diff --git a/autonav_localization/launch/ekf.launch.py b/autonav_localization/launch/ekf.launch.py index 0da0a1f..02ea1ed 100644 --- a/autonav_localization/launch/ekf.launch.py +++ b/autonav_localization/launch/ekf.launch.py @@ -1,17 +1,14 @@ import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition, UnlessCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration, PythonExpression from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): pkg_autonav_localization = get_package_share_directory('autonav_localization') - robot_localization_file_path = os.path.join(pkg_autonav_localization, 'config/ekf.yaml') + robot_localization_file_path = os.path.join(pkg_autonav_localization, 'config/ekf.yaml') start_robot_localization_cmd = Node( package='robot_localization', @@ -19,7 +16,7 @@ def generate_launch_description(): name='ekf_filter_node', output='screen', parameters=[robot_localization_file_path]) - + return LaunchDescription([ start_robot_localization_cmd - ]) \ No newline at end of file + ]) diff --git a/autonav_navigation/launch/cartographer.launch.py b/autonav_navigation/launch/cartographer.launch.py index c006726..dee3301 100644 --- a/autonav_navigation/launch/cartographer.launch.py +++ b/autonav_navigation/launch/cartographer.launch.py @@ -1,104 +1,102 @@ import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ThisLaunchFileDir from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node def generate_launch_description(): - use_sim_time = LaunchConfiguration("use_sim_time", default="false") + use_sim_time = LaunchConfiguration('use_sim_time', default='false') - autonav_navigation_prefix = get_package_share_directory("autonav_navigation") + autonav_navigation_prefix = get_package_share_directory('autonav_navigation') cartographer_config_dir = LaunchConfiguration( - "cartographer_config_dir", - default=os.path.join(autonav_navigation_prefix, "config"), + 'cartographer_config_dir', + default=os.path.join(autonav_navigation_prefix, 'config'), ) configuration_basename = LaunchConfiguration( - "configuration_basename", default="cartographer.lua" + 'configuration_basename', default='cartographer.lua' ) - resolution = LaunchConfiguration("resolution", default="0.05") + resolution = LaunchConfiguration('resolution', default='0.05') - publish_period_sec = LaunchConfiguration("publish_period_sec", default="1.0") + publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0') rviz_config_dir = os.path.join( - get_package_share_directory("autonav_navigation"), - "rviz", - "cartographer.rviz", + get_package_share_directory('autonav_navigation'), + 'rviz', + 'cartographer.rviz', ) return LaunchDescription( [ DeclareLaunchArgument( - name="rviz", default_value="false", description="Run rviz" + name='rviz', default_value='false', description='Run rviz' ), DeclareLaunchArgument( - "cartographer_config_dir", + 'cartographer_config_dir', default_value=cartographer_config_dir, - description="Full path to config file to load", + description='Full path to config file to load', ), DeclareLaunchArgument( - "configuration_basename", + 'configuration_basename', default_value=configuration_basename, - description="Name of lua file for cartographer", + description='Name of lua file for cartographer', ), DeclareLaunchArgument( - "use_sim_time", - default_value="false", - description="Use simulation (Gazebo) clock if true", + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true', ), Node( - package="cartographer_ros", - executable="cartographer_node", - name="cartographer_node", - output="screen", - parameters=[{"use_sim_time": use_sim_time}], + package='cartographer_ros', + executable='cartographer_node', + name='cartographer_node', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], arguments=[ - "-configuration_directory", + '-configuration_directory', cartographer_config_dir, - "-configuration_basename", + '-configuration_basename', configuration_basename, ], - remappings=[("/odom", "/odometry/filtered")], + remappings=[('/odom', '/odometry/filtered')], ), DeclareLaunchArgument( - "resolution", + 'resolution', default_value=resolution, - description="Resolution of a grid cell in the published occupancy grid", + description='Resolution of a grid cell in the published occupancy grid', ), DeclareLaunchArgument( - "publish_period_sec", + 'publish_period_sec', default_value=publish_period_sec, - description="OccupancyGrid publishing period", + description='OccupancyGrid publishing period', ), Node( - package="cartographer_ros", - executable="cartographer_occupancy_grid_node", - name="cartographer_occupancy_grid_node", - output="screen", - parameters=[{"use_sim_time": use_sim_time}], + package='cartographer_ros', + executable='cartographer_occupancy_grid_node', + name='cartographer_occupancy_grid_node', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], arguments=[ - "-resolution", + '-resolution', resolution, - "-publish_period_sec", + '-publish_period_sec', publish_period_sec, ], ), Node( - package="rviz2", - executable="rviz2", - name="rviz2", - arguments=["-d", rviz_config_dir], - parameters=[{"use_sim_time": use_sim_time}], - condition=IfCondition(LaunchConfiguration("rviz")), - output="screen", + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + parameters=[{'use_sim_time': use_sim_time}], + condition=IfCondition(LaunchConfiguration('rviz')), + output='screen', ), ] ) diff --git a/autonav_navigation/launch/navigation.launch.py b/autonav_navigation/launch/navigation.launch.py index b60b508..eefd117 100644 --- a/autonav_navigation/launch/navigation.launch.py +++ b/autonav_navigation/launch/navigation.launch.py @@ -1,20 +1,19 @@ #!/usr/bin/python3 -import os from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.conditions import IfCondition -from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare -MAP_NAME='L1012' #change to the name of your own map here +MAP_NAME = 'L1012' # change to the name of your own map here -MAP_NAME=input("Map name: ") +MAP_NAME = input('Map name: ') -def generate_launch_description(): +def generate_launch_description(): nav2_launch_path = PathJoinSubstitution( [FindPackageShare('nav2_bringup'), 'launch', 'bringup_launch.py'] ) @@ -33,19 +32,19 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( - name='sim', + name='sim', default_value='false', description='Enable use_sime_time to true' ), DeclareLaunchArgument( - name='rviz', + name='rviz', default_value='false', description='Run rviz' ), - DeclareLaunchArgument( - name='map', + DeclareLaunchArgument( + name='map', default_value=default_map_path, description='Navigation map path' ), @@ -53,8 +52,8 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(nav2_launch_path), launch_arguments={ - 'map': LaunchConfiguration("map"), - 'use_sim_time': LaunchConfiguration("sim"), + 'map': LaunchConfiguration('map'), + 'use_sim_time': LaunchConfiguration('sim'), 'params_file': nav2_config_path }.items() ), @@ -65,7 +64,7 @@ def generate_launch_description(): name='rviz2', output='screen', arguments=['-d', rviz_config_path], - condition=IfCondition(LaunchConfiguration("rviz")), - parameters=[{'use_sim_time': LaunchConfiguration("sim")}] + condition=IfCondition(LaunchConfiguration('rviz')), + parameters=[{'use_sim_time': LaunchConfiguration('sim')}] ) - ]) \ No newline at end of file + ]) diff --git a/autonav_navigation/launch/save_map.launch.py b/autonav_navigation/launch/save_map.launch.py index bca3bb2..4f2fc17 100644 --- a/autonav_navigation/launch/save_map.launch.py +++ b/autonav_navigation/launch/save_map.launch.py @@ -1,10 +1,9 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +MAP_NAME = input('Enter map file name:') -MAP_NAME = input("Enter map file name:") def generate_launch_description(): # Declare a launch argument for the map file name and directory @@ -18,13 +17,11 @@ def generate_launch_description(): default_value=f'/colcon_ws/src/autonav_navigation/maps/{MAP_NAME}', description='Directory to save the map file.') - # Retrieve the launch argument values - map_file_name = LaunchConfiguration('map_file_name') map_directory = LaunchConfiguration('map_directory') # Define the map_saver_cli node execution map_saver_cli = ExecuteProcess( - cmd=['ros2', 'run', 'nav2_map_server', 'map_saver_cli', '-f', + cmd=['ros2', 'run', 'nav2_map_server', 'map_saver_cli', '-f', map_directory], shell=True ) @@ -34,4 +31,4 @@ def generate_launch_description(): map_file_name_arg, map_directory_arg, map_saver_cli - ]) \ No newline at end of file + ]) diff --git a/autonav_navigation/launch/slam.launch.py b/autonav_navigation/launch/slam.launch.py index 47a666f..e6634fc 100644 --- a/autonav_navigation/launch/slam.launch.py +++ b/autonav_navigation/launch/slam.launch.py @@ -1,14 +1,10 @@ - -import os from launch import LaunchDescription -from launch import LaunchContext from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.conditions import IfCondition -from launch.substitutions import EnvironmentVariable -from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -25,7 +21,7 @@ def generate_launch_description(): ) nav2_config_path = PathJoinSubstitution( - [FindPackageShare('autonav_navigation'), 'config', 'navigation.yaml'] + [FindPackageShare('autonav_navigation'), 'config', 'navigation.yaml'] ) rviz_config_path = PathJoinSubstitution( @@ -35,22 +31,22 @@ def generate_launch_description(): default_map_path = PathJoinSubstitution( [FindPackageShare('autonav_navigation'), 'maps', 'L1012.yaml'] ) - + return LaunchDescription([ DeclareLaunchArgument( - name='sim', + name='sim', default_value='false', description='Enable use_sime_time to true' ), DeclareLaunchArgument( - name='rviz', + name='rviz', default_value='false', description='Run rviz' ), DeclareLaunchArgument( - name='map', + name='map', default_value=default_map_path, description='Navigation map path' ), @@ -58,8 +54,8 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(navigation_launch_path), launch_arguments={ - 'map': LaunchConfiguration("map"), - 'use_sim_time': LaunchConfiguration("sim"), + 'map': LaunchConfiguration('map'), + 'use_sim_time': LaunchConfiguration('sim'), 'params_file': nav2_config_path }.items() ), @@ -67,7 +63,7 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(slam_launch_path), launch_arguments={ - 'use_sim_time': LaunchConfiguration("sim"), + 'use_sim_time': LaunchConfiguration('sim'), 'params_file': slam_config_path }.items() ), @@ -78,7 +74,7 @@ def generate_launch_description(): name='rviz2', output='screen', arguments=['-d', rviz_config_path], - condition=IfCondition(LaunchConfiguration("rviz")), - parameters=[{'use_sim_time': LaunchConfiguration("sim")}] + condition=IfCondition(LaunchConfiguration('rviz')), + parameters=[{'use_sim_time': LaunchConfiguration('sim')}] ) - ]) \ No newline at end of file + ]) diff --git a/autonav_perception/autonav_perception/camera_publish.py b/autonav_perception/autonav_perception/camera_publish.py index ecbb7f8..68fb27a 100755 --- a/autonav_perception/autonav_perception/camera_publish.py +++ b/autonav_perception/autonav_perception/camera_publish.py @@ -1,13 +1,15 @@ #!/usr/bin/env python3 + +import cv2 +from cv_bridge import CvBridge import rclpy import rclpy.clock from rclpy.node import Node -from sensor_msgs.msg import Image, CompressedImage -import cv2 -from cv_bridge import CvBridge -import numpy as np +from sensor_msgs.msg import CompressedImage + class ImagePublisher(Node): + def __init__(self): super().__init__('image_publisher') self.publisher_ = self.create_publisher(CompressedImage, '/camera', 10) @@ -25,9 +27,10 @@ def publish_image(self): frame_resized = cv2.resize(frame, (self.fixed_width, self.fixed_height)) ros_image = self.bridge.cv2_to_compressed_imgmsg(frame_resized) ros_image.header.stamp = self.get_clock().now().to_msg() # Set the timestamp - ros_image.format = "jpeg" + ros_image.format = 'jpeg' self.publisher_.publish(ros_image) + def main(args=None): rclpy.init(args=args) image_publisher = ImagePublisher() @@ -35,5 +38,6 @@ def main(args=None): image_publisher.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() diff --git a/autonav_perception/autonav_perception/camera_subscribe.py b/autonav_perception/autonav_perception/camera_subscribe.py index 8c0edb9..9d82bcd 100755 --- a/autonav_perception/autonav_perception/camera_subscribe.py +++ b/autonav_perception/autonav_perception/camera_subscribe.py @@ -1,12 +1,14 @@ #!/usr/bin/env python3 +import cv2 +from cv_bridge import CvBridge import rclpy from rclpy.node import Node -from sensor_msgs.msg import Image, CompressedImage -from cv_bridge import CvBridge -import cv2 +from sensor_msgs.msg import CompressedImage + class ImageSubscriber(Node): + def __init__(self): super().__init__('image_subscriber') self.subscription = self.create_subscription( @@ -19,11 +21,12 @@ def __init__(self): def image_callback(self, msg): try: cv_image = self.bridge.compressed_imgmsg_to_cv2(msg) - cv2.imshow("Image", cv_image) + cv2.imshow('Image', cv_image) cv2.waitKey(1) except Exception as e: print(e) + def main(args=None): rclpy.init(args=args) image_subscriber = ImageSubscriber() @@ -31,5 +34,6 @@ def main(args=None): image_subscriber.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main()