From 3e0161d01623d41bf407e73dc749a6aca5b7c4df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= Date: Wed, 7 Jun 2023 11:44:38 +0200 Subject: [PATCH] update --- config/diffbot_controllers.yaml | 87 ++++++++++++++++ config/mapper_params_online_async.yaml | 73 ++++++++++++++ launch/cam.launch.py | 110 +++++++++++++++++++++ launch/controller.launch.py | 110 +++++++++++++++++++++ launch/joy_teleop.launch.py | 110 +++++++++++++++++++++ launch/lidar.launch.py | 110 +++++++++++++++++++++ launch/mapper.launch.py | 131 +++++++++++++++++++++++++ launch/rsp.launch.py | 110 +++++++++++++++++++++ setup.py | 1 + 9 files changed, 842 insertions(+) create mode 100644 config/diffbot_controllers.yaml create mode 100644 config/mapper_params_online_async.yaml create mode 100644 launch/cam.launch.py create mode 100644 launch/controller.launch.py create mode 100644 launch/joy_teleop.launch.py create mode 100644 launch/lidar.launch.py create mode 100644 launch/mapper.launch.py create mode 100644 launch/rsp.launch.py diff --git a/config/diffbot_controllers.yaml b/config/diffbot_controllers.yaml new file mode 100644 index 0000000..6e51218 --- /dev/null +++ b/config/diffbot_controllers.yaml @@ -0,0 +1,87 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + diffbot_base_controller: + type: diff_drive_controller/DiffDriveController + +diffbot_base_controller: + ros__parameters: + # left_wheel_names: ["left_wheel_joint"] + # right_wheel_names: ["right_wheel_joint"] + + right_wheel_names: ["left_wheel_joint"] # needed to switch names because motors are connected to the wrong slots + left_wheel_names: ["right_wheel_joint"] + + wheel_separation: 0.28 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.065 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: -1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 100.0 + odom_frame_id: odom + base_frame_id: base_link + # pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + # twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + pose_covariance_diagonal: + [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: + [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] # changed from https://github.com/kallaspriit/rosbot/blob/main/ros/src/rosbot_description/config/diff_drive_controller.yaml + + open_loop: false + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + velocity_rolling_window_size: 10 # changed from https://github.com/kallaspriit/rosbot/blob/main/ros/src/rosbot_description/config/diff_drive_controller.yaml + + # preserve turning radius when limiting speed/acceleration/jerk + preserve_turning_radius: false # changed from https://github.com/kallaspriit/rosbot/blob/main/ros/src/rosbot_description/config/diff_drive_controller.yaml + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + # linear.x.has_velocity_limits: false + # linear.x.has_acceleration_limits: false + # linear.x.has_jerk_limits: false + # linear.x.max_velocity: 2.0 + # linear.x.min_velocity: -2.0 + # linear.x.max_acceleration: 5.0 + # linear.x.max_jerk: 0.0 + # linear.x.min_jerk: 0.0 + + # angular.z.has_velocity_limits: false + # angular.z.has_acceleration_limits: false + # angular.z.has_jerk_limits: false + # angular.z.max_velocity: 5.0 + # angular.z.min_velocity: -5.0 + # angular.z.max_acceleration: 10.0 + # angular.z.min_acceleration: -10.0 + # angular.z.max_jerk: 0.0 + # angular.z.min_jerk: 0.0 + + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: false + linear.x.has_jerk_limits: false + linear.x.max_velocity: 2.0 + linear.x.min_velocity: -2.0 + linear.x.min_acceleration: -5.0 + linear.x.max_acceleration: 5.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 6.28 + angular.z.min_velocity: -6.28 + angular.z.max_acceleration: 25.0 + angular.z.min_acceleration: -25.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml new file mode 100644 index 0000000..f6cad1e --- /dev/null +++ b/config/mapper_params_online_async.yaml @@ -0,0 +1,73 @@ +slam_toolbox: + ros__parameters: + + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + mode: localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + map_file_name: /home/bjorn/Documents/ros-projects/cps_bot_mini_ws/my_map_serial + # map_start_pose: [0.0, 0.0, 0.0] + map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + max_laser_range: 200.0 #for rastering images, standard was 20 + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/launch/cam.launch.py b/launch/cam.launch.py new file mode 100644 index 0000000..f3b7a60 --- /dev/null +++ b/launch/cam.launch.py @@ -0,0 +1,110 @@ +# Copyright 2022 Factor Robotics +# +# 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. + +import os +from launch import LaunchDescription +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import LifecycleNode +from launch_ros.descriptions import ParameterValue + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("bot_mini_description"), + "urdf", + "odrive_diffbot.urdf.xacro" + ] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "diffbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "-c", "/controller_manager"], + ) + + joystick_spawner = Node( + package="joy", + executable="joy_node" + ) + + teleop_spawner = Node( + package="rmp220_teleop", + executable="rmp220_teleop" + ) + + cam_node = Node( + package="ros2_cam_openCV", + executable="cam_node" + ) + + lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_node = LifecycleNode( + package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[lidar_dir], + ) + + return LaunchDescription([ + #control_node, + #robot_state_pub_node, + #joint_state_broadcaster_spawner, + #robot_controller_spawner, + #joystick_spawner, + #teleop_spawner, + cam_node, + #lidar_node + ]) \ No newline at end of file diff --git a/launch/controller.launch.py b/launch/controller.launch.py new file mode 100644 index 0000000..2a8f348 --- /dev/null +++ b/launch/controller.launch.py @@ -0,0 +1,110 @@ +# Copyright 2022 Factor Robotics +# +# 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. + +import os +from launch import LaunchDescription +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import LifecycleNode +from launch_ros.descriptions import ParameterValue + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("bot_mini_description"), + "urdf", + "odrive_diffbot.urdf.xacro" + ] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "diffbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "-c", "/controller_manager"], + ) + + joystick_spawner = Node( + package="joy", + executable="joy_node" + ) + + teleop_spawner = Node( + package="rmp220_teleop", + executable="rmp220_teleop" + ) + + cam_node = Node( + package="ros2_cam_openCV", + executable="cam_node" + ) + + lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_node = LifecycleNode( + package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[lidar_dir], + ) + + return LaunchDescription([ + control_node, + #robot_state_pub_node, + joint_state_broadcaster_spawner, + robot_controller_spawner, + #joystick_spawner, + #teleop_spawner, + #cam_node, + #lidar_node + ]) \ No newline at end of file diff --git a/launch/joy_teleop.launch.py b/launch/joy_teleop.launch.py new file mode 100644 index 0000000..46a45b4 --- /dev/null +++ b/launch/joy_teleop.launch.py @@ -0,0 +1,110 @@ +# Copyright 2022 Factor Robotics +# +# 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. + +import os +from launch import LaunchDescription +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import LifecycleNode +from launch_ros.descriptions import ParameterValue + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("bot_mini_description"), + "urdf", + "odrive_diffbot.urdf.xacro" + ] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "diffbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "-c", "/controller_manager"], + ) + + joystick_spawner = Node( + package="joy", + executable="joy_node" + ) + + teleop_spawner = Node( + package="rmp220_teleop", + executable="rmp220_teleop" + ) + + cam_node = Node( + package="ros2_cam_openCV", + executable="cam_node" + ) + + lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_node = LifecycleNode( + package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[lidar_dir], + ) + + return LaunchDescription([ + #control_node, + #robot_state_pub_node, + #joint_state_broadcaster_spawner, + #robot_controller_spawner, + joystick_spawner, + teleop_spawner, + #cam_node, + #lidar_node + ]) \ No newline at end of file diff --git a/launch/lidar.launch.py b/launch/lidar.launch.py new file mode 100644 index 0000000..d64648c --- /dev/null +++ b/launch/lidar.launch.py @@ -0,0 +1,110 @@ +# Copyright 2022 Factor Robotics +# +# 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. + +import os +from launch import LaunchDescription +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import LifecycleNode +from launch_ros.descriptions import ParameterValue + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("bot_mini_description"), + "urdf", + "odrive_diffbot.urdf.xacro" + ] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "diffbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "-c", "/controller_manager"], + ) + + joystick_spawner = Node( + package="joy", + executable="joy_node" + ) + + teleop_spawner = Node( + package="rmp220_teleop", + executable="rmp220_teleop" + ) + + cam_node = Node( + package="ros2_cam_openCV", + executable="cam_node" + ) + + lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_node = LifecycleNode( + package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[lidar_dir], + ) + + return LaunchDescription([ + #control_node, + #robot_state_pub_node, + #joint_state_broadcaster_spawner, + #robot_controller_spawner, + #joystick_spawner, + #teleop_spawner, + #cam_node, + lidar_node + ]) \ No newline at end of file diff --git a/launch/mapper.launch.py b/launch/mapper.launch.py new file mode 100644 index 0000000..293c970 --- /dev/null +++ b/launch/mapper.launch.py @@ -0,0 +1,131 @@ +# Copyright 2022 Factor Robotics +# +# 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. + +import os +from launch import LaunchDescription +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import LifecycleNode +from launch_ros.descriptions import ParameterValue +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("bot_mini_description"), + "urdf", + "odrive_diffbot.urdf.xacro" + ] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "diffbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "-c", "/controller_manager"], + ) + + joystick_spawner = Node( + package="joy", + executable="joy_node" + ) + + teleop_spawner = Node( + package="rmp220_teleop", + executable="rmp220_teleop" + ) + + cam_node = Node( + package="ros2_cam_openCV", + executable="cam_node" + ) + + use_sim_time = True + slam_params_file = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "mapper_params_online_async.yaml" + ] + ) + mapper_node = Node( + package="slam_toolbox", + executable="async_slam_toolbox_node", + name='slam_toolbox_node', + output='screen', + parameters=[ + slam_params_file, + {'use_sim_time': use_sim_time} + ], + ) + + lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_node = LifecycleNode( + package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[lidar_dir], + ) + + return LaunchDescription([ + #control_node, + #robot_state_pub_node, + #joint_state_broadcaster_spawner, + #robot_controller_spawner, + #joystick_spawner, + #teleop_spawner, + #cam_node, + #lidar_node, + mapper_node + ]) \ No newline at end of file diff --git a/launch/rsp.launch.py b/launch/rsp.launch.py new file mode 100644 index 0000000..4dbe3a5 --- /dev/null +++ b/launch/rsp.launch.py @@ -0,0 +1,110 @@ +# Copyright 2022 Factor Robotics +# +# 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. + +import os +from launch import LaunchDescription +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import LifecycleNode +from launch_ros.descriptions import ParameterValue + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("bot_mini_description"), + "urdf", + "odrive_diffbot.urdf.xacro" + ] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("bot_mini_bringup"), + "config", + "diffbot_controllers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diffbot_base_controller", "-c", "/controller_manager"], + ) + + joystick_spawner = Node( + package="joy", + executable="joy_node" + ) + + teleop_spawner = Node( + package="rmp220_teleop", + executable="rmp220_teleop" + ) + + cam_node = Node( + package="ros2_cam_openCV", + executable="cam_node" + ) + + lidar_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'params', 'lsx10.yaml') + lidar_node = LifecycleNode( + package='lslidar_driver', + executable='lslidar_driver_node', + name='lslidar_driver_node', + output='screen', + emulate_tty=True, + namespace='', + parameters=[lidar_dir], + ) + + return LaunchDescription([ + #control_node, + robot_state_pub_node, + #joint_state_broadcaster_spawner, + #robot_controller_spawner, + #joystick_spawner, + #teleop_spawner, + #cam_node, + #lidar_node + ]) \ No newline at end of file diff --git a/setup.py b/setup.py index 54c3d91..8af784f 100644 --- a/setup.py +++ b/setup.py @@ -13,6 +13,7 @@ setup( ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')) ], install_requires=['setuptools'], zip_safe=True,