L1.5/ros2aria
L1.5/ros2aria
7
0
Fork 0
Code Issues 5 Pull Requests Projects 1 Releases Wiki Activity

cyclonedds config

Browse Source 
Signed-off-by: Jakub Delicat <jakub.delicat@pwr.edu.pl>
This commit is contained in:
Jakub Delicat 2024-11-15 19:34:32 +01:00
parent e43458de68
commit af473e68f0
21 changed files with 1638 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.env
View File

@ -1 +1 @@
PIONEER_ID=5
PIONEER_ID=4

6
10-cyclone-max.conf Normal file
View File

@ -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

30
Dockerfile.pioneer Normal file
View File

@ -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/*

12
Makefile
View File

@ -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:

16
README.md
View File

@ -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

2
build_devcontainer.sh
View File

@ -1,3 +1,3 @@
#!/bin/sh
docker build -f Dockerfile.aria -t delicjusz/ros2aria .
docker build -f Dockerfile.aria -t delicjusz/ros2aria:jazzy .

5
compose.decawave.yaml
View File

@ -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}/

5
compose.hokuyo.yaml
View File

@ -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: >

26
compose.navigation2.yaml Normal file
View File

@ -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

35
compose.pioneer.yaml
View File

@ -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

5
compose.ros2aria.yaml
View File

@ -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}/

456
config/nav2_params.yaml Normal file
View File

@ -0,0 +1,456 @@
---
### NAVIGATION ###
bt_navigator:
ros__parameters:
use_sim_time: false
global_frame: <robot_namespace>/map
robot_base_frame: <robot_namespace>/base_link
odom_topic: /<robot_namespace>/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: /<robot_namespace>/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: <robot_namespace>/odom
robot_base_frame: <robot_namespace>/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: /<robot_namespace>/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: <robot_namespace>/map
robot_base_frame: <robot_namespace>/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: /<robot_namespace>/map
obstacle_layer:
plugin: nav2_costmap_2d::ObstacleLayer
enabled: true
observation_sources: scan
scan:
topic: /<robot_namespace>/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: <robot_namespace>/odom
robot_base_frame: <robot_namespace>/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: <robot_namespace>/map
odom_frame_id: <robot_namespace>/odom
base_frame_id: <robot_namespace>/base_link
scan_topic: /<robot_namespace>/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: <robot_namespace>/map
### SLAM ###
<robot_namespace>/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: <robot_namespace>/odom
map_frame: <robot_namespace>/map
base_frame: <robot_namespace>/base_link
scan_topic: /<robot_namespace>/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

51
cyclonedds.xml Normal file
View File

@ -0,0 +1,51 @@
<?xml version="1.0" encoding="UTF-8" ?>
<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="https://cdds.io/config https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
<Domain Id="any">
<General>
<Interfaces>
<NetworkInterface autodetermine="false" name="wlp1s0" priority="default" multicast="default" />
</Interfaces>
<AllowMulticast>false</AllowMulticast>
<EnableMulticastLoopback>false</EnableMulticastLoopback>
<MaxMessageSize>65500B</MaxMessageSize>
</General>
<Internal>
<SocketReceiveBufferSize min="10MB"/>
<Watermarks>
<WhcHigh>500kB</WhcHigh>
</Watermarks>
</Internal>
<Discovery>
<ParticipantIndex>auto</ParticipantIndex>
<Peers>
<Peer address="lab15-1"/>
<Peer address="lab15-2"/>
<Peer address="lab15-3"/>
<Peer address="lab15-4"/>
<Peer address="lab15-5"/>
<Peer address="lab15-6"/>
<Peer address="lab15-7"/>
<Peer address="lab15-8"/>
<Peer address="lab15-9"/>
<Peer address="lab15-10"/>
<Peer address="lab15-11"/>
<Peer address="lab15-12"/>
<Peer address="lab15-13"/>
<Peer address="lab15-14"/>
<Peer address="lab15-15"/>
<Peer address="lab15-16"/>
<Peer address="lab15-17"/>
<Peer address="lab15-18"/>
<Peer address="lab15-19"/>
<Peer address="lab15-20"/>
<Peer address="pionier1"/>
<Peer address="pionier2"/>
<Peer address="pionier3"/>
<Peer address="pionier4"/>
<Peer address="pionier5"/>
<Peer address="pionier6"/>
</Peers>
</Discovery>
</Domain>
</CycloneDDS>

48
cyclonedds_pc.xml Normal file
View File

@ -0,0 +1,48 @@
<?xml version="1.0" encoding="UTF-8" ?>
<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="https://cdds.io/config https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
<Domain Id="any">
<General>
<NetworkInterfaceAddress>eno1</NetworkInterfaceAddress>
<AllowMulticast>false</AllowMulticast>
<EnableMulticastLoopback>false</EnableMulticastLoopback>
<MaxMessageSize>65500B</MaxMessageSize>
</General>
<Internal>
<Watermarks>
<WhcHigh>500kB</WhcHigh>
</Watermarks>
</Internal>
<Discovery>
<ParticipantIndex>auto</ParticipantIndex>
<Peers>
<Peer address="lab15-1"/>
<Peer address="lab15-2"/>
<Peer address="lab15-3"/>
<Peer address="lab15-4"/>
<Peer address="lab15-5"/>
<Peer address="lab15-6"/>
<Peer address="lab15-7"/>
<Peer address="lab15-8"/>
<Peer address="lab15-9"/>
<Peer address="lab15-10"/>
<Peer address="lab15-11"/>
<Peer address="lab15-12"/>
<Peer address="lab15-13"/>
<Peer address="lab15-14"/>
<Peer address="lab15-15"/>
<Peer address="lab15-16"/>
<Peer address="lab15-17"/>
<Peer address="lab15-18"/>
<Peer address="lab15-19"/>
<Peer address="lab15-20"/>
<Peer address="pionier1"/>
<Peer address="pionier2"/>
<Peer address="pionier3"/>
<Peer address="pionier4"/>
<Peer address="pionier5"/>
<Peer address="pionier6"/>
</Peers>
</Discovery>
</Domain>
</CycloneDDS>

3
docker-compose@.service
View File

@ -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

184
husarion_utils/bringup_launch.py Normal file
View File

@ -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={"<robot_namespace>": namespace}
)
else:
params_file = ReplaceString(
source_file=params_file, replacements={"<robot_namespace>/": ""}
)
params_file = ReplaceString(
source_file=params_file, replacements={"<observation_topic>": 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),
]
)

193
husarion_utils/localization_launch.py Normal file
View File

@ -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

279
husarion_utils/navigation_launch.py Normal file
View File

@ -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

145
husarion_utils/slam_launch.py Normal file
View File

@ -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

45
install.sh Normal file
View File

@ -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

136
pioneer_status.py Normal file
View File

@ -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()