MIGHTY: Quintic Hermite Spline-based Efficient Trajectory Planning

Submitted to the IEEE Robotics and Automation Letters (RA-L)

General Setup

MIGHTY has been tested on both Docker and native installations on Ubuntu 22.04 with ROS 2 Humble.

Use Docker (Recommended)

1. Install Docker:
   Follow the official Docker installation guide for Ubuntu.

2. Clone the Repository and Navigate to the Docker Folder:

   mkdir -p ~/code/ws/src
   cd ~/code/ws/src
   git clone https://github.com/kotakondo/dynus.git
   cd src/dynus/docker
   git checkout lbfgs

3. Generate a Personal Access Token (PAT):

   • Follow this guide to generate your PAT.
   • When generating your PAT, ensure:
     • Repository access: Set to All repositories
     • In the Repository permissions menu, find the Contents row and select Access > Read and Write

4. BUILD:

   • Navigate to the docker folder in your dynus repo (eg. cd ~/code/dynus_ws/src/dynus/docker/) and run this

   • make build GIT_ACCESS_TOKEN=(copy and paste your_token_here)

5. Run Simulation

   • Run the following command to start the simulation:

     make run-sim

6. (Optional) If you want to run the sim in dynamic environment:

   • comment out python3 src/dynus/launch/run_single_sim.py --env easy_forest in dynus.sh
   • uncomment python3 src/dynus/launch/run_single_sim.py --env dynamic_forest in dynus.sh
   • run

     make run-sim

Useful Docker Commands

• Remove all caches:

  docker builder prune

• Remove all containers:

  docker rm $(docker ps -a -q)

• Remove all images:

  docker rmi $(docker images -q)

5. Run the Docker Container:

   make run-sim

   This command will start the simulation in the Docker container. If you encounter any issues, please refer to the Error Handling section.

Native Installation

1. Clone the Repository and Navigate to the Workspace Folder:

   mkdir -p ~/code/ws
   cd ~/code/ws
   git clone https://github.com/kotakondo/dynus.git
   cd dynus

2. Run the Setup Script:

   ./setup.sh

   This script will first install ROS 2 Humble, then DYNUS and its dependencies. Please note that this script modifies your ~/.bashrc file.

Error Handling

• Error 1: When running colcon build

  By not providing "Finddecomp_util.cmake" in CMAKE_MODULE_PATH, this project
  has asked CMake to find a package configuration file provided by
  "decomp_util", but CMake did not find one.
  

  • Solution:
    Source install/setup.bash and build again.

• Error 2: When running Python script

  No module named 'rclpy._rclpy_pybind11'
  

  • Solution:
    Deactivate the Conda environment if you are using Conda.

• Runtime Error 3:

  "Spawn status: Entity pushed to spawn queue, but spawn service timed out
  waiting for entity to appear in simulation under the name [quadrotor]"
  

  • Solution:
    Go to the world file you are using and make sure sim_time is set to 0.

• Error 4: When building dlio package

  fatal error: numpy/ndarrayobject.h: No such file or directory
  

  • Solution:
    Just cleaned up the workspace and rebuilt it.

