From d6c768b855a4d97e28c6c386b7b1800ef9ee5775 Mon Sep 17 00:00:00 2001 From: jfkeller Date: Tue, 28 Jan 2025 15:41:24 -0500 Subject: [PATCH] Jkeller/jetson 36.4 (#195) * updating docker for l4t 36.4.0, added gui option to robot launch file * added calculation of ROS_DOMAIN_ID and ROBOT_NAME to bashrc for real robot, added sim argument to robot launch file for setting use_sime_time and launching guis, added behavior tree visualization topic to domain bridge * created sim and real ground-control-station extending from same base. they just have differences in their networks. configured some topics for domain bridge * added mavros to robot launch for real drone. tested communication is working between desktop and drone. commands set from the gui are getting to mavros * fixed slow moving trajetory controller bug, cleaned up robot launch and tested that it works in sim and on the real drone --- .../docker/docker-compose.yaml | 58 +++++-------------- ...d-control-station-base-docker-compose.yaml | 46 +++++++++++++++ .../src/gcs_bringup/config/domain_bridge.yaml | 25 ++++++-- robot/docker/.bashrc | 7 +++ robot/docker/Dockerfile.robot | 2 +- robot/docker/docker-compose.yaml | 3 +- .../src/trajectory_library.cpp | 4 +- .../local_bringup/launch/local.launch.xml | 4 +- .../launch/behavior.launch.xml | 6 -- .../src/robot_bringup/launch/robot.launch.xml | 39 ++++++++++--- .../robot_bringup/params/domain_bridge.yaml | 5 ++ 11 files changed, 130 insertions(+), 69 deletions(-) create mode 100644 ground_control_station/docker/ground-control-station-base-docker-compose.yaml diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml index 2345ffbd..9049fae8 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -1,52 +1,24 @@ services: ground-control-station: - image: &gcs_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_gcs - build: - context: ../ - dockerfile: docker/Dockerfile.gcs - tags: - - *gcs_image - container_name: ground-control-station - entrypoint: "" - command: > - bash -c "ssh service restart; - tmux new -d -s gcs_bringup - && tmux send-keys -t gcs_bringup - 'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi; - ros2 launch gcs_bringup gcs.launch.xml' ENTER - && sleep infinity" - # Interactive shell - stdin_open: true # docker run -i - tty: true # docker run -t - # Needed to display graphical applications - # ipc: host - privileged: true + profiles: + - "" + - sitl + extends: + file: ./ground-control-station-base-docker-compose.yaml + service: ground-control-station-base networks: - airstack_network - environment: - - DISPLAY - - QT_X11_NO_MITSHM=1 - deploy: - # let it use the GPU - resources: - reservations: - devices: - - driver: nvidia # https://stackoverflow.com/a/70761193 - count: 1 - capabilities: [gpu] ports: - 2222:22 # for ssh - volumes: - # display stuff - - $HOME/.Xauthority:/root/.Xauthority - - /tmp/.X11-unix:/tmp/.X11-unix - # developer stuff - - .bashrc:/root/.bashrc:rw # bash config - - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name - # autonomy stack stuff - - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages - - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages - - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml + + ground-control-station-real: + profiles: + - hitl + - deploy + extends: + file: ./ground-control-station-base-docker-compose.yaml + service: ground-control-station-base + network_mode: host # include: # - ./tak-docker-compose.yaml \ No newline at end of file diff --git a/ground_control_station/docker/ground-control-station-base-docker-compose.yaml b/ground_control_station/docker/ground-control-station-base-docker-compose.yaml new file mode 100644 index 00000000..8e94b105 --- /dev/null +++ b/ground_control_station/docker/ground-control-station-base-docker-compose.yaml @@ -0,0 +1,46 @@ +# docker compose file +services: + ground-control-station-base: + image: &gcs_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_gcs + build: + context: ../ + dockerfile: docker/Dockerfile.gcs + tags: + - *gcs_image + container_name: ground-control-station + entrypoint: "" + command: > + bash -c "ssh service restart; + tmux new -d -s gcs_bringup + && tmux send-keys -t gcs_bringup + 'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi; + ros2 launch gcs_bringup gcs.launch.xml' ENTER + && sleep infinity" + # Interactive shell + stdin_open: true # docker run -i + tty: true # docker run -t + # Needed to display graphical applications + # ipc: host + privileged: true + environment: + - DISPLAY + - QT_X11_NO_MITSHM=1 + deploy: + # let it use the GPU + resources: + reservations: + devices: + - driver: nvidia # https://stackoverflow.com/a/70761193 + count: 1 + capabilities: [gpu] + volumes: + # display stuff + - $HOME/.Xauthority:/root/.Xauthority + - /tmp/.X11-unix:/tmp/.X11-unix + # developer stuff + - .bashrc:/root/.bashrc:rw # bash config + - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name + # autonomy stack stuff + - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages + - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages + - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml b/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml index df5b7175..d4887648 100644 --- a/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml +++ b/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml @@ -1,11 +1,26 @@ name: my_bridge -from_domain: 0 -to_domain: 1 topics: /robot_1/behavior/behavior_tree_commands: type: behavior_tree_msgs/msg/BehaviorTreeCommands + from_domain: 0 + to_domain: 1 + /robot_2/behavior/behavior_tree_commands: + type: behavior_tree_msgs/msg/BehaviorTreeCommands + from_domain: 0 + to_domain: 2 + /robot_3/behavior/behavior_tree_commands: + type: behavior_tree_msgs/msg/BehaviorTreeCommands + from_domain: 0 + to_domain: 3 /robot_1/fixed_trajectory_generator/fixed_trajectory_command: type: airstack_msgs/msg/FixedTrajectory - /robot_1/behavior/behavior_tree_graphviz: - type: std_msgs/msg/String - reversed: True \ No newline at end of file + from_domain: 0 + to_domain: 1 + /robot_2/fixed_trajectory_generator/fixed_trajectory_command: + type: airstack_msgs/msg/FixedTrajectory + from_domain: 0 + to_domain: 2 + /robot_3/fixed_trajectory_generator/fixed_trajectory_command: + type: airstack_msgs/msg/FixedTrajectory + from_domain: 0 + to_domain: 3 \ No newline at end of file diff --git a/robot/docker/.bashrc b/robot/docker/.bashrc index 7ece30a3..b804cabb 100644 --- a/robot/docker/.bashrc +++ b/robot/docker/.bashrc @@ -176,5 +176,12 @@ CONTAINER_PREFIX="airstack-" export ROBOT_NAME=$(echo "$container_name" | sed "s#/$CONTAINER_PREFIX##" | sed 's#-#_#') export ROS_DOMAIN_ID=$(echo "$ROBOT_NAME" | awk -F'_' '{print $NF}') +if [ "$ROBOT_NAME" == "null" ]; then + num=$(hostname | awk -F'-' '{print $2}') # get number from hostname + num=$((num)) #remove leading zeros + export ROBOT_NAME="robot_$num" + export ROS_DOMAIN_ID=$num +fi + export RCUTILS_COLORIZED_OUTPUT=1 # get colored output from ROS2 tools diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot index a31c32f8..46c4557a 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -110,7 +110,7 @@ RUN apt remove -y libopenvdb*; \ cd ..; rm -rf /opt/openvdb/build # Add ability to SSH -RUN apt-get update && apt-get install -y openssh-server +RUN apt-get update && apt-get install -y openssh-server libimath-dev RUN mkdir /var/run/sshd # Password is airstack diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml index 867dcd65..88837631 100644 --- a/robot/docker/docker-compose.yaml +++ b/robot/docker/docker-compose.yaml @@ -38,8 +38,9 @@ services: build: dockerfile: ./Dockerfile.robot args: - BASE_IMAGE: nvcr.io/nvidia/l4t-pytorch:r35.2.1-pth2.0-py3 + BASE_IMAGE: nvcr.io/nvidia/l4t-jetpack:r36.4.0 tags: - *l4t_image + runtime: nvidia # assumes network isolation via a physical router, so uses network_mode=host network_mode: host \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp index e41603be..5183a45c 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp @@ -483,12 +483,12 @@ bool Trajectory::get_waypoint_sphere_intersection(double initial_time, double ah wp_end = wp_start.interpolate( wp_end, (ahead_distance - current_path_distance) / segment_distance); if (end_waypoint != NULL) *end_waypoint = wp_end; - } else if (wp_end.get_time() > time_end) { + }/* else if (wp_end.get_time() > time_end) { should_break = true; wp_end = wp_start.interpolate(wp_end, (time_end - wp_start.get_time()) / (wp_end.get_time() - wp_start.get_time())); if (end_waypoint != NULL) *end_waypoint = wp_end; - } else + }*/ else current_path_distance += segment_distance; // sphere line intersection equations: diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml index 2f698960..ed55b26d 100644 --- a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml +++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml @@ -60,7 +60,7 @@ - + @@ -83,4 +83,4 @@ output="screen" respawn="true" respawn_delay="1" /> ?> - \ No newline at end of file + diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml index 4fac12e5..711ebb6a 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml @@ -1,12 +1,6 @@ - - - - diff --git a/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml b/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml index dc647d1c..eff9dc54 100644 --- a/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml +++ b/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml @@ -1,9 +1,35 @@ + + + - - + + + + + + + + + + + + + + + + + + + + + @@ -14,14 +40,9 @@ - - - - + - \ No newline at end of file + diff --git a/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml b/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml index e1507cf8..0d588444 100644 --- a/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml +++ b/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml @@ -44,6 +44,11 @@ topics: from_domain: 2 to_domain: 1 + /robot_3/behavior/behavior_tree_graphviz: + type: std_msgs/msg/String + from_domain: 3 + to_domain: 0 + # Bridge "/clock" topic from doman ID 2 to domain ID 3, # Override durability to be 'volatile' and override depth to be 1 clock: