From af473e68f01c99db2877e07b5d2f42580a3792a2 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 15 Nov 2024 19:34:32 +0100 Subject: [PATCH] cyclonedds config Signed-off-by: Jakub Delicat --- .env | 2 +- 10-cyclone-max.conf | 6 + Dockerfile.pioneer | 30 ++ Makefile | 12 +- README.md | 16 + build_devcontainer.sh | 2 +- compose.decawave.yaml | 5 +- compose.hokuyo.yaml | 5 +- compose.navigation2.yaml | 26 ++ compose.pioneer.yaml | 35 -- compose.ros2aria.yaml | 5 +- config/nav2_params.yaml | 456 ++++++++++++++++++++++++++ cyclonedds.xml | 51 +++ cyclonedds_pc.xml | 48 +++ docker-compose@.service | 3 + husarion_utils/bringup_launch.py | 184 +++++++++++ husarion_utils/localization_launch.py | 193 +++++++++++ husarion_utils/navigation_launch.py | 279 ++++++++++++++++ husarion_utils/slam_launch.py | 145 ++++++++ install.sh | 45 +++ pioneer_status.py | 136 ++++++++ 21 files changed, 1638 insertions(+), 46 deletions(-) create mode 100644 10-cyclone-max.conf create mode 100644 Dockerfile.pioneer create mode 100644 compose.navigation2.yaml delete mode 100644 compose.pioneer.yaml create mode 100644 config/nav2_params.yaml create mode 100644 cyclonedds.xml create mode 100644 cyclonedds_pc.xml create mode 100644 husarion_utils/bringup_launch.py create mode 100644 husarion_utils/localization_launch.py create mode 100644 husarion_utils/navigation_launch.py create mode 100644 husarion_utils/slam_launch.py create mode 100644 install.sh create mode 100644 pioneer_status.py diff --git a/.env b/.env index 06bc912..840f29b 100644 --- a/.env +++ b/.env @@ -1 +1 @@ -PIONEER_ID=5 \ No newline at end of file +PIONEER_ID=4 \ No newline at end of file diff --git a/10-cyclone-max.conf b/10-cyclone-max.conf new file mode 100644 index 0000000..0788782 --- /dev/null +++ b/10-cyclone-max.conf @@ -0,0 +1,6 @@ +# Increase the maximum receive buffer size for network packets +net.core.rmem_max=2147483647 # 2 GiB, default is 208 KiB + +# IP fragmentation settings +net.ipv4.ipfrag_time=3 # in seconds, default is 30 s +net.ipv4.ipfrag_high_thresh=134217728 # 128 MiB, default is 256 KiB \ No newline at end of file diff --git a/Dockerfile.pioneer b/Dockerfile.pioneer new file mode 100644 index 0000000..e826a04 --- /dev/null +++ b/Dockerfile.pioneer @@ -0,0 +1,30 @@ +ARG ROS_DISTRO=jazzy +FROM delicjusz/ros2aria:$ROS_DISTRO AS pkg-builder +SHELL ["/bin/bash", "-c"] + +WORKDIR /ros2_ws +COPY ./src /ros2_ws/src +RUN apt update && apt install -y ros-dev-tools && source /opt/ros/$ROS_DISTRO/setup.bash && \ + rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ + rosdep init && \ + rosdep update --rosdistro $ROS_DISTRO && \ + rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \ + colcon build + +FROM delicjusz/ros2aria:$ROS_DISTRO +ARG ROS_DISTRO + +SHELL ["/bin/bash", "-c"] +WORKDIR /ros2_ws + +COPY --from=pkg-builder /ros2_ws /ros2_ws +RUN apt-get update && apt-get install -y ros-dev-tools && \ + rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ + rosdep init && \ + rosdep update --rosdistro $ROS_DISTRO && \ + rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y + +RUN apt-get clean && \ + apt-get remove -y \ + ros-dev-tools && \ + rm -rf /var/lib/apt/lists/* \ No newline at end of file diff --git a/Makefile b/Makefile index 00014b4..dcdd212 100644 --- a/Makefile +++ b/Makefile @@ -11,7 +11,7 @@ build: # .uploaded: build/ros2aria/ros2aria upload: - rsync -r . lab1_5@pionier2:~/ros2aria + rsync -r . lab1_5@pionier4:~/ros2aria touch .uploaded # upload: .uploaded @@ -25,13 +25,13 @@ legacy: push_ros2aria: ./build_devcontainer.sh - docker push delicjusz/ros2aria + docker push delicjusz/ros2aria:jazzy push: - docker build -f Dockerfile -t delicjusz/pioneer . - docker push delicjusz/pioneer - docker build -f Dockerfile.hokuyo -t delicjusz/hokuyo . - docker push delicjusz/hokuyo + docker build -f Dockerfile.pioneer -t delicjusz/pioneer:jazzy . + docker push delicjusz/pioneer:jazzy + docker build -f Dockerfile.hokuyo -t delicjusz/hokuyo:jazzy . + docker push delicjusz/hokuyo:jazzy pull: docker pull delicjusz/ros2aria rosbridge: diff --git a/README.md b/README.md index 4333b71..dd67c8c 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,7 @@ # Instruction on station + +## FastRTPS + Export discovery server configuration file: ```bash export FASTRTPS_DEFAULT_PROFILES_FILE=$(pwd)/fastdds.xml @@ -14,6 +17,19 @@ Export fastdds ros middleware implementation: export RMW_IMPLEMENTATION=rmw_fastrtps_cpp ``` +Restart ROS 2 daemon: +``` +ros2 daemon stop +ros2 daemon start +``` + +## Cyclonedds + +``` +export CYCLONEDDS_URI=file://$(pwd)/cyclonedds_pc.xml +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +``` + Restart ROS 2 daemon: ``` ros2 daemon stop diff --git a/build_devcontainer.sh b/build_devcontainer.sh index b3a7ccf..423575b 100755 --- a/build_devcontainer.sh +++ b/build_devcontainer.sh @@ -1,3 +1,3 @@ #!/bin/sh -docker build -f Dockerfile.aria -t delicjusz/ros2aria . +docker build -f Dockerfile.aria -t delicjusz/ros2aria:jazzy . diff --git a/compose.decawave.yaml b/compose.decawave.yaml index 2ee071c..95cb071 100644 --- a/compose.decawave.yaml +++ b/compose.decawave.yaml @@ -4,11 +4,14 @@ services: image: delicjusz/decawave network_mode: host ipc: host + restart: unless-stopped environment: - - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml + - CYCLONEDDS_URI=file:///cyclonedds.xml volumes: - ./fastdds.xml:/fastdds.xml + - ./cyclonedds.xml:/cyclonedds.xml devices: - /dev/decawave command: ros2 launch decawave_driver decawave.launch.py namespace:=pioneer${PIONEER_ID}/ diff --git a/compose.hokuyo.yaml b/compose.hokuyo.yaml index c3b4218..68c4815 100644 --- a/compose.hokuyo.yaml +++ b/compose.hokuyo.yaml @@ -4,11 +4,14 @@ services: image: delicjusz/hokuyo network_mode: host ipc: host + restart: unless-stopped environment: - - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml + - CYCLONEDDS_URI=file:///cyclonedds.xml volumes: - ./fastdds.xml:/fastdds.xml + - ./cyclonedds.xml:/cyclonedds.xml devices: - /dev/hokuyo command: > diff --git a/compose.navigation2.yaml b/compose.navigation2.yaml new file mode 100644 index 0000000..8534d0b --- /dev/null +++ b/compose.navigation2.yaml @@ -0,0 +1,26 @@ +# docker compose -f compose.navigation2.yaml up + +services: + navigation: + image: husarion/navigation2:humble-1.1.16-20240927 + network_mode: host + ipc: host + restart: unless-stopped + volumes: + - ./config/nav2_params.yaml:/nav2_params.yaml + - ./maps:/maps + - ./husarion_utils:/husarion_utils + - ./fastdds.xml:/fastdds.xml + - ./cyclonedds.xml:/cyclonedds.xml + environment: + - CYCLONEDDS_URI=file:///cyclonedds.xml + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + - SAVE_MAP_PERIOD=${SAVE_MAP_PERIOD-10} + command: > + ros2 launch /husarion_utils/bringup_launch.py + namespace:=pioneer${PIONEER_ID} + observation_topic:=scan + map:=/maps/map.yaml + params_file:=/nav2_params.yaml + slam:=${SLAM:-False} + use_sim_time:=False \ No newline at end of file diff --git a/compose.pioneer.yaml b/compose.pioneer.yaml deleted file mode 100644 index 39bd4af..0000000 --- a/compose.pioneer.yaml +++ /dev/null @@ -1,35 +0,0 @@ -# docker compose -f compose.ros2aria.yaml up -services: - ros2aria-dev: - image: delicjusz/pioneer - network_mode: host - ipc: host - environment: - - RMW_IMPLEMENTATION=rmw_fastrtps_cpp - - LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib - - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml - - ROS_DISCOVERY_SERVER=10.104.16.240:11811 - volumes: - - ./fastdds.xml:/fastdds.xml - devices: - - /dev/ttyS0 - command: ros2 launch pioneer_bringup robot.launch.py namespace:=pioneer${PIONEER_ID}/ - - hokuyo: - image: delicjusz/hokuyo - network_mode: host - ipc: host - environment: - - RMW_IMPLEMENTATION=rmw_fastrtps_cpp - - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml - - ROS_DISCOVERY_SERVER=10.104.16.240:11811 - volumes: - - ./fastdds.xml:/fastdds.xml - devices: - # - /dev/hokuyo - - /dev/ttyACM0 - command: > - ros2 run urg_node urg_node_driver --ros-args -r - __ns:=/pioneer${PIONEER_ID} -p - laser_frame_id:=pioneer${PIONEER_ID}/laser -p - serial_port:=/dev/ttyACM0 \ No newline at end of file diff --git a/compose.ros2aria.yaml b/compose.ros2aria.yaml index 65ab1e3..fc0d35b 100644 --- a/compose.ros2aria.yaml +++ b/compose.ros2aria.yaml @@ -4,12 +4,15 @@ services: image: delicjusz/pioneer:humble-18012024 network_mode: host ipc: host + restart: unless-stopped environment: - - RMW_IMPLEMENTATION=rmw_fastrtps_cpp - LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib + - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml + - CYCLONEDDS_URI=file:///cyclonedds.xml volumes: - ./fastdds.xml:/fastdds.xml + - ./cyclonedds.xml:/cyclonedds.xml devices: - /dev/ttyS0 command: ros2 launch pioneer_bringup robot.launch.py namespace:=pioneer${PIONEER_ID}/ diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml new file mode 100644 index 0000000..e85284c --- /dev/null +++ b/config/nav2_params.yaml @@ -0,0 +1,456 @@ +--- +### NAVIGATION ### + +bt_navigator: + ros__parameters: + use_sim_time: false + global_frame: /map + robot_base_frame: /base_link + odom_topic: //odom + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: false + +controller_server: + ros__parameters: + use_sim_time: false + + controller_frequency: 10.0 + odom_topic: //odom + + # if velocity is below threshold value it is set to 0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + + progress_checker_plugin: progress_checker + progress_checker: + plugin: nav2_controller::SimpleProgressChecker + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + + goal_checker_plugin: goal_checker + goal_checker: + plugin: nav2_controller::SimpleGoalChecker + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.25 + stateful: true # if stateful is true goal checker will not check if the xy position matches again once it is found to be true. + + controller_plugins: [FollowPath] + FollowPath: + # Shim controller first rotates robot in place to orientation of global path + # and the switches to DWB - otherwise DWB won't turn in place, but with arc, which + # is less optimal + plugin: nav2_rotation_shim_controller::RotationShimController + primary_controller: dwb_core::DWBLocalPlanner + angular_dist_threshold: 0.4 # quite low value to avoid moving in arc + forward_sampling_distance: 0.5 + rotate_to_heading_angular_vel: 1.5 + # I'm not sure why, but ang vel is limited max_angular_accel param divided by 10 + # actual acceleration will be limited by controller, so it can be set to higher value + max_angular_accel: 10.0 + simulate_ahead_time: 1.0 + + debug_trajectory_details: true + + # Velocity/acceleration limits also have to be adjusted in the velocity smoother + min_vel_x: -0.25 + max_vel_x: 0.5 + acc_lim_x: 1.0 + decel_lim_x: -1.0 + min_speed_xy: 0.007 + max_speed_xy: 0.5 + + max_vel_theta: 1.5 + min_speed_theta: 0.4 + acc_lim_theta: 4.0 + decel_lim_theta: -4.0 + + min_vel_y: 0.0 + max_vel_y: 0.0 + acc_lim_y: 0.0 + decel_lim_y: 0.0 + + vx_samples: 20 + vy_samples: 0 + vtheta_samples: 20 + + transform_tolerance: 0.2 + short_circuit_trajectory_evaluation: true + + trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator + sim_time: 0.75 # low value to avoid slow movement near goal + discretize_by_time: false + linear_granularity: 0.05 + angular_granularity: 0.025 + + # Critics used for scoring trajectories created by trajectory generator + # Final score is a sum of critics' score + critics: [BaseObstacle, ObstacleFootprint, GoalAlign, GoalDist, PathAlign, PathDist, RotateToGoal, + Oscillation] + + BaseObstacle.scale: 0.02 + + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + GoalDist.scale: 24.0 + + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + + xy_goal_tolerance: 0.1 + trans_stopped_velocity: 0.25 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: false + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: false + + global_frame: /odom + robot_base_frame: /base_link + + update_frequency: 5.0 + publish_frequency: 2.0 + + width: 5 + height: 5 + resolution: 0.05 + + always_send_full_costmap: true + rolling_window: true + + transform_tolerance: 0.1 + footprint: '[[0.45, 0.47], [0.45, -0.47], [-0.45, -0.47], [-0.45, 0.47]]' + + plugins: [obstacle_layer, inflation_layer] + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + enabled: true + observation_sources: scan + scan: + topic: //scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: LaserScan + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + enabled: true + cost_scaling_factor: 1.3 + inflation_radius: 1.5 + + local_costmap_client: + ros__parameters: + use_sim_time: false + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: false + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: false + + global_frame: /map + robot_base_frame: /base_link + + update_frequency: 1.0 + publish_frequency: 1.0 + + resolution: 0.05 + + always_send_full_costmap: true + track_unknown_space: true # if false, treats unknown space as free space, else as unknown space + + transform_tolerance: 0.1 + footprint: '[[0.45, 0.47], [0.45, -0.47], [-0.45, -0.47], [-0.45, 0.47]]' + + plugins: [static_layer, obstacle_layer, inflation_layer] + static_layer: + plugin: nav2_costmap_2d::StaticLayer + enabled: true + map_subscribe_transient_local: true + map_topic: //map + obstacle_layer: + plugin: nav2_costmap_2d::ObstacleLayer + enabled: true + observation_sources: scan + scan: + topic: //scan + max_obstacle_height: 2.0 + clearing: true + marking: true + data_type: LaserScan + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + inflation_layer: + plugin: nav2_costmap_2d::InflationLayer + enabled: true + cost_scaling_factor: 1.5 + inflation_radius: 1.5 + + global_costmap_client: + ros__parameters: + use_sim_time: false + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: false + +planner_server: + ros__parameters: + use_sim_time: false + + planner_plugins: [GridBased] + GridBased: + plugin: nav2_smac_planner/SmacPlanner2D + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 2.0 # max time in s for planner to plan, smooth + motion_model_for_search: MOORE # 2D Moore, Von Neumann + cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: false + +recoveries_server: + ros__parameters: + use_sim_time: false + + global_frame: /odom + robot_base_frame: /base_link + transform_tolerance: 0.1 + + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + + recovery_plugins: [spin, backup, wait] + spin: + plugin: nav2_recoveries/Spin + backup: + plugin: nav2_recoveries/BackUp + wait: + plugin: nav2_recoveries/Wait + + # spin & backup + simulate_ahead_time: 2.0 + + # spin + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +behavior_server: + ros__parameters: + use_sim_time: false + +waypoint_follower: + ros__parameters: + use_sim_time: false + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: wait_at_waypoint + wait_at_waypoint: + plugin: nav2_waypoint_follower::WaitAtWaypoint + enabled: true + waypoint_pause_duration: 200 + +### LOCALIZATION ### + +amcl: + ros__parameters: + use_sim_time: false + + global_frame_id: /map + odom_frame_id: /odom + base_frame_id: /base_link + + scan_topic: //scan + + robot_model_type: nav2_amcl::DifferentialMotionModel + + set_initial_pose: true + always_reset_initial_pose: true + initial_pose: + x: 0.0 + y: 0.0 + yaw: 0.0 + + tf_broadcast: true + transform_tolerance: 1.0 + + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + + # Beam skipping - ignores beams for which a majoirty of particles do not agree with the map + # prevents correct particles from getting down weighted because of unexpected obstacles + # such as humans + do_beamskip: false + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + + lambda_short: 0.1 + + laser_model_type: likelihood_field + laser_likelihood_max_dist: 2.0 + laser_max_range: 12.0 + laser_min_range: -1.0 + + max_beams: 60 + + max_particles: 2000 + min_particles: 500 + + pf_err: 0.05 + pf_z: 0.99 + + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + + resample_interval: 1 + save_pose_rate: 0.5 + sigma_hit: 0.2 + + update_min_a: 0.1 + update_min_d: 0.15 + + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + +amcl_map_client: + ros__parameters: + use_sim_time: false + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: false + +### MAP SERVER ### + +map_server: + ros__parameters: + use_sim_time: false + yaml_filename: map.yaml + frame_id: /map + +### SLAM ### + +/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 #HuberLoss + + # ROS Parameters + odom_frame: /odom + map_frame: /map + base_frame: /base_link + scan_topic: //scan + mode: mapping #localization + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.04 + map_update_interval: 1.0 + resolution: 0.1 + max_laser_range: 12.0 #for rastering images + minimum_time_interval: 0.05 + transform_timeout: 0.5 + tf_buffer_duration: 20.0 + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: false + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.2 + minimum_travel_heading: 0.1 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 0.5 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 0.75 + 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 + + use_sim_time: false diff --git a/cyclonedds.xml b/cyclonedds.xml new file mode 100644 index 0000000..a56edde --- /dev/null +++ b/cyclonedds.xml @@ -0,0 +1,51 @@ + + + + + + + + false + false + 65500B + + + + + 500kB + + + + auto + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/cyclonedds_pc.xml b/cyclonedds_pc.xml new file mode 100644 index 0000000..3958a9e --- /dev/null +++ b/cyclonedds_pc.xml @@ -0,0 +1,48 @@ + + + + + eno1 + false + false + 65500B + + + + 500kB + + + + auto + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/docker-compose@.service b/docker-compose@.service index 853d9f6..5920408 100644 --- a/docker-compose@.service +++ b/docker-compose@.service @@ -7,8 +7,11 @@ After=docker.service Type=oneshot RemainAfterExit=true WorkingDirectory=/etc/docker/compose/%i +ExecStartPre= /bin/sh -c 'until ping -c1 denali; do sleep 1; done;' ExecStart=/usr/bin/docker compose up -d --remove-orphans ExecStop=/usr/bin/docker compose down +Restart=on-failure +RestartSec=5s [Install] WantedBy=multi-user.target \ No newline at end of file diff --git a/husarion_utils/bringup_launch.py b/husarion_utils/bringup_launch.py new file mode 100644 index 0000000..2d833d5 --- /dev/null +++ b/husarion_utils/bringup_launch.py @@ -0,0 +1,184 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, + OpaqueFunction, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import RewrittenYaml, ReplaceString + + +def launch_setup(context, *args, **kwargs): + # Get the launch directory + launch_dir = "/husarion_utils" + + # Create the launch configuration variables + namespace = LaunchConfiguration("namespace").perform(context) + observation_topic = LaunchConfiguration("observation_topic").perform(context) + map_yaml_file = LaunchConfiguration("map").perform(context) + use_sim_time = LaunchConfiguration("use_sim_time").perform(context) + params_file = LaunchConfiguration("params_file").perform(context) + slam = LaunchConfiguration("slam").perform(context) + autostart = LaunchConfiguration("autostart").perform(context) + use_composition = LaunchConfiguration("use_composition").perform(context) + use_respawn = LaunchConfiguration("use_respawn").perform(context) + log_level = LaunchConfiguration("log_level").perform(context) + + # Create our own temporary YAML files that include substitutions + param_substitutions = {"use_sim_time": use_sim_time, "yaml_filename": map_yaml_file} + + if namespace: + params_file = ReplaceString( + source_file=params_file, replacements={"": namespace} + ) + else: + params_file = ReplaceString( + source_file=params_file, replacements={"/": ""} + ) + + params_file = ReplaceString( + source_file=params_file, replacements={"": observation_topic} + ) + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + + # Specify the actions + bringup_cmd_group = GroupAction( + [ + PushRosNamespace(namespace), + Node( + condition=IfCondition(use_composition), + name="nav2_container", + package="rclcpp_components", + executable="component_container_isolated", + parameters=[configured_params, {"autostart": autostart}], + arguments=["--ros-args", "--log-level", log_level], + output="screen", + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "slam_launch.py")), + condition=IfCondition(slam), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "use_respawn": use_respawn, + "params_file": params_file, + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "localization_launch.py")), + condition=IfCondition(PythonExpression(["not ", slam])), + launch_arguments={ + "namespace": namespace, + "map": map_yaml_file, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "use_composition": use_composition, + "use_respawn": use_respawn, + "container_name": "nav2_container", + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, "navigation_launch.py")), + launch_arguments={ + "namespace": namespace, + "use_sim_time": use_sim_time, + "autostart": autostart, + "params_file": params_file, + "use_composition": use_composition, + "use_respawn": use_respawn, + "container_name": "nav2_container", + }.items(), + ), + ] + ) + + healthcheck_node = Node( + package="healthcheck_pkg", + executable="healthcheck_node", + name="healthcheck_navigation", + namespace=namespace, + output="screen", + ) + + return [bringup_cmd_group, healthcheck_node] + + +def generate_launch_description(): + return LaunchDescription( + [ + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Top-level namespace", + ), + DeclareLaunchArgument( + "observation_topic", + default_value="", + description="Topic name for PointCloud2 messages.", + ), + DeclareLaunchArgument("slam", default_value="False", description="Whether run a SLAM"), + DeclareLaunchArgument("map", description="Full path to map yaml file to load"), + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation (Gazebo) clock if true", + ), + DeclareLaunchArgument( + "params_file", + default_value=os.path.join("/params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ), + DeclareLaunchArgument( + "autostart", + default_value="true", + description="Automatically startup the nav2 stack", + ), + DeclareLaunchArgument( + "use_composition", + default_value="True", + description="Whether to use composed bringup", + ), + DeclareLaunchArgument( + "use_respawn", + default_value="False", + description="Whether to respawn if a node crashes. Applied when composition is disabled.", + ), + DeclareLaunchArgument("log_level", default_value="info", description="log level"), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/husarion_utils/localization_launch.py b/husarion_utils/localization_launch.py new file mode 100644 index 0000000..2546550 --- /dev/null +++ b/husarion_utils/localization_launch.py @@ -0,0 +1,193 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + + namespace = LaunchConfiguration("namespace") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") + use_composition = LaunchConfiguration("use_composition") + container_name = LaunchConfiguration("container_name") + container_name_full = (namespace, "/", container_name) + use_respawn = LaunchConfiguration("use_respawn") + log_level = LaunchConfiguration("log_level") + + lifecycle_nodes = ["map_server", "amcl"] + + # Create our own temporary YAML files that include substitutions + param_substitutions = {"use_sim_time": use_sim_time, "yaml_filename": map_yaml_file} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + + stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1") + + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", description="Full path to map yaml file to load" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation (Gazebo) clock if true" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + "use_composition", default_value="False", description="Use composed bringup if True" + ) + + declare_container_name_cmd = DeclareLaunchArgument( + "container_name", + default_value="nav2_container", + description="the name of container that nodes will load in if use composition", + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + "use_respawn", + default_value="False", + description="Whether to respawn if a node crashes. Applied when composition is disabled.", + ) + + declare_log_level_cmd = DeclareLaunchArgument( + "log_level", default_value="info", description="log level" + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(["not ", use_composition])), + actions=[ + Node( + package="nav2_map_server", + executable="map_server", + name="map_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + ), + Node( + package="nav2_amcl", + executable="amcl", + name="amcl", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_localization", + output="screen", + arguments=["--ros-args", "--log-level", log_level], + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + ), + ], + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package="nav2_map_server", + plugin="nav2_map_server::MapServer", + name="map_server", + parameters=[configured_params], + ), + ComposableNode( + package="nav2_amcl", + plugin="nav2_amcl::AmclNode", + name="amcl", + parameters=[configured_params], + ), + ComposableNode( + package="nav2_lifecycle_manager", + plugin="nav2_lifecycle_manager::LifecycleManager", + name="lifecycle_manager_localization", + parameters=[ + { + "use_sim_time": use_sim_time, + "autostart": autostart, + "node_names": lifecycle_nodes, + } + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/husarion_utils/navigation_launch.py b/husarion_utils/navigation_launch.py new file mode 100644 index 0000000..8bf31f2 --- /dev/null +++ b/husarion_utils/navigation_launch.py @@ -0,0 +1,279 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + + namespace = LaunchConfiguration("namespace") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") + use_composition = LaunchConfiguration("use_composition") + container_name = LaunchConfiguration("container_name") + container_name_full = (namespace, "/", container_name) + use_respawn = LaunchConfiguration("use_respawn") + log_level = LaunchConfiguration("log_level") + + lifecycle_nodes = [ + "controller_server", + "smoother_server", + "planner_server", + "behavior_server", + "bt_navigator", + "waypoint_follower", + "velocity_smoother", + ] + + # Create our own temporary YAML files that include substitutions + param_substitutions = {"use_sim_time": use_sim_time, "autostart": autostart} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + + stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1") + + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Top-level namespace", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation (Gazebo) clock if true" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + "use_composition", default_value="False", description="Use composed bringup if True" + ) + + declare_container_name_cmd = DeclareLaunchArgument( + "container_name", + default_value="nav2_container", + description="the name of container that nodes will load in if use composition", + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + "use_respawn", + default_value="False", + description="Whether to respawn if a node crashes. Applied when composition is disabled.", + ) + + declare_log_level_cmd = DeclareLaunchArgument( + "log_level", default_value="info", description="log level" + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(["not ", use_composition])), + actions=[ + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=[("cmd_vel", "cmd_vel_nav")], + ), + Node( + package="nav2_smoother", + executable="smoother_server", + name="smoother_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + ), + Node( + package="nav2_velocity_smoother", + executable="velocity_smoother", + name="velocity_smoother", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=[("cmd_vel", "cmd_vel_nav"), ("cmd_vel_smoothed", "cmd_vel")], + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + arguments=["--ros-args", "--log-level", log_level], + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + ), + ], + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package="nav2_controller", + plugin="nav2_controller::ControllerServer", + name="controller_server", + parameters=[configured_params], + remappings=[("cmd_vel", "cmd_vel_nav")], + ), + ComposableNode( + package="nav2_smoother", + plugin="nav2_smoother::SmootherServer", + name="smoother_server", + parameters=[configured_params], + ), + ComposableNode( + package="nav2_planner", + plugin="nav2_planner::PlannerServer", + name="planner_server", + parameters=[configured_params], + ), + ComposableNode( + package="nav2_behaviors", + plugin="behavior_server::BehaviorServer", + name="behavior_server", + parameters=[configured_params], + ), + ComposableNode( + package="nav2_bt_navigator", + plugin="nav2_bt_navigator::BtNavigator", + name="bt_navigator", + parameters=[configured_params], + ), + ComposableNode( + package="nav2_waypoint_follower", + plugin="nav2_waypoint_follower::WaypointFollower", + name="waypoint_follower", + parameters=[configured_params], + ), + ComposableNode( + package="nav2_velocity_smoother", + plugin="nav2_velocity_smoother::VelocitySmoother", + name="velocity_smoother", + parameters=[configured_params], + remappings=[("cmd_vel", "cmd_vel_nav"), ("cmd_vel_smoothed", "cmd_vel")], + ), + ComposableNode( + package="nav2_lifecycle_manager", + plugin="nav2_lifecycle_manager::LifecycleManager", + name="lifecycle_manager_navigation", + parameters=[ + { + "use_sim_time": use_sim_time, + "autostart": autostart, + "node_names": lifecycle_nodes, + } + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/husarion_utils/slam_launch.py b/husarion_utils/slam_launch.py new file mode 100644 index 0000000..f2e38c3 --- /dev/null +++ b/husarion_utils/slam_launch.py @@ -0,0 +1,145 @@ +# Copyright (c) 2020 Samsung Research Russia +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Input parameters declaration + namespace = LaunchConfiguration("namespace") + params_file = LaunchConfiguration("params_file") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + use_respawn = LaunchConfiguration("use_respawn") + log_level = LaunchConfiguration("log_level") + + # Variables + lifecycle_nodes = ["map_saver"] + + # Getting directories and launch-files + slam_toolbox_dir = get_package_share_directory("slam_toolbox") + slam_launch_file = os.path.join(slam_toolbox_dir, "launch", "online_sync_launch.py") + + # Create our own temporary YAML files that include substitutions + param_substitutions = {"use_sim_time": use_sim_time} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="robot", description="Top-level namespace" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value="/params.yaml", + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="True", description="Use simulation (Gazebo) clock if true" + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="True", description="Automatically startup the nav2 stack" + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + "use_respawn", + default_value="False", + description="Whether to respawn if a node crashes. Applied when composition is disabled.", + ) + + declare_log_level_cmd = DeclareLaunchArgument( + "log_level", default_value="info", description="log level" + ) + + # Nodes launching commands + + start_map_saver_server_cmd = Node( + package="nav2_map_server", + executable="map_saver_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + arguments=["--ros-args", "--log-level", log_level], + parameters=[configured_params], + ) + + start_lifecycle_manager_cmd = Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_slam", + output="screen", + arguments=["--ros-args", "--log-level", log_level], + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + ) + + start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={"use_sim_time": use_sim_time, "slam_params_file": params_file}.items(), + ) + + group_actions = [] + + # Remapping + group_actions.append(SetRemap("/map", "map")) + group_actions.append(SetRemap("/map_metadata", "map_metadata")) + group_actions.append(SetRemap("/scan", "scan")) + + # Running Map Saver Server + group_actions.append(start_map_saver_server_cmd) + group_actions.append(start_lifecycle_manager_cmd) + + # Running SLAM Toolbox (Only one of them will be run) + group_actions.append(start_slam_toolbox_cmd_with_params) + + slam_ns = GroupAction(actions=group_actions) + + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Running SLAM + ld.add_action(slam_ns) + + return ld diff --git a/install.sh b/install.sh new file mode 100644 index 0000000..451324b --- /dev/null +++ b/install.sh @@ -0,0 +1,45 @@ +#!/bin/bash + +# https://autowarefoundation.github.io/autoware-documentation/main/installation/additional-settings-for-developers/network-configuration/dds-settings/ +# Check if root +if [ "$EUID" -ne 0 ] + then echo "Please run as root" + exit +fi + +cp docker-compose@.service /etc/systemd/system/ + +mkdir -p /etc/docker/compose/ros2aria/ +mkdir -p /etc/docker/compose/hokuyo/ +mkdir -p /etc/docker/compose/decawave/ +mkdir -p /etc/docker/compose/navigation2/ + +cp compose.ros2aria.yaml /etc/docker/compose/ros2aria/compose.yaml +cp compose.hokuyo.yaml /etc/docker/compose/hokuyo/compose.yaml +cp compose.decawave.yaml /etc/docker/compose/decawave/compose.yaml + +cp compose.navigation2.yaml /etc/docker/compose/navigation2/compose.yaml + + + +cp .env fastdds.xml cyclonedds.xml /etc/docker/compose/ros2aria/ +cp .env fastdds.xml cyclonedds.xml /etc/docker/compose/hokuyo/ +cp .env fastdds.xml cyclonedds.xml /etc/docker/compose/decawave/ +cp .env fastdds.xml cyclonedds.xml /etc/docker/compose/navigation2/ + +cp -r husarion_utils config maps /etc/docker/compose/navigation2/ + +cp 10-cyclone-max.conf /etc/sysctl.d/10-cyclone-max.conf + +sysctl --system | grep 10-cyclone-max.conf +systemctl daemon-reload + +systemctl enable docker-compose@ros2aria.service +systemctl enable docker-compose@hokuyo.service +systemctl disable docker-compose@navigation2.service +systemctl enable docker-compose@decawave.service + +systemctl restart docker-compose@ros2aria.service +systemctl restart docker-compose@hokuyo.service +systemctl stop docker-compose@navigation2.service +systemctl restart docker-compose@decawave.service \ No newline at end of file diff --git a/pioneer_status.py b/pioneer_status.py new file mode 100644 index 0000000..5d9a113 --- /dev/null +++ b/pioneer_status.py @@ -0,0 +1,136 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState +from nav_msgs.msg import Odometry +from sensor_msgs.msg import LaserScan +import time +import sys +from PyQt5.QtWidgets import QApplication, QWidget, QLabel, QGridLayout, QVBoxLayout, QMainWindow +from PyQt5.QtCore import QTimer, Qt + +class PioneerStatusNode(Node): + def __init__(self, gui): + super().__init__('pioneer_status_node') + self.gui = gui # Reference to the GUI + self.topic_data = {} + + # Subscribing to topics for pioneers 1-6 + for i in range(1, 7): + for topic_type, msg_type in [("odom", Odometry), ("joint_states", JointState), ("scan", LaserScan)]: + topic_name = f'/pioneer{i}/{topic_type}' + self.topic_data[topic_name] = {"last_message_time": 0, "message_count": 0} + self.create_subscription(msg_type, topic_name, lambda msg, topic=topic_name: self.message_callback(msg, topic), 10) + + def message_callback(self, msg, topic_name): + # Update the last received time and message count for each topic + self.topic_data[topic_name]["last_message_time"] = time.time() + self.topic_data[topic_name]["message_count"] += 1 + + def update_gui(self): + current_time = time.time() + + for topic_name, data in self.topic_data.items(): + # Calculate frequency + time_diff = current_time - data["last_message_time"] + frequency = data["message_count"] / time_diff if time_diff > 0 else 0 + + # Determine status (active if received within last 2 seconds) + status = "Active" if time_diff < 2 else "Inactive" + + # Update the GUI labels for this topic + self.gui.update_status(topic_name, frequency, status) + + # Reset message count to calculate frequency anew in the next interval + data["message_count"] = 0 + +class PioneerStatusGUI(QMainWindow): + def __init__(self): + super().__init__() + + self.setWindowTitle("Pioneer Status Monitor") + self.setGeometry(100, 100, 600, 400) + + # Main layout and widget setup + central_widget = QWidget() + self.setCentralWidget(central_widget) + layout = QVBoxLayout() + central_widget.setLayout(layout) + + self.grid = QGridLayout() + layout.addLayout(self.grid) + + # Header labels + self.grid.addWidget(QLabel("Topic"), 0, 0, alignment=Qt.AlignCenter) + self.grid.addWidget(QLabel("Frequency (Hz)"), 0, 1, alignment=Qt.AlignCenter) + self.grid.addWidget(QLabel("Status"), 0, 2, alignment=Qt.AlignCenter) + + # Dictionary to store labels for each topic's frequency and status + self.topic_labels = {} + + # Populate rows with placeholders for each topic + row = 1 + for i in range(1, 7): + for topic_type in ["odom", "joint_states", "scan"]: + topic_name = f'/pioneer{i}/{topic_type}' + + # Topic name label + self.grid.addWidget(QLabel(topic_name), row, 0) + + # Frequency label + freq_label = QLabel("0.00") + freq_label.setAlignment(Qt.AlignCenter) + self.grid.addWidget(freq_label, row, 1) + + # Status label + status_label = QLabel("Inactive") + status_label.setAlignment(Qt.AlignCenter) + status_label.setStyleSheet("color: red;") + self.grid.addWidget(status_label, row, 2) + + # Store references to labels for updates + self.topic_labels[topic_name] = { + "frequency": freq_label, + "status": status_label + } + row += 1 + + def update_status(self, topic_name, frequency, status): + # Update frequency label + if topic_name in self.topic_labels: + self.topic_labels[topic_name]["frequency"].setText(f"{frequency:.2f}") + + # Update status label color and text based on status + status_label = self.topic_labels[topic_name]["status"] + status_label.setText(status) + status_label.setStyleSheet("color: green;" if status == "Active" else "color: red;") + +def main(args=None): + rclpy.init(args=args) + + # PyQt5 application setup + app = QApplication(sys.argv) + gui = PioneerStatusGUI() + gui.show() + + # Create the ROS 2 node + node = PioneerStatusNode(gui) + + # QTimer to call rclpy.spin_once periodically + timer = QTimer() + timer.timeout.connect(lambda: rclpy.spin_once(node, timeout_sec=0.1)) + timer.start(100) # Check for ROS messages every 100 ms + + # QTimer to update the GUI every second + update_timer = QTimer() + update_timer.timeout.connect(node.update_gui) + update_timer.start(500) # Update GUI every 1 second + + # Run the PyQt application + try: + sys.exit(app.exec_()) + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main()