• Error 5: Gazebo runtime error

  [gzserver-1] gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:728: 
  typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const 
  [with T = gazebo::rendering::Scene; typename boost::detail::sp_member_access<T>::type = gazebo::rendering::Scene*]: 
  Assertion `px != 0' failed.
  

  • Solution:
    Source Gazebo's setup file:

    source /usr/share/gazebo/setup.bash
    

• Error 6: Missing package configuration file

  Could not find a package configuration file provided by "diagnostic_updater" 
  with any of the following names: when building realsense-ros.
  

  • Solution:
    Install the missing package:

    sudo apt-get install ros-humble-diagnostic-updater
    

• Error 7: When running colcon build

  /lib/libgurobi_c++.a, needed by test_yaw_solver
  

  • Solution:
    Set the Gurobi path: (even if you put this in bashrc - just to make sure it's set)

    export GUROBI_HOME=/opt/gurobi1103/linux64
    colcon build --cmake-args "-DCMAKE_BUILD_TYPE=Release"
    

Run the Simulation

Notes

ROS2 Multiagent Hardware Setup

Debugging Useful Commands:

• ros2 multicast send
• ros2 multicast receive
• ros2 run demo_nodes_cpp talker
• ros2 run demo_nodes_cpp listener

Commands to Run:

• sudo ufw disable
• sudo ufw allow in proto udp to 224.0.0.0/4
  (if you disable ufw, you don't need this)
• sudo ufw allow in proto udp from 224.0.0.0/4
  (if you disable ufw, you don't need this)
• sudo ufw allow in proto udp from 192.168.1.0/24
  (if you disable ufw, you don't need this)

Things to Check:

• export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
• export ROS_LOCALHOST_ONLY=0
• Ensure ROS_DOMAIN_ID is set to the same value for all agents.
• (For ground station) You may need to unplug other network interfaces to encourage the computer to use the correct network.

How to Monitor Compute

• Record Performance Data:

  perf record -g -- tmuxp load src/dynus/launch/default_sim.yaml

• Generate Performance Script:

  perf script >> script.txt

• Generate Flame Graph:

  cat /home/kkondo/code/dynus_ws/script.txt | ./stackcollapse-perf.pl | ./flamegraph.pl > flame.html

• View Flame Graph:
  Open flame.html in your browser (e.g., type the path in your browser).

Gazebo

• Hospital Models & World Files:
  • OpenRobotics Hospital Models
  • Useful World Files for Gazebo and ROS 2 Simulations

For Octomap

• Installation Commands:

  sudo apt-get install ros-humble-octomap
  sudo apt-get install ros-humble-octomap-msgs
  sudo apt-get install ros-humble-octomap-ros
  sudo apt-get install ros-humble-octomap-rviz-plugins
  sudo apt install ros-humble-rviz-common

If You Use Conda for Dynus

• Use Python 3.10 or below (to avoid pybind errors).
• Install GCC 12.1.0 to prevent gcc errors:

  conda install -c conda-forge gcc=12.1.0

• Install required Python packages:

  pip install numpy lxml pyyaml empy==3.3.4 catkin_pkg lark

How to Build Docker Using Private Repo with PAT

• Generate a Personal Access Token (PAT):
  • Follow this guide to generate your PAT.
  • When generating your PAT, ensure:
    • Repository access: Set to All repositories
    • In the Repository permissions menu, find the Contents row and select Access > Read and Write

Docker-related Issues

- Rviz GUI not showing up? Run this in the terminal: ```bash xhost + ```

Object Detection Module

• Install Dependencies:

  pip install opencv-python
  pip install ultralytics

• Download YOLO Model:
  • Use yolo11n.pt (for example):

    wget https://github.com/ultralytics/assets/releases/download/v8.3.0/yolo11n.pt

• Install Specific Numpy Version:

  pip install numpy==1.26.4

New Conda Environment for YOLO

• Create and Configure Environment:

  conda install -n yolo -c conda-forge libstdcxx-ng

When you get Found ROS process to kill: defunct

- **Solution:** - Run the following command to kill the defunct process: ```bash ps -ef | grep defunct ``` - And kill the process like this: ```bash kill -9 ```

Goal Commands

• Office Space:

  ros2 topic pub /NX01/term_goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 36.0, y: 42.0, z: 2.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}" --once

• Forest3:

  ros2 topic pub /NX01/term_goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 50.0, y: 0.0, z: 3.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}" --once

• Path Push Visualization:

  ros2 topic pub /NX01/term_goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 8.0, y: 0.0, z: 3.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}" --once

• Ground Robot:

  ros2 topic pub /NX01/term_goal geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'map'}, pose: {position: {x: 50.0, y: 0.0, z: 0.5}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}" --once

Bag Record Commands

• Path Push Visualization Bag:

  ros2 bag record /NX01/dgp_path_marker /tf /tf_static /rosout /NX01/point_G /NX01/cluster_bounding_boxes /NX01/tracked_obstacles /NX01/fov /NX01/uncertainty_spheres -o test7

• Ground Robot Visualization Bag:

  ros2 bag record /NX01/d435/color/image_raw /NX01/fov /NX01/point_G /NX01/actual_traj /NX01/mid360_PointCloud2 /tf /tf_static -o test0

(ACL Specific) Running a Large Scale Simulation on Lambda Machines

Setup:

• Connect a monitor to the machine.
• Set the display:

  export DISPLAY=:1

• Run:

  xhost +

• If you see "Depth Camera not found" in base_dynus.launch..py, try rebuilding the docker image.

Error Troubleshooting:

• Error:

  docker: Error response from daemon: could not select device driver "" with capabilities: [[gpu]].
  make: *** [Makefile:11: run-sim] Error 125
  

• Solution:
  • Follow this StackOverflow thread.
  • Use run-sim-no-gpu in dynus/docker/Makefile (currently Lambda machines' GPU is not working—should be fixed soon).
  • Note: Realsense won’t work without a display. (SSH with -X may not work; a physical monitor is required.)
  • Once the simulation is running, the bag file is saved inside Docker. To copy it to your local machine:

    docker cp <container_id>:/path/to/bag/file /path/to/local/machine

    (Find <container_id> by running docker ps.)

Goal Sender Examples:

• Forest3 Goal Sender (Straight Path):

  ros2 launch dynus goal_sender.launch.py list_agents:="['NX01', 'NX02', 'NX03', 'NX04', 'NX05']" list_goals:="['[50.0, 20.0, 0.0]', '[50.0, 10.0, 0.0]', '[50.0, 0.0, 0.0]', '[50.0, -10.0, 0.0]', '[50.0, -20.0, 0.0]']" default_goal_z:=2.0

• Forest3 Goal Sender (Cross Path):

  ros2 launch dynus goal_sender.launch.py list_agents:="['NX01', 'NX02', 'NX03', 'NX04', 'NX05']" list_goals:="['[50.0, -20.0, 0.0]', '[50.0, -10.0, 0.0]', '[50.0, 0.0, 0.0]', '[50.0, 10.0, 0.0]', '[50.0, 20.0, 0.0]']" default_goal_z:=2.0

• Big Forest High Res Goal Sender (Straight Path):

  ros2 launch dynus goal_sender.launch.py list_agents:="['NX01', 'NX02', 'NX03', 'NX04', 'NX05']" list_goals:="['[35.0, 20.0, 0.0]', '[35.0, 10.0, 0.0]', '[35.0, 0.0, 0.0]', '[35.0, -10.0, 0.0]', '[35.0, -20.0, 0.0]']" default_goal_z:=3.0

• Big Forest High Res Goal Sender (Cross Path):

  ros2 launch dynus goal_sender.launch.py list_agents:="['NX01', 'NX02', 'NX03', 'NX04', 'NX05']" list_goals:="['[35.0, -20.0, 0.0]', '[35.0, -10.0, 0.0]', '[35.0, 0.0, 0.0]', '[35.0, 10.0, 0.0]', '[35.0, 20.0, 0.0]']" default_goal_z:=3.0

• Quadruped Forest3 Goal Sender:

  ros2 launch dynus goal_sender.launch.py list_agents:="['NX01']" list_goals:="['[45.0, 0.0, 0.0]']" default_goal_z:=1.0