diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..25ecb17ef --- /dev/null +++ b/.gitmodules @@ -0,0 +1,15 @@ +[submodule "octomap_msgs"] + path = octomap_msgs + url = https://github.com/OctoMap/octomap_msgs + branch = ros2 +[submodule "octomap_server2"] + path = octomap_server2 + url = https://github.com/ldg810/octomap_server2 +[submodule "perception_pcl"] + path = perception_pcl + url = https://github.com/ros-perception/perception_pcl + branch = foxy-devel +[submodule "pcl_msgs"] + path = pcl_msgs + url = https://github.com/ros-perception/pcl_msgs + branch = ros2 diff --git a/README.md b/README.md index 96f0f12ac..fa1ae6cb2 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,4 @@ + # Obstacle Detection and Avoidance [![Release Status](https://img.shields.io/github/v/release/PX4/avoidance)](https://github.com/PX4/avoidance/releases) [![Build Status](https://travis-ci.org/PX4/avoidance.svg?branch=master)](https://travis-ci.org/PX4/avoidance) @@ -50,119 +51,33 @@ The documentation contains information about how to setup and run the two planne ### Installation This is a step-by-step guide to install and build all the prerequisites for running the avoidance module on either: -- **Ubuntu 18.04:** *ROS Melodic* with Gazebo 9 (preferred). -- **Ubuntu 16.04:** *ROS Kinetic* with Gazebo 7 +- **Ubuntu 20.04:** *ROS2 Foxy* with Gazebo 11 (preferred). You might want to skip some steps if your system is already partially installed. -> **Note:** These instructions assume your catkin workspace (in which we will build the avoidance module) is in `~/catkin_ws`, and the PX4 Firmware directory is `~/Firmware`. +> **Note:** These instructions assume your workspace (in which we will build the avoidance module) is in `~/avoidance_ws/src/`, and the PX4 Firmware directory is `~/Firmware`. Feel free to adapt this to your situation. -1. Add ROS to sources.list: - * Melodic - ```bash - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - sudo apt update - ``` - * Kinetic - ```bash - echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - sudo apt update - ``` - -1. Install ROS with Gazebo: - * ROS Melodic (with Gazebo 9) - ```bash - sudo apt install ros-melodic-desktop-full - - # Source ROS - echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc - source ~/.bashrc - ``` - * ROS Kinetic (with Gazebo 7) - ```bash - sudo apt install ros-kinetic-desktop-full - - # Source ROS - source /opt/ros/kinetic/setup.bash - ``` - > **Note** We recommend you use the version of Gazebo that comes with your (full) installation of ROS. - > If you must to use another Gazebo version, remember to install associated ros-gazebo related packages: - > - For Gazebo 8, - ```sh - sudo apt install ros-kinetic-gazebo8-* - ``` - > - For Gazebo 9, - ``` - sudo apt install ros-kinetic-gazebo9-* - ``` - -1. Initialize rosdep. - ```bash - rosdep init - rosdep update - ``` - -1. Install catkin and create your catkin workspace directory. - - ```bash - sudo apt install python-catkin-tools - mkdir -p ~/catkin_ws/src - ``` - -1. Install MAVROS (version 0.29.0 or above). - > **Note:** Instructions to install MAVROS from sources can be found [here](https://dev.px4.io/en/ros/mavros_installation.html). - - * Melodic - ```bash - sudo apt install ros-melodic-mavros ros-melodic-mavros-extras - ``` - * Kinetic - ```bash - sudo apt install ros-kinetic-mavros ros-kinetic-mavros-extras - ``` - -1. Install the *geographiclib* dataset - - ```bash - wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh - chmod +x install_geographiclib_datasets.sh - sudo ./install_geographiclib_datasets.sh - ``` - -1. Install avoidance module dependencies (pointcloud library and octomap). - - Melodic - ```bash - sudo apt install libpcl1 ros-melodic-octomap-* - ``` - - Kinetic - ```bash - sudo apt install libpcl1 ros-kinetic-octomap-* ros-kinetic-yaml-* - ``` +1. Install ROS foxy to your PC. Follow steps on [#Installing ROS 2 via Debian Packages](https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Install-Debians/) -1. Clone this repository in your catkin workspace in order to build the avoidance node. +1. Clone this repository in your avoidance workspace in order to build the avoidance node (ros2 branch) and px4_ros_com to use micrortps_agent ```bash - cd ~/catkin_ws/src - git clone https://github.com/PX4/avoidance.git + cd ~/avoidance_ws/src/ + git clone -b ros2 --recursive https://github.com/PX4/avoidance.git + git clone https://github.com/PX4/px4_ros_com.git ``` -1. Actually build the avoidance node. +1. Actually build the avoidance node. (exclude local_planner since it is not fully working at this time) ```bash - catkin build -w ~/catkin_ws + cd ~/avoidance_ws + colcon build --packages-skip local_planner ``` - Note that you can build the node in release mode this way: - - ```bash - catkin build -w ~/catkin_ws --cmake-args -DCMAKE_BUILD_TYPE=Release - ``` -1. Source the catkin setup.bash from your catkin workspace: +1. Source the workspace setup.bash from your avoidance workspace: ```bash - echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc + echo "source ~/avoidance_ws/src/PX4-Avoidance/install/setup.bash" >> ~/.bashrc source ~/.bashrc ``` @@ -170,7 +85,7 @@ You might want to skip some steps if your system is already partially installed. In the following section we guide you through installing and running a Gazebo simulation of both local and global planner. -### Build and Run the Simulator +### Build the Simulator 1. Clone the PX4 Firmware and all its submodules (it may take some time). @@ -196,115 +111,78 @@ In the following section we guide you through installing and running a Gazebo si export QT_X11_NO_MITSHM=1 # Build and run simulation - make px4_sitl_default gazebo + make px4_sitl_rtps gazebo # Quit the simulation (Ctrl+C) # Setup some more Gazebo-related environment variables (modify this line based on the location of the Firmware folder on your machine) - . ~/Firmware/Tools/setup_gazebo.bash ~/Firmware ~/Firmware/build/px4_sitl_default + . ~/Firmware/Tools/setup_gazebo.bash ~/Firmware ~/Firmware/build/px4_sitl_rtps ``` -1. Add the Firmware directory to ROS_PACKAGE_PATH so that ROS can start PX4: +1. Set the GAZEBO_MODEL_PATH in your bashrc: ```bash - export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/Firmware - ``` -1. Finally, set the GAZEBO_MODEL_PATH in your bashrc: - ```bash - echo "export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:~/catkin_ws/src/avoidance/avoidance/sim/models:~/catkin_ws/src/avoidance/avoidance/sim/worlds" >> ~/.bashrc + echo "export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:~/avoidance_ws/src/PX4-avoidance/avoidance/sim/models:~/avoidance_ws/src/PX4-avoidance/avoidance/sim/worlds" >> ~/.bashrc ``` -The last three steps, together with sourcing your catkin **setup.bash** (`source ~/catkin_ws/devel/setup.bash`) should be repeated each time a new terminal window is open. +The last three steps, together with sourcing your ros2 workspace **setup.bash** (`source ~/avoidance_ws/install/setup.bash`) should be repeated each time a new terminal window is open. You should now be ready to run the simulation using local or global planner. -### Local Planner (default, heavily flight tested) +### Run gazebo simulator for using planners + +1. Run px4_sitl_rtps + ```bash + # Build and run simulation + make px4_sitl_rtps gazebo + ``` + +2. Run px4_ros_com to send/receive msgs with drone +```bash + ~/avoidance_ws/build/px4_ros_com/micrortps_agent -t UDP +``` + +3. Check communication via ros2 has no problem +```bash + # See which topics are existing + ros2 topic list + + # Echo topic data for test + ros2 topic echo /VehicleLocalPosition_PubSubTopic +``` + +### Local Planner (default, ros2 version is not ready) This section shows how to start the *local_planner* and use it for avoidance in mission or offboard mode. The planner is based on the [3DVFH+](http://ceur-ws.org/Vol-1319/morse14_paper_08.pdf) algorithm. > **Note:** You *may* need to install some additional dependencies to run the following code (if not installed): -> * Melodic: +> * Foxy: > ```sh -> sudo apt install ros-melodic-stereo-image-proc ros-melodic-image-view -> ``` -> * Kinetic: -> ```sh -> sudo apt install ros-kinetic-stereo-image-proc ros-kinetic-image-view +> sudo apt install ros-foxy-stereo-image-proc ros-foxy-image-view > ``` -Any of the following three launch file scripts can be used to run local planner: -> **Note:** The scripts run the same planner but simulate different sensor/camera setups. They all enable *Obstacle Avoidance* and *Collision Prevention*. -* `local_planner_stereo`: simulates a vehicle with a stereo camera that uses OpenCV's block matching algorithm (SGBM by default) to generate depth information - ```bash - roslaunch local_planner local_planner_stereo.launch - ``` - - > **Note:** The disparity map from `stereo-image-proc` is published as a [stereo_msgs/DisparityImage](http://docs.ros.org/api/stereo_msgs/html/msg/DisparityImage.html) message, which is not supported by rviz or rqt. - > To visualize the message, first open a *new terminal* and setup the required environment variables: - > ```bash - > source devel/setup.bash - > ``` - > Then do either of: - > - run: - > ```bash - > rosrun image_view stereo_view stereo:=/stereo image:=image_rect_color - > ``` - > - publish the `DisparityImage` as a simple `sensor_msgs/Image`: - > ```bash - > rosrun topic_tools transform /stereo/disparity /stereo/disparity_image sensor_msgs/Image 'm.image' - > ``` - > The disparity map can then be visualized by *rviz* or *rqt* under the topic */stereo/disparity_image*. - -* `local_planner_depth_camera`: simulates vehicle with one forward-facing kinect sensor - ```bash - roslaunch local_planner local_planner_depth-camera.launch - ``` - -* `local_planner_sitl_3cam`: simulates vehicle with 3 kinect sensors (left, right, front) - ```bash - roslaunch local_planner local_planner_sitl_3cam.launch - ``` - -You will see the Iris drone unarmed in the Gazebo world. -To start flying, there are two options: OFFBOARD or MISSION mode. -For OFFBOARD, run: +** Documentation for local planner in ros2 environment should be written here. ** -```bash -# In another terminal -rosrun mavros mavsys mode -c OFFBOARD -rosrun mavros mavsafety arm -``` - -The drone will first change its altitude to reach the goal height. -It is possible to modify the goal altitude with `rqt_reconfigure` GUI. -![Screenshot rqt_reconfigure goal height](docs/lp_goal_height.png) -Then the drone will start moving towards the goal. -The default x, y goal position can be changed in Rviz by clicking on the 2D Nav Goal button and then choosing the new goal x and y position by clicking on the visualized gray space. -If the goal has been set correctly, a yellow sphere will appear where you have clicked in the grey world. -![Screenshot rviz goal selection](docs/lp_goal_rviz.png) - -For MISSIONS, open [QGroundControl](http://qgroundcontrol.com/) and plan a mission as described [here](https://docs.px4.io/en/flight_modes/mission.html). Set the parameter `COM_OBS_AVOID` true. -Start the mission and the vehicle will fly the mission waypoints dynamically recomputing the path such that it is collision free. +### Global Planner (advanced, simulation tested) -### Global Planner (advanced, not flight tested) - -This section shows how to start the *global_planner* and use it for avoidance in offboard mode. +This section shows how to start the *global_planner* and use it for avoidance in auto_loiter mode. ```bash -roslaunch global_planner global_planner_stereo.launch +ros2 launch global_planner global_planner_depth_camera.launch.py ``` -You should now see the drone unarmed on the ground in a forest environment as pictured below. +You should now see the drone unarmed on the ground in a forest environment as pictured below. (gazebo and rviz2) +![Screenshot showing gazebo](docs/gazebo_simple_obstacle_screenshot.png) -![Screenshot showing gazebo and rviz](docs/simulation_screenshot.png) +![Screenshot showing rviz](docs/simulation_screenshot.png) -To start flying, put the drone in OFFBOARD mode and arm it. The avoidance node will then take control of it. +To start flying, arm it. The avoidance node will then take control of it. +You may use QGroundControl to arm the drone. ```bash # In another terminal -rosrun mavros mavsys mode -c OFFBOARD -rosrun mavros mavsafety arm +ros2 topic pub --once /VehicleCommand_PubSubTopic px4_msgs/msg/VehicleCommand "{target_system: 1, command: 400, param1: 1.0, from_external: true}" ``` Initially the drone should just hover at 3.5m altitude. @@ -320,12 +198,12 @@ The planned path should show up in rviz and the drone should follow the path, up It is also possible to set a goal without using the obstacle avoidance (i.e. the drone will go straight to this goal and potentially collide with obstacles). To do so, set the position with the *2D Pose Estimate* button in rviz. -### Safe Landing Planner +### Safe Landing Planner (ros2 version is not ready) This section shows how to start the *safe_landing_planner* and use it to land safely in mission or auto land mode. To run the node: ```bash -roslaunch safe_landing_planner safe_landing_planner.launch +ros2 launch safe_landing_planner safe_landing_planner.launch.py ``` You will see an unarmed vehicle on the ground. Open [QGroundControl](http://qgroundcontrol.com/), either plan a mission with the last item of type *Land* or fly around the world in Position Control, click the *Land* button on the left side where you wish to land. @@ -334,155 +212,12 @@ If the ground is flat, the vehicle will continue landing. Otherwise it will eval # Run on Hardware -## Prerequisite - -### Camera - -Both planners require a 3D point cloud of type `sensor_msgs::PointCloud2`. Any camera that can provide such data is compatible. - -The officially supported camera is Intel Realsense D435. We recommend using Firmware version 5.9.13.0. The instructions on how to update the Firmware of the camera can be found [here](https://www.intel.com/content/www/us/en/support/articles/000028171/emerging-technologies/intel-realsense-technology.html) - -> **Tip:** Be careful when attaching the camera with a USB3 cable. USB3 might might interfere with GPS and other signals. If possible, always use USB2 cables. - -Other tested camera models are: Intel Realsense D415 and R200, Occipital Structure Core. - -#### Generating Point-clouds from Depth-maps - -In case the point-cloud stream already exists, this step can be skipped. - -Assuming there already exists a stream of depth-maps on the ROS-topic , we need to generate a corresponding stream of depth-maps. -Start by following the instructions from [PX4/disparity_to_point_cloud](https://github.com/PX4/disparity_to_point_cloud). -Now run the point-cloud generation with the parameters for the camera intrinsics: - -```bash -rosrun disparity_to_point_cloud disparity_to_point_cloud_node \ - fx_:=fx fy_:=fy cx_:=cx cy_:=cy base_line_:=base_line disparity:= -``` - -A stream of point-clouds should now be published to */point_cloud*. - -### PX4 Autopilot - -Parameters to set through QGC: -* `COM_OBS_AVOID` to Enabled -* `MAV_1_CONFIG`, `MAV_1_MODE`, `SER_TEL2_BAUD` to enable MAVLink on a serial port. For more information: [PX4 Dev Guide](http://dev.px4.io/en/companion_computer/pixhawk_companion.html#pixhawk-setup) - -### Companion Computer - -* OS: Ubuntu 16.04 OS or a docker container running Ubuntu 16.04 must be setup (e.g. if using on a Yocto based system). -* ROS Kinetic: see [Installation](#installaton) -* Other Required Components for Intel Realsense: - - Librealsense (Realsense SDK). The installation instructions can be found [here](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) - - [Librealsense ROS wrappers](https://github.com/intel-ros/realsense.git) -* Other Required Components for Occipital Structure Core: - - Download the [Structure SDK](https://structure.io/developers). The version tested with this package is `0.7.1`. Create the `build` directory and build the SDK - ```bash - mkdir build - cd build - cmake .. - make - ``` - - Clone the [ROS wrapper](https://github.com/Auterion/struct_core_ros) in the `catkin_ws` - - Copy the shared object `Libraries/Structure/Linux/x86_64/libStructure.so` from the SDK into `/usr/local/lib/` - - Copy the headers from `Libraries/Structure/Headers/` in the SDK to the ROS wrapper include directory `~/catkin_ws/src/struct_core_ros/include` - -Tested models: -- local planner: Intel NUC, Jetson TX2, Intel Atom x7-Z8750 (built-in on Intel Aero RTF drone) -- global planner: Odroid - -## Global Planner - -The global planner has been so far tested on a Odroid companion computer by the development team. - -## Local Planner - -Once the catkin workspace has been built, to run the planner with a Realsense D435 or Occipital Structure Core camera you can generate the launch file using the script *generate_launchfile.sh* - -1. `export CAMERA_CONFIGS="camera_namespace, camera_type, serial_n, tf_x, tf_y, tf_z, tf_yaw, tf_pitch, tf_roll"` where `camera_type` is either `realsense` or `struct_core_ros`, `tf_*` represents the displacement between the camera and the flight controller. If more than one camera is present, list the different camera configuration separated by a semicolon. Within each camera configuration the parameters are separated by commas. -2. `export DEPTH_CAMERA_FRAME_RATE=frame_rate`. If this variable isn't set, the default frame rate will be taken. -3. `export VEHICLE_CONFIG=/path/to/params.yaml` where the yaml file contains the value of some parameters different from the defaults set in the cfg file. If this variable isn't set, the default parameters values will be used. - -Changing the serial number and `DEPTH_CAMERA_FRAME_RATE` don't have any effect on the Structure Core. - -For example: -```bash -export CAMERA_CONFIGS="camera_main,realsense,819612070807,0.3,0.32,-0.11,0,0,0" -export DEPTH_CAMERA_FRAME_RATE=30 -export VEHICLE_CONFIG=~/catkin_ws/src/avoidance/local_planner/cfg/params_vehicle_1.yaml -./tools/generate_launchfile.sh -roslaunch local_planner avoidance.launch fcu_url:=/dev/ttyACM0:57600 -``` - -where `fcu_url` representing the port connecting the companion computer to the flight controller. -The planner is running correctly if the rate of the processed point cloud is around 10-20 Hz. To check the rate run: - -```bash -rostopic hz /local_pointcloud -``` - -If you would like to read debug statements on the console, please change `custom_rosconsole.conf` to -```bash -log4j.logger.ros.local_planner=DEBUG -``` - -## Safe Landing Planner - -Once the catkin workspace has been built, to run the planner with a Realsense D435 and Occipital Structure Core, you can generate the launch file using the script *generate_launchfile.sh*. The script works the same as described in the section above for the Local Planner. For example: - -```bash -export CAMERA_CONFIGS="camera_main,struct_core,819612070807,0.3,0.32,-0.11,0,0,0" -export VEHICLE_CONFIG_SLP=~/catkin_ws/src/avoidance/safe_landing_planner/cfg/slpn_structure_core.yaml -export VEHICLE_CONFIG_WPG=~/catkin_ws/src/avoidance/safe_landing_planner/cfg/wpgn_structure_core.yaml -./safe_landing_planner/tools/generate_launchfile.sh -roslaunch safe_landing_planner safe_landing_planner_launch.launch -``` - -In the `cfg/` folder there are camera specific configurations for the algorithm nodes. These parameters can be loaded by specifying the file in the `VEHICLE_CONFIG_SLP` and `VEHICLE_CONFIG_WPG` system variable for the safe_landing_planner_node and for the waypoint_generator_node respectively. - -The size of the squared shape patch of terrain below the vehicle that is evaluated by the algorithm can be changed to suit different vehicle sizes with the WaypointGeneratorNode parameter `smoothing_land_cell`. The algorithm behavior will also be affected by the height at which the decision to land or not is taken (`loiter_height` parameter in WaypointGeneratorNode) and by the size of neighborhood filter smoothing (`smoothing_size` in LandingSiteDetectionNode). - -For different cameras you might also need to tune the thresholds on the number of points in each bin, standard deviation and mean. +Testing avoidance nodes in ros2 environment is not yet done. +We need support to test nodes with ros2. # Troubleshooting -### I see the drone position in rviz (shown as a red arrow), but the world around is empty -Check that some camera topics (including */camera/depth/points*) are published with the following command: - -```bash -rostopic list | grep camera -``` - -If */camera/depth/points* is the only one listed, it may be a sign that gazebo is not actually publishing data from the simulated depth camera. Verify this claim by running: - -```bash -rostopic echo /camera/depth/points -``` - -When everything runs correctly, the previous command should show a lot of unreadable data in the terminal. If you don't receive any message, it probably means that gazebo is not publishing the camera data. - -Check that the clock is being published by Gazebo: - -```bash -rostopic echo /clock -``` - -If it is not, you have a problem with Gazebo (Did it finish loading the world? Do you see the buildings and the drone in the Gazebo UI?). However, if it is publishing the clock, then it might be a problem with the depth camera plugin. Make sure the package `ros-kinetic-gazebo-ros-pkgs` is installed. If not, install it and rebuild the Firmware (with `$ make px4_sitl_default gazebo` as explained above). - -### I see the drone and world in rviz, but the drone does not move when I set a new "2D Nav Goal" -Is the drone in OFFBOARD mode? Is it armed and flying? - -```bash -# Set the drone to OFFBOARD mode -rosrun mavros mavsys mode -c OFFBOARD -# Arm -rosrun mavros mavsafety arm -``` - -### I see the drone and world in rviz, but the drone does not follow the path properly -Some tuning may be required in the file *"/posix-configs/SITL/init/rcS_gazebo_iris"*. - -### I see the drone and world in rviz, I am in OFFBOARD mode, but the planner is still not working -Some parameters that can be tuned in *rqt reconfigure*. +Troubleshooting for ros2 environment is not yet written. # Advanced @@ -492,65 +227,29 @@ More information about the communication between avoidance system and the Autopi ### PX4 and local planner -This is the complete message flow *from* PX4 Firmware to the local planner. - -PX4 topic | MAVLink | MAVROS Plugin | ROS Msgs. | ROS Topic ---- | --- | --- | --- | --- -vehicle_local_position | LOCAL_POSITION_NED | local_position | geometry_msgs::PoseStamped | mavros/local_position/pose -vehicle_local_position | LOCAL_POSITION_NED | local_position | geometry_msgs::TwistStamped | mavros/local_position/velocity -vehicle_local_position | ALTITUDE | altitude | mavros_msgs::Altitude | mavros/altitude -home_position | ALTITUDE | altitude | mavros_msgs::Altitude | mavros/altitude -vehicle_air_data | ALTITUDE | altitude | mavros_msgs::Altitude | mavros/altitude -vehicle_status | HEARTBEAT | sys_status | mavros_msgs::State | mavros/state -vehicle_trajectory_waypoint_desired | TRAJECTORY_REPRESENTATION_WAYPOINT | trajectory | mavros_msgs::Trajectory | mavros/trajectory/desired -- | MAVLINK_MSG_ID_PARAM_REQUEST_LIST | param | mavros_msgs::Param | /mavros/param/param_value -- | MISSION_ITEM | waypoint | mavros_msgs::WaypointList | /mavros/mission/waypoints - - -This is the complete message flow *to* PX4 Firmware from the local planner. - -ROS topic | ROS Msgs. | MAVROS Plugin | MAVLink | PX4 Topic ---- | --- | --- | --- | --- -/mavros/setpoint_position/local (offboard) | geometry_msgs::PoseStamped | setpoint_position | SET_POSITION_LOCAL_POSITION_NED | position_setpoint_triplet -/mavros/trajectory/generated (mission) | mavros_msgs::Trajectory | trajectory | TRAJECTORY_REPRESENTATION_WAYPOINT | vehicle_trajectory_waypoint -/mavros/obstacle/send | sensor_msgs::LaserScan | obstacle_distance | OBSTACLE_DISTANCE | obstacle_distance -/mavros/companion_process/status | mavros_msgs::CompanionProcessStatus | companion_process_status | HEARTBEAT | telemetry_status +Message flow for ros2 environment is not yet written. ### PX4 and global planner This is the complete message flow *from* PX4 Firmware *to* the global planner. -PX4 topic | MAVLink | MAVROS Plugin | ROS Msgs. | Topic ---- | --- | --- | --- | --- -vehicle_local_position | LOCAL_POSITION_NED | local_position | geometry_msgs::PoseStamped | mavros/local_position/pose -vehicle_local_position | LOCAL_POSITION_NED | local_position | geometry_msgs::TwistStamped | mavros/local_position/velocity -vehicle_trajectory_waypoint_desired | TRAJECTORY_REPRESENTATION_WAYPOINT | trajectory | mavros_msgs::Trajectory | mavros/trajectory/desired +PX4 topic | MAVLink | ROS Msgs. | Topic +--- | --- | --- | --- +vehicle_attitude | ATTITUDE | px4_msgs::msg::VehicleAttitude | /VehicleAttitude_PubSubTopic +vehicle_global_position | GLOBAL_POSITION_INT | px4_msgs::msg::VehicleGlobalPosition | /VehicleGlobalPosition_PubSubTopic +vehicle_local_position | LOCAL_POSITION_NED | px4_msgs::msg::VehicleLocalPosition | /VehicleLocalPosition_PubSubTopic This is the complete message flow *to* PX4 Firmware *from* the global planner. -ROS topic | ROS Msgs. | MAVROS Plugin | MAVLink | PX4 Topic ---- | --- | --- | --- | --- -/mavros/setpoint_position/local (offboard) | geometry_msgs::PoseStamped | setpoint_position | SET_POSITION_LOCAL_POSITION_NED | position_setpoint_triplet -/mavros/trajectory/generated (mission) | mavros_msgs::Trajectory | trajectory | TRAJECTORY_REPRESENTATION_WAYPOINT | vehicle_trajectory_waypoint +ROS topic | ROS Msgs. | MAVLink | PX4 Topic +--- | --- | --- | --- +/VehicleCommand_PubSubTopic | px4_msgs::msg::VehicleCommand | VEHICLE_COMMAND | vehicle_command ### PX4 and safe landing planner -This is the complete message flow *from* PX4 Firmware to the safe landing planner. - -PX4 topic | MAVLink | MAVROS Plugin | ROS Msgs. | ROS Topic ---- | --- | --- | --- | --- -vehicle_local_position | LOCAL_POSITION_NED | local_position | geometry_msgs::PoseStamped | mavros/local_position/pose -vehicle_status | HEARTBEAT | sys_status | mavros_msgs::State | mavros/state -vehicle_trajectory_waypoint_desired | TRAJECTORY_REPRESENTATION_WAYPOINT | trajectory | mavros_msgs::Trajectory | mavros/trajectory/desired -- | MAVLINK_MSG_ID_PARAM_REQUEST_LIST | param | mavros_msgs::Param | /mavros/param/param_value - -This is the complete message flow *to* PX4 Firmware from the safe landing planner. +Message flow for ros2 environment is not yet written. -ROS topic | ROS Msgs. | MAVROS Plugin | MAVLink | PX4 Topic ---- | --- | --- | --- | --- -/mavros/trajectory/generated (mission) | mavros_msgs::Trajectory | trajectory | TRAJECTORY_REPRESENTATION_WAYPOINT | vehicle_trajectory_waypoint -/mavros/companion_process/status | mavros_msgs::CompanionProcessStatus | companion_process_status | HEARTBEAT | telemetry_status # Contributing diff --git a/avoidance/CMakeLists.txt b/avoidance/CMakeLists.txt index 6576beacf..02e515026 100644 --- a/avoidance/CMakeLists.txt +++ b/avoidance/CMakeLists.txt @@ -23,7 +23,9 @@ find_package(PythonInterp REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED NO_MODULE) find_package(Boost REQUIRED COMPONENTS system) -find_package(PCL REQUIRED) +find_package(PCL 1.8 REQUIRED) + +link_directories(${PCL_LIBRARY_DIRS}) if(DISABLE_SIMULATION) message(STATUS "Building avoidance without Gazebo Simulation") @@ -64,7 +66,7 @@ endif() # Add avoidance lib add_library(avoidance SHARED "${AVOIDANCE_CPP_FILES}") -ament_target_dependencies(avoidance Eigen3 px4_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS}) +ament_target_dependencies(avoidance Eigen3 px4_msgs PCL ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_include_directories(avoidance PUBLIC $ $ @@ -75,15 +77,16 @@ target_include_directories(avoidance PUBLIC target_link_libraries( avoidance Eigen3::Eigen + ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES} ) # Add node dependencies -ament_target_dependencies(avoidance px4_msgs rclcpp Eigen3::Eigen) +ament_target_dependencies(avoidance px4_msgs rclcpp Eigen3) # Export information to downstream packages ament_export_dependencies(ament_cmake rclcpp rosidl_default_runtime eigen3_cmake_module Eigen3 px4_msgs geometry_msgs sensor_msgs std_msgs) -ament_export_interfaces(export_avoidance HAS_LIBRARY_TARGET) +ament_export_targets(export_avoidance HAS_LIBRARY_TARGET) ament_export_include_directories(include) ament_export_libraries(avoidance) @@ -113,6 +116,11 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME} ) +install(DIRECTORY + sim + DESTINATION share/${PROJECT_NAME} +) + ############# ## Testing ## ############# diff --git a/avoidance/include/avoidance/avoidance_node.h b/avoidance/include/avoidance/avoidance_node.h index a60dc2bb3..15ebe0c07 100644 --- a/avoidance/include/avoidance/avoidance_node.h +++ b/avoidance/include/avoidance/avoidance_node.h @@ -5,11 +5,16 @@ #include "avoidance/common.h" #include +#include +#include +#include #include namespace avoidance { +using std::placeholders::_1; + class AvoidanceNode { public: AvoidanceNode(); @@ -37,13 +42,20 @@ class AvoidanceNode { private: // telemetry_status Publisher rclcpp::Publisher::SharedPtr telemetry_status_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr position_sub_; rclcpp::executors::MultiThreadedExecutor cmdloop_executor_; rclcpp::executors::MultiThreadedExecutor statusloop_executor_; - rclcpp::TimerBase::SharedPtr cmdloop_timer_; rclcpp::TimerBase::SharedPtr statusloop_timer_; + std::thread* statusloop_thread; + std::thread* cmdloop_thread; + + rclcpp::Node::SharedPtr avoidance_node_status; + rclcpp::Node::SharedPtr avoidance_node_cmd; + MAV_STATE companion_state_ = MAV_STATE::MAV_STATE_STANDBY; // PX4 Firmware parameters @@ -56,15 +68,16 @@ class AvoidanceNode { rclcpp::Duration timeout_critical_; rclcpp::Duration timeout_startup_; + int agent_number_ = 1; bool position_received_; bool should_exit_; float mission_item_speed_; void publishSystemStatus(); - + rclcpp::Logger avoidance_node_logger_ = rclcpp::get_logger("avoidance_node"); }; } -#endif // AVOIDANCE_AVOIDANCE_NODE_H +#endif // AVOIDANCE_AVOIDANCE_NODE_H \ No newline at end of file diff --git a/avoidance/include/avoidance/common.h b/avoidance/include/avoidance/common.h index 336dc346e..3877dbd32 100644 --- a/avoidance/include/avoidance/common.h +++ b/avoidance/include/avoidance/common.h @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -294,6 +295,19 @@ float getYawFromQuaternion(const Eigen::Quaternionf q); **/ float getPitchFromQuaternion(const Eigen::Quaternionf q); + +/** +* @brief Tranform yaw angle to quaternion msg +* @returns quaternion msg (in geometry_msgs) +**/ +geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw); + +/** +* @brief Tranform between NED PoseStamped msg and ENU PoseStamped msg +* @returns PoseStamped msg (in geometry_msgs) +**/ +geometry_msgs::msg::PoseStamped transformNEDandENU(geometry_msgs::msg::PoseStamped pose); + /** * @brief wrappes the input angle in to plus minus PI space * @param[in] angle to be wrapped [rad] diff --git a/avoidance/include/avoidance/rviz_world_loader.h b/avoidance/include/avoidance/rviz_world_loader.h index 6dc43b2fc..3949d52ee 100644 --- a/avoidance/include/avoidance/rviz_world_loader.h +++ b/avoidance/include/avoidance/rviz_world_loader.h @@ -13,12 +13,21 @@ #include "rclcpp/rclcpp.hpp" #include #include -#include +// #include +#include +#include +#include +#include +#include +#include #include namespace avoidance { +using std::placeholders::_1; +using namespace std::chrono_literals; + // data types struct world_object { std::string type; @@ -44,15 +53,22 @@ class WorldVisualizer : public rclcpp::Node int resolveUri(std::string& uri) const; rclcpp::TimerBase::SharedPtr loop_timer_; + rclcpp::TimerBase::SharedPtr visualize_drone_timer_; - rclcpp::Subscription::SharedPtr pose_sub_; + // rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Publisher::SharedPtr world_pub_; rclcpp::Publisher::SharedPtr drone_pub_; std::string world_path_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr transform_broadcaster_; + void loopCallback(); + void tf2Callback(const std::shared_future& tf); + public: /** * @brief WorldVisualizer Node Class @@ -63,7 +79,7 @@ class WorldVisualizer : public rclcpp::Node /** * @brief callback for subscribing mav pose topic **/ - void positionCallback(const px4_msgs::msg::VehicleOdometry::SharedPtr msg) const; + // void positionCallback(const px4_msgs::msg::VehicleOdometry::SharedPtr msg) const; /** * @brief parse the yaml file and publish world marker @@ -75,7 +91,7 @@ class WorldVisualizer : public rclcpp::Node * @brief visualize the drone mesh at the current drone position * @param[in] pose, current drone pose **/ - int visualizeDrone(const px4_msgs::msg::VehicleOdometry& pose) const; + void visualizeDrone(); }; } diff --git a/avoidance/sim/worlds/simple_obstacle.yaml b/avoidance/sim/worlds/simple_obstacle.yaml index b029f400e..82c708b0e 100644 --- a/avoidance/sim/worlds/simple_obstacle.yaml +++ b/avoidance/sim/worlds/simple_obstacle.yaml @@ -1,7 +1,7 @@ - type: "mesh" name: "pinetree1" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://pine_tree/meshes/pine_tree.dae" position: [4.0, 7.0, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -9,7 +9,7 @@ - type: "mesh" name: "pinetree2" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://pine_tree/meshes/pine_tree.dae" position: [6.1, 7.6, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -17,7 +17,7 @@ - type: "mesh" name: "pinetree3" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://pine_tree/meshes/pine_tree.dae" position: [19.3, -14.0, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -25,7 +25,7 @@ - type: "mesh" name: "pinetree4" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://pine_tree/meshes/pine_tree.dae" position: [16.0, -11.5, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -33,7 +33,7 @@ - type: "mesh" name: "oaktree1" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://oak_tree/meshes/oak_tree.dae" position: [10.0, 0.0, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -41,7 +41,7 @@ - type: "mesh" name: "oaktree2" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://oak_tree/meshes/oak_tree.dae" position: [10.25, -4.25, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -49,7 +49,7 @@ - type: "mesh" name: "oaktree3" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://oak_tree/meshes/oak_tree.dae" position: [20.2, 14.4, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -57,7 +57,7 @@ - type: "mesh" name: "oaktree4" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://oak_tree/meshes/oak_tree.dae" position: [30.0, 8.5, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -65,7 +65,7 @@ - type: "mesh" name: "oaktree5" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://oak_tree/meshes/oak_tree.dae" position: [136.2, -0.4, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -73,7 +73,7 @@ - type: "mesh" name: "oaktree6" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://oak_tree/meshes/oak_tree.dae" position: [38.0, -9.8, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -81,7 +81,7 @@ - type: "mesh" name: "oaktree7" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://oak_tree/meshes/oak_tree.dae" position: [38.6, -4.6, 0.0] orientation: [0.0, 0.0, 0.0, 1.0] @@ -89,7 +89,7 @@ - type: "mesh" name: "oaktree8" - frame_id: "local_origin" + frame_id: "base_frame" mesh_resource: "model://oak_tree/meshes/oak_tree.dae" position: [16.0, 30.0, 1.6] orientation: [0.0, 0.0, 0.0, 1.0] diff --git a/avoidance/src/avoidance_node.cpp b/avoidance/src/avoidance_node.cpp index 06485a107..3a2c39ea0 100644 --- a/avoidance/src/avoidance_node.cpp +++ b/avoidance/src/avoidance_node.cpp @@ -9,7 +9,7 @@ namespace avoidance { AvoidanceNode::AvoidanceNode() : cmdloop_dt_(100ms), statusloop_dt_(200ms), - timeout_termination_(15000000000ns), // ns + timeout_termination_(15000000000000ns), // ns timeout_critical_(500000000ns), // ns timeout_startup_(5000000000ns), // ns position_received_(true), @@ -29,29 +29,27 @@ AvoidanceNode::~AvoidanceNode() {} void AvoidanceNode::init() { setSystemStatus(MAV_STATE::MAV_STATE_BOOT); - // Command loop executor - auto cmd_loop_callback = [&]() { /* Empty ?? */ }; - auto avoidance_node_cmd = rclcpp::Node::make_shared("avoidance_node_cmd"); - cmdloop_timer_ = avoidance_node_cmd->create_wall_timer(cmdloop_dt_, cmd_loop_callback); + + avoidance_node_cmd = rclcpp::Node::make_shared("avoidance_node_cmd"); + cmdloop_timer_ = avoidance_node_cmd->create_wall_timer(cmdloop_dt_, [&](){}); cmdloop_executor_.add_node(avoidance_node_cmd); - // Status loop executor - auto status_loop_callback = [&]() { publishSystemStatus(); }; - auto avoidance_node_status = rclcpp::Node::make_shared("avoidance_node_status"); + // Start cmdloop thread + cmdloop_thread = new std::thread([&]() { + cmdloop_executor_.spin(); + }); + + avoidance_node_status = rclcpp::Node::make_shared("avoidance_node_status"); // This is a passthrough that replaces the usage of Mavlink Heartbeats telemetry_status_pub_ = - avoidance_node_status->create_publisher("TelemetryStatus_PubSubTopic", 1); - statusloop_timer_ = avoidance_node_status->create_wall_timer(statusloop_dt_, status_loop_callback); + avoidance_node_status->create_publisher("/TelemetryStatus_PubSubTopic", 1); + statusloop_timer_ = avoidance_node_status->create_wall_timer(statusloop_dt_, [&](){ publishSystemStatus(); }); statusloop_executor_.add_node(avoidance_node_status); - auto statusloop_spin_executor = [&]() { + // Start statusloop thread + statusloop_thread = new std::thread([&]() { statusloop_executor_.spin(); - }; - - // Launch both executors - std::thread execution_thread(statusloop_spin_executor); - cmdloop_executor_.spin(); - execution_thread.join(); + }); } void AvoidanceNode::setSystemStatus(MAV_STATE state) { @@ -66,13 +64,14 @@ void AvoidanceNode::publishSystemStatus() { // Publish companion process status as telemetry_status msg auto status_msg = px4_msgs::msg::TelemetryStatus(); - status_msg.timestamp = std::chrono::time_point_cast(std::chrono::steady_clock::now()).time_since_epoch().count(); + // TODO : TelemetryStatus.msg is totally changed in px4_msgs. It should be reflected. + /*status_msg.timestamp = std::chrono::time_point_cast(std::chrono::steady_clock::now()).time_since_epoch().count(); status_msg.heartbeat_time = status_msg.timestamp; status_msg.remote_system_id = 1; status_msg.remote_component_id = px4_msgs::msg::TelemetryStatus::COMPONENT_ID_OBSTACLE_AVOIDANCE; status_msg.remote_type = px4_msgs::msg::TelemetryStatus::MAV_TYPE_ONBOARD_CONTROLLER; status_msg.remote_system_status = (int)(this->getSystemStatus()); - status_msg.type = px4_msgs::msg::TelemetryStatus::LINK_TYPE_WIRE; + status_msg.type = px4_msgs::msg::TelemetryStatus::LINK_TYPE_WIRE;*/ telemetry_status_pub_->publish(status_msg); } diff --git a/avoidance/src/common.cpp b/avoidance/src/common.cpp index f97ce79b8..76aa38eb6 100644 --- a/avoidance/src/common.cpp +++ b/avoidance/src/common.cpp @@ -253,6 +253,27 @@ float getPitchFromQuaternion(const Eigen::Quaternionf q) { return pitch * RAD_TO_DEG; } +geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw) { + tf2::Quaternion quat_tf; + quat_tf.setRPY( 0, 0, yaw ); + geometry_msgs::msg::Quaternion quat_msg; + quat_msg = tf2::toMsg(quat_tf); + return quat_msg; +} + +geometry_msgs::msg::PoseStamped transformNEDandENU(geometry_msgs::msg::PoseStamped pose) { + geometry_msgs::msg::TransformStamped transformStamped; + tf2::Quaternion tf2_q_NED; + tf2_q_NED.setRPY(3.14, 0, 1.57); + geometry_msgs::msg::Quaternion geomsg_q; + tf2::convert(tf2_q_NED, geomsg_q); + transformStamped.transform.rotation = geomsg_q; + + geometry_msgs::msg::PoseStamped transformed_pose; + tf2::doTransform(pose, transformed_pose, transformStamped); + return transformed_pose; +} + float wrapAngleToPlusMinusPI(float angle) { return angle - 2.0f * M_PI_F * std::floor(angle / (2.0f * M_PI_F) + 0.5f); } float wrapAngleToPlusMinus180(float angle) { return angle - 360.f * std::floor(angle / 360.f + 0.5f); } @@ -304,7 +325,7 @@ void transformToTrajectory(px4_msgs::msg::VehicleTrajectoryWaypoint& obst_avoid, fillUnusedTrajectoryPoint(obst_avoid.waypoints[3]); fillUnusedTrajectoryPoint(obst_avoid.waypoints[4]); - for (size_t i = 0; i < sizeof(obst_avoid.waypoints); i++) { + for (size_t i = 0; i < obst_avoid.waypoints.size(); i++) { obst_avoid.waypoints[i].timestamp = obst_avoid.timestamp; obst_avoid.waypoints[i].point_valid = false; } @@ -424,4 +445,4 @@ void updateFOVFromMaxima(FOV& fov, const pcl::PointCloud& maxima) } } -} // namespace avoidance +} // namespace avoidance \ No newline at end of file diff --git a/avoidance/src/rviz_world_loader.cpp b/avoidance/src/rviz_world_loader.cpp index 352fa4f7d..e3cf54db7 100644 --- a/avoidance/src/rviz_world_loader.cpp +++ b/avoidance/src/rviz_world_loader.cpp @@ -9,12 +9,17 @@ WorldVisualizer::WorldVisualizer() : Node("world_visualizer"), world_path_(this->declare_parameter("world_path", "")) { - pose_sub_ = this->create_subscription( - "VehicleOdometry_PubSubTopic", 1, std::bind(&WorldVisualizer::positionCallback, this, _1)); - world_pub_ = this->create_publisher("/world", 1); drone_pub_ = this->create_publisher("/drone", 1); loop_timer_ = this->create_wall_timer(2s, std::bind(&WorldVisualizer::loopCallback, this)); + visualize_drone_timer_ = this->create_wall_timer(200ms, std::bind(&WorldVisualizer::visualizeDrone, this)); + + tf_buffer_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); this->get_parameter("world_path", world_path_); } @@ -26,15 +31,6 @@ void WorldVisualizer::loopCallback() { } } -void WorldVisualizer::positionCallback(const px4_msgs::msg::VehicleOdometry::SharedPtr msg) const { - // visualize drone in RVIZ - if (!world_path_.empty()) { - if (visualizeDrone(*msg)) { - RCLCPP_WARN(this->get_logger(), "Failed to visualize drone in RViz"); - } - } -} - int WorldVisualizer::resolveUri(std::string& uri) const { // Iterate through all locations in GAZEBO_MODEL_PATH char* gazebo_model_path = getenv("GAZEBO_MODEL_PATH"); @@ -132,37 +128,49 @@ int WorldVisualizer::visualizeRVIZWorld(const std::string& world_path) { return 0; } -int WorldVisualizer::visualizeDrone(const px4_msgs::msg::VehicleOdometry& pose) const { - auto drone = visualization_msgs::msg::Marker(); - drone.header.frame_id = "local_origin"; - drone.header.stamp = rclcpp::Clock().now(); - drone.type = visualization_msgs::msg::Marker::MESH_RESOURCE; - drone.mesh_resource = "model://matrice_100/meshes/Matrice_100.dae"; - if (drone.mesh_resource.find("model://") != std::string::npos) { - if (resolveUri(drone.mesh_resource)) { - RCLCPP_ERROR(this->get_logger(), "RVIZ world loader could not find drone model"); - return 1; +void WorldVisualizer::visualizeDrone(){ + tf_buffer_->waitForTransform("base_frame", "local_origin_odom", rclcpp::Clock().now(), std::chrono::milliseconds(500), + std::bind(&WorldVisualizer::tf2Callback, this, _1)); +} + +void WorldVisualizer::tf2Callback(const std::shared_future& tf) { + try { + geometry_msgs::msg::TransformStamped transformStamped = tf.get(); + + auto drone = visualization_msgs::msg::Marker(); + drone.header.frame_id = "base_frame"; + drone.header.stamp = rclcpp::Clock().now(); + drone.type = visualization_msgs::msg::Marker::MESH_RESOURCE; + drone.mesh_resource = "model://matrice_100/meshes/Matrice_100.dae"; + if (drone.mesh_resource.find("model://") != std::string::npos) { + if (resolveUri(drone.mesh_resource)) { + RCLCPP_ERROR(this->get_logger(), "RVIZ world loader could not find drone model"); + return; + } } + drone.mesh_use_embedded_materials = true; + drone.scale.x = 1.5; + drone.scale.y = 1.5; + drone.scale.z = 1.5; + drone.pose.position.x = transformStamped.transform.translation.x; + drone.pose.position.y = transformStamped.transform.translation.y; + drone.pose.position.z = transformStamped.transform.translation.z; + drone.pose.orientation.x = transformStamped.transform.rotation.x; + drone.pose.orientation.y = transformStamped.transform.rotation.y; + drone.pose.orientation.z = transformStamped.transform.rotation.z; + drone.pose.orientation.w = transformStamped.transform.rotation.w; + drone.color.r = 0.0f; + drone.color.g = 1.0f; + drone.color.b = 0.0f; + drone.color.a = 1.0; + drone.id = 0; + drone.lifetime = rclcpp::Duration(0); + drone.action = visualization_msgs::msg::Marker::ADD; + + drone_pub_->publish(drone); + } catch(tf2::TimeoutException const& ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); } - drone.mesh_use_embedded_materials = true; - drone.scale.x = 1.5; - drone.scale.y = 1.5; - drone.scale.z = 1.5; - //TODO: apply frame transforms? - drone.pose.position.x = pose.x; - drone.pose.position.y = pose.y; - drone.pose.position.z = pose.z; - drone.pose.orientation.x = pose.q[3]; - drone.pose.orientation.y = pose.q[0]; - drone.pose.orientation.z = pose.q[1]; - drone.pose.orientation.w = pose.q[2]; - drone.id = 0; - drone.lifetime = rclcpp::Duration(0); - drone.action = visualization_msgs::msg::Marker::ADD; - - drone_pub_->publish(drone); - - return 0; } // extraction operators diff --git a/avoidance/src/transform_buffer.cpp b/avoidance/src/transform_buffer.cpp index 2fdfa7b54..8b5f00fe3 100644 --- a/avoidance/src/transform_buffer.cpp +++ b/avoidance/src/transform_buffer.cpp @@ -6,7 +6,7 @@ namespace tf_buffer { TransformBuffer::TransformBuffer(float buffer_size_s) : buffer_size_(rclcpp::Duration(buffer_size_s)) { startup_time_ = rclcpp::Clock().now(); -}; +} std::string TransformBuffer::getKey(const std::string& source_frame, const std::string& target_frame) const { return source_frame + "_to_" + target_frame; @@ -26,9 +26,10 @@ bool TransformBuffer::interpolateTransform(const geometry_msgs::msg::TransformSt tf2::Vector3 tf_earlier_translation; tf2::Quaternion tf_earlier_rotation; tf2::Quaternion tf_later_rotation; - tf2::fromMsg(tf_earlier_translation, tf_earlier.transform.translation); - tf2::fromMsg(tf_earlier_rotation, tf_earlier.transform.rotation); - tf2::fromMsg(tf_later_rotation, tf_later.transform.rotation); + tf_earlier_translation.setValue(tf_earlier.transform.translation.x, tf_earlier.transform.translation.y, tf_earlier.transform.translation.z); + // tf2::fromMsg(tf_earlier.transform.translation, tf_earlier_translation); + tf2::fromMsg(tf_earlier.transform.rotation, tf_earlier_rotation); + tf2::fromMsg(tf_later.transform.rotation, tf_later_rotation); const tf2::Vector3 translation = tf_earlier_translation * (1.f - tau) + tf_earlier_translation * tau; const tf2::Quaternion rotation = tf_earlier_rotation.slerp(tf_later_rotation, tau); diff --git a/dependencies.rosinstall b/dependencies.rosinstall deleted file mode 100644 index bd6e1bedf..000000000 --- a/dependencies.rosinstall +++ /dev/null @@ -1 +0,0 @@ -- git: {local-name: yaml_cpp_catkin, uri: 'git@github.com:ethz-asl/yaml_cpp_catkin.git', version: 6cad108c4d5a9b9ddc95edd6f7cb23b572926351} \ No newline at end of file diff --git a/docs/gazebo_simple_obstacle_screenshot.png b/docs/gazebo_simple_obstacle_screenshot.png new file mode 100644 index 000000000..06b5679fe Binary files /dev/null and b/docs/gazebo_simple_obstacle_screenshot.png differ diff --git a/docs/lp_goal_height.png b/docs/lp_goal_height.png deleted file mode 100644 index 519201184..000000000 Binary files a/docs/lp_goal_height.png and /dev/null differ diff --git a/docs/lp_goal_rviz.png b/docs/lp_goal_rviz.png deleted file mode 100644 index 58fdcf466..000000000 Binary files a/docs/lp_goal_rviz.png and /dev/null differ diff --git a/docs/simulation_screenshot.png b/docs/simulation_screenshot.png index 082ee3b98..653e48428 100644 Binary files a/docs/simulation_screenshot.png and b/docs/simulation_screenshot.png differ diff --git a/global_planner/CMakeLists.txt b/global_planner/CMakeLists.txt index 8f2af83ea..426ecd07c 100644 --- a/global_planner/CMakeLists.txt +++ b/global_planner/CMakeLists.txt @@ -1,246 +1,136 @@ -cmake_minimum_required(VERSION 2.8.12) +cmake_minimum_required(VERSION 3.5) project(global_planner) -add_definitions(-std=c++11) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - dynamic_reconfigure - message_generation - tf - pcl_ros - mavlink - mavros_msgs - mavros - mavros_extras - avoidance -) -find_package(PCL 1.7 REQUIRED) +# Disable Wredundant-decls warnings since rosidl generates redundant function declarations +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++1z") +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_kdl REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(avoidance REQUIRED) find_package(octomap REQUIRED) +find_package(octomap_msgs REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/PathWithRiskMsg.msg" + DEPENDENCIES + std_msgs + geometry_msgs +) + +# find_package PCL interferes with message generation(https://github.com/ros2/rosidl/issues/402). It should be below of rosidl_generate_interfaces + +find_package(PCL 1.8 REQUIRED) + +set(DISABLE_SIMULATION OFF) if(DISABLE_SIMULATION) message(STATUS "Building avoidance without Gazebo Simulation") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDISABLE_SIMULATION") endif() -################################################ -## Gazebo Simulation -################################################ - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - PathWithRiskMsg.msg +include_directories( + include + ${avoidance_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${OCTOMAP_INCLUDE_DIRS} + ${octomap_msgs_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ${tf2_sensor_msgs_INCLUDE_DIRS} + ${tf2_geometry_msgs_INCLUDE_DIRS} + src/library + src/nodes) + +link_directories(${Boost_LIBRARY_DIRS}) + +set(GLOBAL_PLANNER_CPP_FILES + "src/library/cell.cpp" + "src/library/global_planner.cpp" + "src/library/node.cpp" + "src/nodes/global_planner_node.cpp" + "src/nodes/global_planner_node_main.cpp" ) -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) +add_executable(global_planner_node + ${GLOBAL_PLANNER_CPP_FILES}) -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -generate_dynamic_reconfigure_options( - cfg/GlobalPlannerNode.cfg +target_link_libraries(global_planner_node + ${Boost_LIBRARIES} + ${tf2_LIBRARIES} + ${PCL_LIBRARIES} + ${OCTOMAP_LIBRARIES} ) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs sensor_msgs message_runtime tf -# DEPENDS system_lib +ament_target_dependencies(global_planner_node + rclcpp + avoidance + px4_msgs + geometry_msgs + std_msgs + sensor_msgs + nav_msgs + octomap + octomap_msgs + visualization_msgs + tf2 + tf2_ros + tf2_kdl + tf2_sensor_msgs + tf2_geometry_msgs + PCL) + +rosidl_target_interfaces(global_planner_node ${PROJECT_NAME} "rosidl_typesupport_cpp") + +# Install header files +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" ) -########### -## Build ## -########### +# Install artifacts +install(TARGETS global_planner_node + DESTINATION lib/${PROJECT_NAME} +) -## CMake Setup -# Build in Release mode if nothing is specified -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif(NOT CMAKE_BUILD_TYPE) +# Install launch files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY + resource + DESTINATION share/${PROJECT_NAME}) -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${OCTOMAP_INCLUDE_DIRS} - ${YAML_CPP_INCLUDE_DIR} -) -link_libraries(${OCTOMAP_LIBRARIES}) - -## Declare a C++ library -add_library(global_planner - src/library/node.cpp - src/library/cell.cpp - src/library/global_planner.cpp - src/nodes/global_planner_node.cpp +install(TARGETS global_planner_node + RUNTIME DESTINATION bin ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(global_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_library(avoidance SHARED ${GLOBAL_PLANNER_CPP_FILES}) -## Declare a C++ executable -add_executable(global_planner_node src/nodes/global_planner_node_main.cpp) +ament_export_libraries(avoidance) -## Specify libraries to link a library or executable target against -target_link_libraries(global_planner_node - global_planner ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${YAML_CPP_LIBRARIES} -) +ament_package() -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS avoidance avoidance_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -# Add gtest based cpp test target and link libraries -if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp - test/test_example.cpp) - if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${YAML_CPP_LIBRARIES}) - endif() - - - if (${CMAKE_BUILD_TYPE} STREQUAL "Coverage") - SET(CMAKE_CXX_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage --coverage") - SET(CMAKE_C_FLAGS "-g -O0 -fprofile-arcs -ftest-coverage --coverage") - SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage") - - add_custom_target(${PROJECT_NAME}-test_coverage - COMMAND lcov --zerocounters --directory ${PROJECT_BINARY_DIR} - COMMAND lcov --capture --initial --no-external --directory ${PROJECT_BINARY_DIR} --base-directory ${${PROJECT_NAME}_SOURCE_DIR} --output-file base_coverage.info --rc lcov_branch_coverage=1 - COMMAND ${PROJECT_NAME}-test - COMMAND lcov --capture --no-external --directory ${PROJECT_BINARY_DIR} --base-directory ${${PROJECT_NAME}_SOURCE_DIR} --output-file test_coverage.info --rc lcov_branch_coverage=1 - COMMAND lcov -a base_coverage.info -a test_coverage.info -o coverage.info --rc lcov_branch_coverage=1 - COMMAND lcov --rc lcov_branch_coverage=1 --summary coverage.info - WORKING_DIRECTORY . - DEPENDS ${PROJECT_NAME}-test - ) - add_custom_target(${PROJECT_NAME}-test_coverage_html - COMMAND genhtml coverage.info --output-directory out --branch-coverage - COMMAND x-www-browser out/index.html - WORKING_DIRECTORY . - DEPENDS ${PROJECT_NAME}-test_coverage - ) - endif() -endif() -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/global_planner/COLCON_IGNORE b/global_planner/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/global_planner/cfg/GlobalPlannerNode.cfg b/global_planner/cfg/GlobalPlannerNode.cfg deleted file mode 100755 index 1d9c3a1a5..000000000 --- a/global_planner/cfg/GlobalPlannerNode.cfg +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "global_planner" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# global_planner -gen.add("min_altitude_", int_t, 0, "Minimum planned altitude", 1, 0, 10) -gen.add("max_altitude_", int_t, 0, "Maximum planned altitude", 10, 0, 50) -gen.add("max_cell_risk_", double_t, 0, "Maximum risk allowed per cells", 0.2, 0.0, 1.0) -gen.add("smooth_factor_", double_t, 0, "Cost of turning", 20.0, 0.0, 100.0) -gen.add("vert_to_hor_cost_", double_t, 0, "Cost of changing between horizontal and vertical direction", 3.0, 0.0, 10.0) -gen.add("risk_factor_", double_t, 0, "Cost of crashing", 500.0, 0.0, 1000.0) -gen.add("neighbor_risk_flow_", double_t, 0, "The effect of the risk of neighboring cells", 1.0, 0.0, 1.0) -gen.add("expore_penalty_", double_t, 0, "The cost of unexplored space", 0.005, 0.0, 0.01) -gen.add("up_cost_", double_t, 0, "Cost of ascending 1m", 5.0, 0.0, 10.0) -gen.add("down_cost_", double_t, 0, "Cost of descending 1m", 1.0, 0.0, 10.0) -gen.add("search_time_", double_t, 0, "Time it takes to return a new path", 0.5, 0.0, 1.0) -gen.add("min_overestimate_factor_", double_t, 0, "The minimum overestimation for heuristics", 1.03, 1.0, 1.5) -gen.add("max_overestimate_factor_", double_t, 0, "The minimum overestimation for heuristics", 2.0, 1.0, 5.0) -gen.add("max_iterations_", int_t, 0, "Maximum number of iterations", 2000, 0, 10000) -gen.add("goal_must_be_free_", bool_t, 0, "Don't bother trying to find a path if the exact goal is occupied", True) -gen.add("use_current_yaw_", bool_t, 0, "The current yaw affects the pathfinding", True) -gen.add("use_risk_heuristics_", bool_t, 0, "Use non underestimating heuristics for risk", True) -gen.add("use_speedup_heuristics_", bool_t, 0, "Use non underestimating heuristics for speedup", True) - -# global_planner_node -gen.add("clicked_goal_alt_", double_t, 0, "The altitude of clicked goals", 3.5, 0.0, 10.0) -gen.add("clicked_goal_radius_", double_t, 0, "Minimum allowed distance from path end to goal", 1.0, 0.0, 10.0) -gen.add("simplify_iterations_", int_t, 0, "Maximum number of iterations to simplify a path", 1, 0, 100) -gen.add("simplify_margin_", double_t, 0, "The allowed cost increase for simplifying an edge", 1.01, 0.0, 2.0) - -# cell -gen.add("CELL_SCALE", double_t, 2, "Size of a cell, should be divisable by the OctoMap resolution", 1.0, 0.5, 2.0) - -# node -gen.add("SPEEDNODE_RADIUS", double_t, 4, "Maximum length of edge between two Cells", 5.0, 0.0, 10.0) - -node_type_enum = gen.enum([ gen.const("Node", str_t, "Node", "Normal node"), - gen.const("NodeWithoutSmooth", str_t, "NodeWithoutSmooth", "No smooth cost"), - gen.const("SpeedNode", str_t, "SpeedNode", "Search with speed")], - "Change search mode") - -gen.add("default_node_type_", str_t, 4, "Change search mode", "SpeedNode", edit_method=node_type_enum) - -exit(gen.generate(PACKAGE, "global_planner", "GlobalPlannerNode")) diff --git a/global_planner/include/global_planner/analysis.h b/global_planner/include/global_planner/analysis.h index d4c903eb6..2292a30c6 100644 --- a/global_planner/include/global_planner/analysis.h +++ b/global_planner/include/global_planner/analysis.h @@ -92,7 +92,7 @@ void printPathStats(GlobalPlanner* global_planner, const std::vector& path double num_45_deg_turns = std::ceil(ang_diff3 / (M_PI / 4)); // Minimum number of 45-turns to goal printf("\t|| \t%3.2f \t%3.2f \t%3.2f \t%3.2f \t%3.2f \t%3.2f \n", u_ang, goal_ang, ang_diff, ang_diff2, ang_diff3, num_45_deg_turns); - ROS_INFO("WTF? \n %f %f \n\n\n\n\n\n\n\n\n\n\n\n", angleToRange(5.5), angleToRange(-5.5)); + // ROS_INFO("WTF? \n %f %f \n\n\n\n\n\n\n\n\n\n\n\n", angleToRange(5.5), angleToRange(-5.5)); } } printf("\n\n"); @@ -102,35 +102,35 @@ void printPathStats(GlobalPlanner* global_planner, const std::vector& path template void printPointStats(GlobalPlanner* global_planner, double x, double y, double z) { Cell cell(x, y, z); - ROS_INFO("\n\nDEBUG INFO FOR %s", cell.asString().c_str()); - ROS_INFO("Rist cost: %2.2f", global_planner->risk_factor_ * global_planner->getRisk(cell)); - ROS_INFO("getRisk: %2.2f", global_planner->getRisk(cell)); - ROS_INFO("singleCellRisk: %2.2f", global_planner->getSingleCellRisk(cell)); - ROS_INFO( - "Neighbors:\n \t %2.2f \t \t \t %2.2f \n %2.2f \t \t %2.2f \n \t %2.2f " - "\t \t \t %2.2f", - global_planner->getSingleCellRisk(Cell(x, y + 1, z)), global_planner->getSingleCellRisk(Cell(x, y, z + 1)), - global_planner->getSingleCellRisk(Cell(x - 1, y, z)), global_planner->getSingleCellRisk(Cell(x + 1, y, z)), - global_planner->getSingleCellRisk(Cell(x, y - 1, z)), global_planner->getSingleCellRisk(Cell(x, y, z - 1))); + // ROS_INFO("\n\nDEBUG INFO FOR %s", cell.asString().c_str()); + // ROS_INFO("Rist cost: %2.2f", global_planner->risk_factor_ * global_planner->getRisk(cell)); + // ROS_INFO("getRisk: %2.2f", global_planner->getRisk(cell)); + // ROS_INFO("singleCellRisk: %2.2f", global_planner->getSingleCellRisk(cell)); + // ROS_INFO( + // "Neighbors:\n \t %2.2f \t \t \t %2.2f \n %2.2f \t \t %2.2f \n \t %2.2f " + // "\t \t \t %2.2f", + // global_planner->getSingleCellRisk(Cell(x, y + 1, z)), global_planner->getSingleCellRisk(Cell(x, y, z + 1)), + // global_planner->getSingleCellRisk(Cell(x - 1, y, z)), global_planner->getSingleCellRisk(Cell(x + 1, y, z)), + // global_planner->getSingleCellRisk(Cell(x, y - 1, z)), global_planner->getSingleCellRisk(Cell(x, y, z - 1))); double heuristics = global_planner->getHeuristic(Node(cell, cell), global_planner->goal_pos_); - ROS_INFO("Heuristics: %2.2f", heuristics); + // ROS_INFO("Heuristics: %2.2f", heuristics); octomap::OcTreeNode* node = global_planner->octree_->search(x, y, z); if (node) { double prob = octomap::probability(node->getValue()); double post_prob = posterior(global_planner->getAltPrior(cell), prob); - ROS_INFO("prob: %2.2f \t post_prob: %2.2f", prob, post_prob); + // ROS_INFO("prob: %2.2f \t post_prob: %2.2f", prob, post_prob); if (global_planner->occupied_.find(cell) != global_planner->occupied_.end()) { - ROS_INFO("Cell in occupied, posterior: %2.2f", post_prob); + // ROS_INFO("Cell in occupied, posterior: %2.2f", post_prob); } else { - ROS_INFO("Cell NOT in occupied, posterior: %2.2f", global_planner->expore_penalty_ * post_prob); + // ROS_INFO("Cell NOT in occupied, posterior: %2.2f", global_planner->expore_penalty_ * post_prob); } } else { - ROS_INFO("Cell not in tree, prob: %2.2f", global_planner->expore_penalty_ * global_planner->getAltPrior(cell)); + // ROS_INFO("Cell not in tree, prob: %2.2f", global_planner->expore_penalty_ * global_planner->getAltPrior(cell)); } } } // namespace global_planner -#endif /* GLOBAL_PLANNER_ANALYSIS_H_ */ +#endif /* GLOBAL_PLANNER_ANALYSIS_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/bezier.h b/global_planner/include/global_planner/bezier.h index 1333530ac..265179e74 100644 --- a/global_planner/include/global_planner/bezier.h +++ b/global_planner/include/global_planner/bezier.h @@ -36,7 +36,7 @@ std::vector

threePointBezier(const P& p0, const P& p1, const P& p2, int num_s // Returns a quadratic Bezier-curve starting in p0 and and ending in p2 template -nav_msgs::Path threePointBezier(const Path& path, int num_steps = 10) { +nav_msgs::msg::Path threePointBezier(const Path& path, int num_steps = 10) { if (path.poses.size() != 3) { printf("Path size error, %d != 3 \n", static_cast(path.poses.size())); return path; @@ -119,14 +119,15 @@ double getAccelerationMagnitude(const P& p0, const P& p1, const P& p2, double du } template -nav_msgs::Path pathToTriplets(const nav_msgs::Path& path, std::vector triplets, std::vector speed) { +nav_msgs::msg::Path pathToTriplets(const nav_msgs::msg::Path& path, std::vector triplets, + std::vector speed) { if (path.poses.size() < 3) { return path; } // Extract the points from path, duplicate the first and last point to // indicate acceleration at the beginning and deceleration at the end - std::vector points; + std::vector points; points.push_back(path.poses.front().pose.position); for (auto pose : path.poses) { points.push_back(pose.pose.position); @@ -134,9 +135,9 @@ nav_msgs::Path pathToTriplets(const nav_msgs::Path& path, std::vector points.push_back(path.poses.back().pose.position); for (int i = 1; i < path.poses.size(); i++) { - geometry_msgs::Point prev = middlePoint(points[i - 1], points[i]); - geometry_msgs::Point ctrl = points[i]; - geometry_msgs::Point next = middlePoint(points[i], points[i + 1]); + geometry_msgs::msg::Point prev = middlePoint(points[i - 1], points[i]); + geometry_msgs::msg::Point ctrl = points[i]; + geometry_msgs::msg::Point next = middlePoint(points[i], points[i + 1]); ; BezierMsg msg; fillBezierMsg(msg, prev, ctrl, next, 1.0); @@ -144,4 +145,4 @@ nav_msgs::Path pathToTriplets(const nav_msgs::Path& path, std::vector } } // namespace global_planner -#endif /* GLOBAL_PLANNER_BEZIER_H_ */ +#endif /* GLOBAL_PLANNER_BEZIER_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/cell.h b/global_planner/include/global_planner/cell.h index 5eb30d38a..40557a930 100644 --- a/global_planner/include/global_planner/cell.h +++ b/global_planner/include/global_planner/cell.h @@ -5,7 +5,7 @@ #include #include -#include +#include #include "global_planner/common.h" @@ -19,7 +19,7 @@ class Cell { Cell(std::tuple new_tuple); Cell(double x, double y, double z); Cell(double x, double y); - Cell(geometry_msgs::Point point); + Cell(geometry_msgs::msg::Point point); // Cell(Eigen::Vector3d point); // Get the indices of the Cell @@ -32,7 +32,7 @@ class Cell { double yPos() const; double zPos() const; - geometry_msgs::Point toPoint() const; + geometry_msgs::msg::Point toPoint() const; double manhattanDist(double _x, double _y, double _z) const; double distance2D(const Cell& b) const; @@ -110,4 +110,4 @@ struct hash { } // namespace std -#endif // GLOBAL_PLANNER_CELL +#endif // GLOBAL_PLANNER_CELL \ No newline at end of file diff --git a/global_planner/include/global_planner/common.h b/global_planner/include/global_planner/common.h index 04ebe423c..df06b329f 100644 --- a/global_planner/include/global_planner/common.h +++ b/global_planner/include/global_planner/common.h @@ -1,8 +1,13 @@ #ifndef GLOBAL_PLANNER_COMMON_H_ #define GLOBAL_PLANNER_COMMON_H_ +#define M_PI 3.1415926535897932384626433832 +#define EARTH_MEAN_RADIUS 6371.0072 #include // sqrt +#include +#include #include +#include "rclcpp/rclcpp.hpp" namespace global_planner { @@ -111,6 +116,81 @@ inline double posterior(double p, double prior) { return prob_obstacle / (prob_obstacle + prob_free + 0.0001); } +inline double distanceTo(geographic_msgs::msg::GeoPoint pos_ref, geographic_msgs::msg::GeoPoint pos_to) { + // Haversine formula + double dlat = (pos_to.latitude - pos_ref.latitude) * M_PI / 180; + double dlon = (pos_to.longitude - pos_ref.longitude) * M_PI / 180; + double haversine_dlat = sin(dlat / 2.0); + haversine_dlat *= haversine_dlat; + double haversine_dlon = sin(dlon / 2.0); + haversine_dlon *= haversine_dlon; + double y = haversine_dlat + cos(pos_ref.latitude * M_PI / 180) * cos(pos_to.latitude * M_PI / 180) * haversine_dlon; + double x = 2 * asin(sqrt(y)); + return (x * EARTH_MEAN_RADIUS * 1000); +} + +inline geographic_msgs::msg::GeoPoint atDistanceAndAzimuth(geographic_msgs::msg::GeoPoint pos, double distance, + double azimuth) { + double latRad = pos.latitude * M_PI / 180; + double lonRad = pos.longitude * M_PI / 180; + double cosLatRad = cos(latRad); + double sinLatRad = sin(latRad); + + double azimuthRad = azimuth * M_PI / 180; + + double ratio = (distance / (EARTH_MEAN_RADIUS * 1000.0)); + double cosRatio = cos(ratio); + double sinRatio = sin(ratio); + + double resultLatRad = asin(sinLatRad * cosRatio + cosLatRad * sinRatio * cos(azimuthRad)); + double resultLonRad = + lonRad + atan2(sin(azimuthRad) * sinRatio * cosLatRad, cosRatio - sinLatRad * sin(resultLatRad)); + + pos.latitude = resultLatRad * 180 / M_PI; + pos.longitude = resultLonRad * 180 / M_PI; + return pos; +} + +// geometry_msgs::msg::Point start_pos_ +inline geometry_msgs::msg::Point LLH2NED(geographic_msgs::msg::GeoPoint pos_ref, + geographic_msgs::msg::GeoPoint pos_to) { + // Calc x,y,z of pos with refPos + geographic_msgs::msg::GeoPoint pos_calc_X, pos_calc_Y; + + pos_calc_X.latitude = pos_to.latitude; + pos_calc_X.longitude = pos_ref.longitude; + pos_calc_X.altitude = pos_ref.altitude; + + pos_calc_Y.latitude = pos_ref.latitude; + pos_calc_Y.longitude = pos_to.longitude; + pos_calc_Y.altitude = pos_ref.latitude; + + double NED_X = distanceTo(pos_ref, pos_calc_X); + if (pos_to.latitude < pos_ref.latitude) NED_X = -NED_X; + double NED_Y = distanceTo(pos_ref, pos_calc_Y); + if (pos_to.longitude < pos_ref.longitude) NED_Y = -NED_Y; + double NED_Z = -(pos_to.altitude - pos_ref.altitude); + + geometry_msgs::msg::Point result_point; + result_point.x = NED_X; + result_point.y = NED_Y; + result_point.z = NED_Z; + return result_point; +} + +inline geographic_msgs::msg::GeoPoint NED2LLH(geographic_msgs::msg::GeoPoint pos_ref, + geometry_msgs::msg::Point pos_to) { + geographic_msgs::msg::GeoPoint result_point; + // Calc lat, lon, alt of pos with refPos + + // QGeoCoordinate LLHPosition = QGeoCoordinate(refPos.latitude(), refPos.longitude(), refPos.altitude()); + result_point = atDistanceAndAzimuth(pos_ref, pos_to.x, 0); + result_point = atDistanceAndAzimuth(result_point, pos_to.y, 90); + result_point.altitude = result_point.altitude - pos_to.z; + + return result_point; +} + } // namespace global_planner -#endif /* GLOBAL_PLANNER_COMMON_H_ */ +#endif /* GLOBAL_PLANNER_COMMON_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/common_ros.h b/global_planner/include/global_planner/common_ros.h index b72d1aee2..58c497a12 100644 --- a/global_planner/include/global_planner/common_ros.h +++ b/global_planner/include/global_planner/common_ros.h @@ -3,12 +3,15 @@ #include -#include #include // sqrt -#include -#include -#include // getYaw createQuaternionMsgFromYaw -#include +#include +#include +#include // getYaw createQuaternionMsgFromYaw +#include +#include +#include +#include +#include #include "global_planner/common.h" // hasSameYawAndAltitude @@ -19,30 +22,33 @@ namespace global_planner { // GLOBAL PLANNER template -tf::Vector3 toTfVector3(const P& point) { - return tf::Vector3(point.x, point.y, point.z); +tf2::Vector3 toTfVector3(const P& point) { + return tf2::Vector3(point.x, point.y, point.z); } -inline double distance(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b) { +inline double distance(const geometry_msgs::msg::PoseStamped& a, const geometry_msgs::msg::PoseStamped& b) { return distance(a.pose.position, b.pose.position); } -inline geometry_msgs::TwistStamped transformTwistMsg(const tf::TransformListener& listener, - const std::string& target_frame, const std::string& fixed_frame, - const geometry_msgs::TwistStamped& msg) { +// https://github.com/trainman419/fiducials_ros/blob/master/src/fiducials_localization.cpp +inline geometry_msgs::msg::TwistStamped transformTwistMsg(const tf2_ros::TransformListener& listener, + const std::string& target_frame, + const std::string& fixed_frame, + const geometry_msgs::msg::TwistStamped& msg) { auto transformed_msg = msg; - geometry_msgs::Vector3Stamped before; + geometry_msgs::msg::Vector3Stamped before; before.vector = msg.twist.linear; before.header = msg.header; - geometry_msgs::Vector3Stamped after; - listener.transformVector(target_frame, ros::Time(0), before, fixed_frame, after); + geometry_msgs::msg::Vector3Stamped after; + // TODO :: apply tf2 transformVector + // listener.transformVector(target_frame, rclcpp::Time(0), before, fixed_frame, after); transformed_msg.twist.linear = after.vector; return transformed_msg; } // Returns a spectral color between red (0.0) and blue (1.0) -inline std_msgs::ColorRGBA spectralColor(double hue, double alpha = 1.0) { - std_msgs::ColorRGBA color; +inline std_msgs::msg::ColorRGBA spectralColor(double hue, double alpha = 1.0) { + std_msgs::msg::ColorRGBA color; color.r = std::max(0.0, 2 * hue - 1); color.g = 1.0 - 2.0 * std::abs(hue - 0.5); color.b = std::max(0.0, 1.0 - 2 * hue); @@ -51,15 +57,15 @@ inline std_msgs::ColorRGBA spectralColor(double hue, double alpha = 1.0) { } template -visualization_msgs::Marker createMarker(int id, Point position, Color color, double scale = 0.1, - std::string frame_id = "/world") { - visualization_msgs::Marker marker; +visualization_msgs::msg::Marker createMarker(int id, Point position, Color color, double scale = 0.1, + std::string frame_id = "/world") { + visualization_msgs::msg::Marker marker; marker.id = id; marker.header.frame_id = frame_id; - marker.header.stamp = ros::Time(); + marker.header.stamp = rclcpp::Time(); marker.pose.position = position; - marker.type = visualization_msgs::Marker::CUBE; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; marker.scale.x = marker.scale.y = marker.scale.z = scale; marker.color = color; return marker; @@ -73,12 +79,12 @@ visualization_msgs::Marker createMarker(int id, Point position, Color color, dou // } // Returns true if msg1 and msg2 have both the same altitude and orientation -inline bool hasSameYawAndAltitude(const geometry_msgs::Pose& msg1, const geometry_msgs::Pose& msg2) { +inline bool hasSameYawAndAltitude(const geometry_msgs::msg::Pose& msg1, const geometry_msgs::msg::Pose& msg2) { return msg1.orientation.z == msg2.orientation.z && msg1.orientation.w == msg2.orientation.w && msg1.position.z == msg2.position.z; } -inline double pathLength(const nav_msgs::Path& path) { +inline double pathLength(const nav_msgs::msg::Path& path) { double total_dist = 0.0; for (int i = 1; i < path.poses.size(); ++i) { total_dist += distance(path.poses[i - 1], path.poses[i]); @@ -87,8 +93,9 @@ inline double pathLength(const nav_msgs::Path& path) { } // Returns a path with only the corner points of msg -inline std::vector filterPathCorners(const std::vector& msg) { - std::vector corners = msg; +inline std::vector filterPathCorners( + const std::vector& msg) { + std::vector corners = msg; corners.clear(); if (msg.size() < 1) { return corners; @@ -97,9 +104,9 @@ inline std::vector filterPathCorners(const std::vect int n = msg.size(); corners.push_back(msg.front()); for (int i = 1; i < n - 1; ++i) { - geometry_msgs::Point last = msg[i - 1].pose.position; - geometry_msgs::Point curr = msg[i].pose.position; - geometry_msgs::Point next = msg[i + 1].pose.position; + geometry_msgs::msg::Point last = msg[i - 1].pose.position; + geometry_msgs::msg::Point curr = msg[i].pose.position; + geometry_msgs::msg::Point next = msg[i + 1].pose.position; bool same_x = (next.x - curr.x) == (curr.x - last.x); bool same_y = (next.y - curr.y) == (curr.y - last.y); bool same_z = (next.z - curr.z) == (curr.z - last.z); @@ -111,7 +118,7 @@ inline std::vector filterPathCorners(const std::vect return corners; } -inline double pathKineticEnergy(const nav_msgs::Path& path) { +inline double pathKineticEnergy(const nav_msgs::msg::Path& path) { if (path.poses.size() < 3) { return 0.0; } @@ -133,7 +140,7 @@ inline double pathKineticEnergy(const nav_msgs::Path& path) { return total_energy; } -inline double pathEnergy(const nav_msgs::Path& path, double up_penalty) { +inline double pathEnergy(const nav_msgs::msg::Path& path, double up_penalty) { double total_energy = 0.0; for (int i = 1; i < path.poses.size(); ++i) { total_energy += distance(path.poses[i - 1], path.poses[i]); @@ -145,4 +152,4 @@ inline double pathEnergy(const nav_msgs::Path& path, double up_penalty) { } // namespace global_planner -#endif /* GLOBAL_PLANNER_COMMON_ROS_H_ */ +#endif /* GLOBAL_PLANNER_COMMON_ROS_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/global_planner.h b/global_planner/include/global_planner/global_planner.h index 0059335c8..8d1a574d5 100644 --- a/global_planner/include/global_planner/global_planner.h +++ b/global_planner/include/global_planner/global_planner.h @@ -10,15 +10,17 @@ #include #include -#include -#include -#include // getYaw createQuaternionMsgFromYaw +#include +#include +#include +#include +#include #include #include -#include -#include +// #include +// #include #include "global_planner/analysis.h" #include "global_planner/cell.h" #include "global_planner/common.h" @@ -58,9 +60,9 @@ class GlobalPlanner { // TODO: rename and remove not needed std::vector path_back_; - geometry_msgs::Point curr_pos_; + geometry_msgs::msg::Point curr_pos_; double curr_yaw_; - geometry_msgs::Vector3 curr_vel_; + geometry_msgs::msg::Vector3 curr_vel_; GoalCell goal_pos_ = GoalCell(0.5, 0.5, 3.5); bool going_back_ = true; // we start by just finding the start position @@ -72,18 +74,21 @@ class GlobalPlanner { // Dynamic reconfigure parameters int min_altitude_ = 1; int max_altitude_ = 10; - double max_cell_risk_ = 0.2; + double max_cell_risk_ = 0.5; double smooth_factor_ = 10.0; double vert_to_hor_cost_ = 1.0; // The cost of changing between vertical and // horizontal motion (TODO: use it) double risk_factor_ = 500.0; double neighbor_risk_flow_ = 1.0; - double expore_penalty_ = 0.005; + double explore_penalty_ = 0.005; double up_cost_ = 3.0; double down_cost_ = 1.0; double search_time_ = 0.5; // The time it takes to find a path in worst case double min_overestimate_factor_ = 1.03; double max_overestimate_factor_ = 2.0; + double risk_threshold_risk_based_speedup_ = 0.5; + double default_speed_ = 1.0; // Default speed of flight. + double max_speed_ = 3.0; // Maximum speed of flight. int max_iterations_ = 2000; bool goal_is_blocked_ = false; bool current_cell_blocked_ = false; @@ -91,15 +96,18 @@ class GlobalPlanner { bool use_current_yaw_ = true; // The current orientation is factored into the smoothness bool use_risk_heuristics_ = true; bool use_speedup_heuristics_ = true; + bool use_risk_based_speedup_ = true; std::string default_node_type_ = "SpeedNode"; std::string frame_id_ = "world"; + std::string position_mode_ = "local_position"; + geographic_msgs::msg::GeoPoint ref_point_; GlobalPlanner(); ~GlobalPlanner(); void calculateAccumulatedHeightPrior(); - void setPose(const geometry_msgs::PoseStamped& new_pose); + void setPose(const geometry_msgs::msg::PoseStamped new_pose, const double yaw); void setGoal(const GoalCell& goal); void setPath(const std::vector& path); void setFrame(std::string frame_id); @@ -116,7 +124,7 @@ class GlobalPlanner { bool isLegal(const Node& node); double getRisk(const Cell& cell); double getRisk(const Node& node); - double getRiskOfCurve(const std::vector& msg); + double getRiskOfCurve(const std::vector& msg); double getTurnSmoothness(const Node& u, const Node& v); double getEdgeCost(const Node& u, const Node& v); @@ -126,10 +134,10 @@ class GlobalPlanner { double altitudeHeuristic(const Cell& u, const Cell& goal); double getHeuristic(const Node& u, const Cell& goal); - geometry_msgs::PoseStamped createPoseMsg(const Cell& cell, double yaw); - nav_msgs::Path getPathMsg(); - nav_msgs::Path getPathMsg(const std::vector& path); - PathWithRiskMsg getPathWithRiskMsg(); + geometry_msgs::msg::PoseStamped createPoseMsg(const Cell& cell, double yaw); + nav_msgs::msg::Path getPathMsg(); + nav_msgs::msg::Path getPathMsg(const std::vector& path); + // avoidance_msgs::msg::PathWithRiskMsg getPathWithRiskMsg(); PathInfo getPathInfo(const std::vector& path); NodePtr getStartNode(const Cell& start, const Cell& parent, const std::string& type); @@ -142,7 +150,7 @@ class GlobalPlanner { private: double robot_radius_; - double octree_resolution_; + double octree_resolution_ = 1.0; }; } // namespace global_planner diff --git a/global_planner/include/global_planner/global_planner_node.h b/global_planner/include/global_planner/global_planner_node.h index 6020f8afb..e1986712d 100644 --- a/global_planner/include/global_planner/global_planner_node.h +++ b/global_planner/include/global_planner/global_planner_node.h @@ -7,26 +7,37 @@ #include #include #include +#include "rclcpp/rclcpp.hpp" -#include -#include -#include -#include -#include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include -#include #include +#include #include "avoidance/avoidance_node.h" #include "global_planner/global_planner.h" @@ -37,82 +48,81 @@ namespace global_planner { +using std::placeholders::_1; +using namespace std::chrono_literals; + struct cameraData { - ros::Subscriber pointcloud_sub_; + rclcpp::Subscription::SharedPtr pointcloud_sub_; }; -class GlobalPlannerNode { +class GlobalPlannerNode : public rclcpp::Node { public: // TODO: Deque instead of vector GlobalPlanner global_planner_; std::vector waypoints_; // Intermediate goals, from file, mavros // mission or intermediate goals - GlobalPlannerNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); + GlobalPlannerNode(); ~GlobalPlannerNode(); private: std::mutex mutex_; - ros::NodeHandle nh_; - ros::NodeHandle nh_private_; - // Subscribers - ros::Subscriber octomap_sub_; - ros::Subscriber octomap_full_sub_; - ros::Subscriber ground_truth_sub_; - ros::Subscriber velocity_sub_; - ros::Subscriber clicked_point_sub_; - ros::Subscriber move_base_simple_sub_; - ros::Subscriber laser_sensor_sub_; - ros::Subscriber fcu_input_sub_; + rclcpp::Subscription::SharedPtr octomap_full_sub_; + rclcpp::Subscription::SharedPtr local_position_sub_; + rclcpp::Subscription::SharedPtr global_position_sub_; + rclcpp::Subscription::SharedPtr attitude_sub_; + rclcpp::Subscription::SharedPtr status_sub_; + rclcpp::Subscription::SharedPtr clicked_point_sub_; + rclcpp::Subscription::SharedPtr move_base_simple_sub_; // Publishers - ros::Publisher global_temp_path_pub_; - ros::Publisher smooth_path_pub_; - ros::Publisher actual_path_pub_; - ros::Publisher explored_cells_pub_; - ros::Publisher global_goal_pub_; - ros::Publisher global_temp_goal_pub_; - ros::Publisher mavros_obstacle_free_path_pub_; - ros::Publisher mavros_waypoint_publisher_; - ros::Publisher current_waypoint_publisher_; - ros::Publisher pointcloud_pub_; - - ros::Time start_time_; - ros::Time last_wp_time_; - - ros::Timer cmdloop_timer_; - ros::Timer plannerloop_timer_; - ros::CallbackQueue cmdloop_queue_; - ros::CallbackQueue plannerloop_queue_; - std::unique_ptr cmdloop_spinner_; - std::unique_ptr plannerloop_spinner_; - - tf::TransformListener listener_; - dynamic_reconfigure::Server server_; - - nav_msgs::Path actual_path_; - geometry_msgs::Point start_pos_; - geometry_msgs::PoseStamped current_goal_; - geometry_msgs::PoseStamped last_goal_; - geometry_msgs::PoseStamped last_pos_; - - std::vector last_clicked_points; - std::vector path_; + rclcpp::Publisher::SharedPtr global_temp_path_pub_; + rclcpp::Publisher::SharedPtr smooth_path_pub_; + rclcpp::Publisher::SharedPtr actual_path_pub_; + rclcpp::Publisher::SharedPtr explored_cells_pub_; + rclcpp::Publisher::SharedPtr global_goal_pub_; + rclcpp::Publisher::SharedPtr global_temp_goal_pub_; + rclcpp::Publisher::SharedPtr vehicle_command_pub_; + rclcpp::Publisher::SharedPtr current_waypoint_publisher_; + rclcpp::Publisher::SharedPtr pointcloud_pub_; + + rclcpp::Time start_time_; + rclcpp::Time last_wp_time_; + + rclcpp::TimerBase::SharedPtr gp_cmdloop_timer_; + rclcpp::TimerBase::SharedPtr gp_plannerloop_timer_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr transform_broadcaster_; + + nav_msgs::msg::Path actual_path_; + geometry_msgs::msg::Point start_pos_; + geometry_msgs::msg::PoseStamped current_goal_; + geometry_msgs::msg::PoseStamped last_goal_; + geometry_msgs::msg::PoseStamped last_pos_; + px4_msgs::msg::VehicleStatus last_vehicle_status_; + sensor_msgs::msg::PointCloud2 pointcloud2_; + + std::vector last_clicked_points; + std::vector path_; std::vector cameras_; int num_octomap_msg_ = 0; - int num_pos_msg_ = 0; - double cmdloop_dt_; - double plannerloop_dt_; + int num_local_pos_msg_ = 0; + int num_global_pos_msg_ = 0; + std::chrono::milliseconds gp_cmdloop_dt_; + std::chrono::milliseconds gp_plannerloop_dt_; double mapupdate_dt_; double min_speed_; double speed_ = min_speed_; double start_yaw_; bool position_received_; std::string frame_id_; - - // Dynamic Reconfiguration + std::string camera_frame_id_; + bool set_init_pos_ = false; + double clicked_goal_alt_; double clicked_goal_radius_; bool hover_; @@ -121,27 +131,28 @@ class GlobalPlannerNode { avoidance::AvoidanceNode avoidance_node_; #ifndef DISABLE_SIMULATION - std::unique_ptr world_visualizer_; + avoidance::WorldVisualizer::SharedPtr world_visualizer_; + rclcpp::executors::MultiThreadedExecutor world_visualizer_executor_; + rclcpp::TimerBase::SharedPtr world_visualizer_timer_; #endif void readParams(); void initializeCameraSubscribers(std::vector& camera_topics); - void receivePath(const nav_msgs::Path& msg); void setNewGoal(const GoalCell& goal); void popNextGoal(); void planPath(); void setIntermediateGoal(); bool isCloseToGoal(); - void setCurrentPath(const std::vector& poses); - void dynamicReconfigureCallback(global_planner::GlobalPlannerNodeConfig& config, uint32_t level); - void velocityCallback(const geometry_msgs::TwistStamped& msg); - void positionCallback(const geometry_msgs::PoseStamped& msg); - void clickedPointCallback(const geometry_msgs::PointStamped& msg); - void moveBaseSimpleCallback(const geometry_msgs::PoseStamped& msg); - void octomapFullCallback(const octomap_msgs::Octomap& msg); - void depthCameraCallback(const sensor_msgs::PointCloud2& msg); - void fcuInputGoalCallback(const mavros_msgs::Trajectory& msg); - void cmdLoopCallback(const ros::TimerEvent& event); - void plannerLoopCallback(const ros::TimerEvent& event); + void setCurrentPath(const std::vector& poses); + void localPositionCallback(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg); + void globalPositionCallback(const px4_msgs::msg::VehicleGlobalPosition::SharedPtr msg); + void vehicleStatusCallback(const px4_msgs::msg::VehicleStatus::SharedPtr msg); + void clickedPointCallback(const geometry_msgs::msg::PointStamped::SharedPtr msg); + void moveBaseSimpleCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void octomapFullCallback(const octomap_msgs::msg::Octomap::SharedPtr msg); + void depthCameraCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void attitudeCallback(const px4_msgs::msg::VehicleAttitude::SharedPtr msg); + void cmdLoopCallback(); + void plannerLoopCallback(); void publishGoal(const GoalCell& goal); void publishPath(); void publishSetpoint(); @@ -150,4 +161,4 @@ class GlobalPlannerNode { } // namespace global_planner -#endif // GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H +#endif // GLOBAL_PLANNER_GLOBAL_PLANNER_NODE_H \ No newline at end of file diff --git a/global_planner/include/global_planner/node.h b/global_planner/include/global_planner/node.h index 49c134fdd..e0e0c9871 100644 --- a/global_planner/include/global_planner/node.h +++ b/global_planner/include/global_planner/node.h @@ -118,4 +118,4 @@ struct hash { } // namespace std -#endif // GLOBAL_PLANNER_NODE +#endif // GLOBAL_PLANNER_NODE \ No newline at end of file diff --git a/global_planner/include/global_planner/search_tools.h b/global_planner/include/global_planner/search_tools.h index 65ac372e9..693667172 100644 --- a/global_planner/include/global_planner/search_tools.h +++ b/global_planner/include/global_planner/search_tools.h @@ -31,33 +31,33 @@ struct SearchInfo { inline void printSearchInfo(SearchInfo info, std::string node_type = "Node", double overestimate_factor = 1.0) { double avg_time = info.search_time / info.num_iter; - std::cout << std::setw(20) << std::left << node_type << std::setw(10) << std::setprecision(3) << avg_time - << std::setw(10) << std::setprecision(3) << overestimate_factor << std::setw(10) << info.num_iter - << std::setw(10) << 0.0; + // std::cout << std::setw(20) << std::left << node_type << std::setw(10) << std::setprecision(3) << avg_time + // << std::setw(10) << std::setprecision(3) << overestimate_factor << std::setw(10) << info.num_iter + // << std::setw(10) << 0.0; } // Returns a path where corners are smoothed with quadratic Bezier-curves -inline nav_msgs::Path smoothPath(const nav_msgs::Path& path) { +inline nav_msgs::msg::Path smoothPath(const nav_msgs::msg::Path& path) { if (path.poses.size() < 3) { return path; } - nav_msgs::Path smooth_path; + nav_msgs::msg::Path smooth_path; smooth_path.header = path.header; // Repeat the first and last points to get the first half of the first edge // and the second half of the last edge smooth_path.poses.push_back((path.poses.front())); for (int i = 2; i < path.poses.size(); i++) { - geometry_msgs::Point p0 = path.poses[i - 2].pose.position; - geometry_msgs::Point p1 = path.poses[i - 1].pose.position; - geometry_msgs::Point p2 = path.poses[i].pose.position; + geometry_msgs::msg::Point p0 = path.poses[i - 2].pose.position; + geometry_msgs::msg::Point p1 = path.poses[i - 1].pose.position; + geometry_msgs::msg::Point p2 = path.poses[i].pose.position; p0 = middlePoint(p0, p1); p2 = middlePoint(p1, p2); - std::vector smooth_turn = threePointBezier(p0, p1, p2); + std::vector smooth_turn = threePointBezier(p0, p1, p2); for (const auto& point : smooth_turn) { - geometry_msgs::PoseStamped pose_msg = path.poses.front(); // Copy the original header info + geometry_msgs::msg::PoseStamped pose_msg = path.poses.front(); // Copy the original header info pose_msg.pose.position = point; smooth_path.poses.push_back(pose_msg); } @@ -353,4 +353,4 @@ bool findPathOld(GlobalPlanner* global_planner, std::vector& path, const C } } // namespace global_planner -#endif /* GLOBAL_PLANNER_SEARCH_TOOLS_H_ */ +#endif /* GLOBAL_PLANNER_SEARCH_TOOLS_H_ */ \ No newline at end of file diff --git a/global_planner/include/global_planner/visitor.h b/global_planner/include/global_planner/visitor.h index dc386d65b..1c9a1ec0f 100644 --- a/global_planner/include/global_planner/visitor.h +++ b/global_planner/include/global_planner/visitor.h @@ -37,4 +37,4 @@ class NullVisitor { } // namespace global_planner -#endif // GLOBAL_PLANNER_VISITOR +#endif // GLOBAL_PLANNER_VISITOR \ No newline at end of file diff --git a/global_planner/launch/global_planner_depth-camera.launch b/global_planner/launch/global_planner_depth-camera.launch deleted file mode 100644 index 674a28331..000000000 --- a/global_planner/launch/global_planner_depth-camera.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - > - - - diff --git a/global_planner/launch/global_planner_depth_camera.launch.py b/global_planner/launch/global_planner_depth_camera.launch.py new file mode 100644 index 000000000..f2508f259 --- /dev/null +++ b/global_planner/launch/global_planner_depth_camera.launch.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python + +import os.path as osp + +from launch import LaunchDescription, logging +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import \ + PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import ThisLaunchFileDir + +def generate_launch_description(): + pkg_share_path = FindPackageShare('global_planner').find('global_planner') + pkg_share_path_avoidance = FindPackageShare('avoidance').find('avoidance') + + camera_frame_name = 'camera_frame' + + tf2_static_pub_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_depth_camera', + arguments=['0', '0', '0', + '-1.57', '0', '-1.57', + 'local_origin_odom', 'camera_frame'], + output='screen') + + tf2_static_pub_node2 = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_frd_ned', + arguments=['0', '0', '0', + '1.57', '0', '3.14', + 'base_frame', 'base_frame_ned'], + output='screen') + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', pkg_share_path + '/resource/global_planner.rviz']) + + gp_params = {'frame_id': 'base_frame', + 'position_mode': 'local_position', + 'world_path': pkg_share_path_avoidance + '/sim/worlds/simple_obstacle.yaml', + 'pointcloud_topics': ['/camera/points'], + 'start_pos_x': 0.0, + 'start_pos_y': 0.0, + 'start_pos_z': 3.0, + 'min_altitude': 1, + 'max_altitude': 5, + 'max_cell_risk': 0.5, + 'smooth_factor': 10.0, + 'vert_to_hor_cost': 1.0, + 'risk_factor': 500.0, + 'neighbor_risk_flow': 1.0, + 'explore_penalty': 0.005, + 'up_cost': 3.0, + 'down_cost': 1.0, + 'search_time': 0.5, + 'min_overestimate_factor': 1.03, + 'max_overestimate_factor': 2.0, + 'risk_threshold_risk_based_speedup': 0.5, + 'default_speed': 1.0, + 'max_speed': 2.0, + 'max_iterations': 1000, + 'goal_is_blocked': False, + 'current_cell_blocked': False, + 'goal_must_be_free': True, + 'use_current_yaw': True, + 'use_risk_heuristics': True, + 'use_speedup_heuristics': True, + 'use_risk_based_speedup': False} + + # Remapping rules for using other types of topic name instead of default PubSubTopic names + agent_id = 1 + gp_remap = [('/VehicleAttitude_PubSubTopic', '/agent{}/vehicle_attitude'.format(agent_id)), + ('/VehicleLocalPosition_PubSubTopic', '/agent{}/vehicle_local_position'.format(agent_id)), + ('/VehicleGlobalPosition_PubSubTopic','/agent{}/vehicle_global_position'.format(agent_id)), + ('/VehicleStatus_PubSubTopic', '/agent{}/vehicle_status'.format(agent_id)), + ('/VehicleCommand_PubSubTopic','/agent{}/vehicle_command'.format(agent_id))] + + gp_node = Node(package='global_planner', + executable='global_planner_node', + output='screen', +# remappings = gp_remap, + parameters=[gp_params]) + + octomap_params = {'resolution': 1.0, + 'frame_id': 'base_frame', + 'base_frame_id': 'base_frame', + 'height_map': True, + 'colored_map': False, + 'color_factor': 0.8, + 'filter_ground': True, + 'filter_speckles': False, + 'compress_map': True, + 'incremental_2D_projection': False, + 'sensor_model/max_range': 9.0, + 'sensor_model/hit': 0.9, + 'sensor_model/miss': 0.45, + 'sensor_model/min': 0.01, + 'sensor_model/max': 0.99, + 'pointcloud_max_x': 100.0, + 'pointcloud_max_y': 100.0, + 'pointcloud_max_z': 100.0, + 'pointcloud_min_x': -100.0, + 'pointcloud_min_y': -100.0, + 'pointcloud_min_z': -100.0, + 'occupancy_min_z': 0.0, + 'color/r': 0.0, + 'color/g': 0.0, + 'color/b': 1.0, + 'color/a': 1.0, + 'color_free/r': 0.0, + 'color_free/g': 0.0, + 'color_free/b': 1.0, + 'color_free/a': 1.0, + 'publish_free_space': False, + } + + octomap_node = Node(package='octomap_server2', + executable='octomap_server', + output='log', + # remappings=octomap_remap, + parameters=[octomap_params]) + return LaunchDescription([tf2_static_pub_node, tf2_static_pub_node2, octomap_node, gp_node, rviz2_node]) diff --git a/global_planner/launch/global_planner_depth_camera_real.launch.py b/global_planner/launch/global_planner_depth_camera_real.launch.py new file mode 100644 index 000000000..6673b1f2f --- /dev/null +++ b/global_planner/launch/global_planner_depth_camera_real.launch.py @@ -0,0 +1,188 @@ +#!/usr/bin/env python + +import os.path as osp + +from launch import LaunchDescription, logging +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import \ + PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import ThisLaunchFileDir +from launch.substitutions import LaunchConfiguration, PythonExpression + +def declare_configurable_parameters(parameters): + return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters] + +def set_configurable_parameters(parameters): + return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters]) + +def generate_launch_description(): + + camera_frame_name = 'camera_frame' # Realsense camera frame name + + configurable_parameters_realsense = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'}, + {'name': 'serial_no', 'default': '', 'description': 'choose device by serial number'}, + {'name': 'usb_port_id', 'default': '', 'description': 'choose device by usb port id'}, + {'name': 'device_type', 'default': '', 'description': 'choose device by type'}, + {'name': 'config_file', 'default': '', 'description': 'yaml config file'}, + {'name': 'enable_pointcloud', 'default': 'true', 'description': 'enable pointcloud'}, + {'name': 'unite_imu_method', 'default': '', 'description': '[copy|linear_interpolation]'}, + {'name': 'json_file_path', 'default': '', 'description': 'allows advanced configuration'}, + {'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'}, + {'name': 'depth_width', 'default': '256', 'description': 'depth image width'}, + {'name': 'depth_height', 'default': '144', 'description': 'depth image height'}, + {'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'color_width', 'default': '1280', 'description': 'color image width'}, + {'name': 'color_height', 'default': '720', 'description': 'color image height'}, + {'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'}, + {'name': 'infra_width', 'default': '640', 'description': 'infra width'}, + {'name': 'infra_height', 'default': '480', 'description': 'infra width'}, + {'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'}, + {'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'}, + {'name': 'infra_rgb', 'default': 'false', 'description': 'enable infra2 stream'}, + {'name': 'fisheye_width', 'default': '848', 'description': 'fisheye width'}, + {'name': 'fisheye_height', 'default': '800', 'description': 'fisheye width'}, + {'name': 'enable_fisheye1', 'default': 'false', 'description': 'enable fisheye1 stream'}, + {'name': 'enable_fisheye2', 'default': 'false', 'description': 'enable fisheye2 stream'}, + {'name': 'confidence_width', 'default': '640', 'description': 'depth image width'}, + {'name': 'confidence_height', 'default': '480', 'description': 'depth image height'}, + {'name': 'enable_confidence', 'default': 'true', 'description': 'enable depth stream'}, + {'name': 'fisheye_fps', 'default': '30.', 'description': ''}, + {'name': 'depth_fps', 'default': '90.', 'description': ''}, + {'name': 'confidence_fps', 'default': '30.', 'description': ''}, + {'name': 'infra_fps', 'default': '30.', 'description': ''}, + {'name': 'color_fps', 'default': '30.', 'description': ''}, + {'name': 'gyro_fps', 'default': '400.', 'description': ''}, + {'name': 'accel_fps', 'default': '250.', 'description': ''}, + {'name': 'enable_gyro', 'default': 'false', 'description': ''}, + {'name': 'enable_accel', 'default': 'false', 'description': ''}, + {'name': 'pointcloud_texture_stream', 'default': 'RS2_STREAM_COLOR', 'description': 'testure stream for pointcloud'}, + {'name': 'pointcloud_texture_index', 'default': '0', 'description': 'testure stream index for pointcloud'}, + {'name': 'enable_sync', 'default': 'false', 'description': ''}, + {'name': 'align_depth', 'default': 'false', 'description': ''}, + {'name': 'filters', 'default': '', 'description': ''}, + {'name': 'clip_distance', 'default': '-2.', 'description': ''}, + {'name': 'linear_accel_cov', 'default': '0.01', 'description': ''}, + {'name': 'initial_reset', 'default': 'false', 'description': ''}, + {'name': 'allow_no_texture_points', 'default': 'false', 'description': ''}, + {'name': 'calib_odom_file', 'default': '', 'description': ''}, + {'name': 'topic_odom_in', 'default': '', 'description': 'topic for T265 wheel odometry'}, + ] + + realsense2_node = LaunchDescription(declare_configurable_parameters(configurable_parameters_realsense) + [ + # Realsense + Node( + package='realsense2_camera', + namespace=LaunchConfiguration("camera_name"), + name=LaunchConfiguration("camera_name"), + executable='realsense2_camera_node', + parameters = [set_configurable_parameters(configurable_parameters_realsense)], + output='screen', + emulate_tty=True, + ) + ]) + + tf2_static_pub_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_depth_camera', + arguments=['0', '0', '0', + '-1.57', '0', '-1.57', + 'local_origin_odom', camera_frame_name], + output='screen') + + tf2_static_pub_node2 = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_frd_ned', + arguments=['0', '0', '0', + '1.57', '0', '3.14', + 'base_frame', 'base_frame_ned'], + output='screen') + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', '/home/karidrone/git/global_planner_ws/src/PX4-global-planner-ros2/global_planner/resources/global_planner.rviz']) + + gp_params = {'frame_id': 'base_frame', + 'agent_number': 14, + 'position_mode': 'local_position', + 'world_path': '/home/user/git/global_planner_ws/src/PX4-global-planner-ros2/avoidance/sim/worlds/simple_obstacle.yaml', + 'pointcloud_topics': ['/camera/depth/color/points'], + 'start_pos_x': 0.0, + 'start_pos_y': 0.0, + 'start_pos_z': 3.0, + 'min_altitude': 1, + 'max_altitude': 5, + 'max_cell_risk': 0.5, + 'smooth_factor': 10.0, + 'vert_to_hor_cost': 1.0, + 'risk_factor': 500.0, + 'neighbor_risk_flow': 1.0, + 'explore_penalty': 0.005, + 'up_cost': 3.0, + 'down_cost': 1.0, + 'search_time': 0.5, + 'min_overestimate_factor': 1.03, + 'max_overestimate_factor': 2.0, + 'risk_threshold_risk_based_speedup': 0.5, + 'default_speed': 1.0, + 'max_speed': 3.0, + 'max_iterations': 2000, + 'goal_is_blocked': False, + 'current_cell_blocked': False, + 'goal_must_be_free': True, + 'use_current_yaw': True, + 'use_risk_heuristics': True, + 'use_speedup_heuristics': True, + 'use_risk_based_speedup': True} + + gp_node = Node(package='global_planner', + executable='global_planner_node', + output='screen', + parameters=[gp_params]) + + octomap_params = {'resolution': 1.0, + 'frame_id': 'base_frame', + 'base_frame_id': 'base_footprint', + 'height_map': True, + 'colored_map': False, + 'color_factor': 0.8, + 'filter_ground': False, + 'filter_speckles': False, + 'compress_map': True, + 'incremental_2D_projection': False, + 'sensor_model/max_range': 6.0, + 'sensor_model/hit': 0.9, + 'sensor_model/miss': 0.45, + 'sensor_model/min': 0.01, + 'sensor_model/max': 0.99, + 'pointcloud_max_x': 100.0, + 'pointcloud_max_y': 100.0, + 'pointcloud_max_z': 100.0, + 'pointcloud_min_x': -100.0, + 'pointcloud_min_y': -100.0, + 'pointcloud_min_z': -100.0, + 'occupancy_min_z': 0.5, + 'color/r': 0.0, + 'color/g': 0.0, + 'color/b': 1.0, + 'color/a': 1.0, + 'color_free/r': 0.0, + 'color_free/g': 0.0, + 'color_free/b': 1.0, + 'color_free/a': 1.0, + 'publish_free_space': False, + } + + # octomap_remap = [('cloud_in', '/camera/points')] + + octomap_node = Node(package='octomap_server2', + executable='octomap_server', + output='log', + # remappings=octomap_remap, + parameters=[octomap_params]) + return LaunchDescription([realsense2_node, tf2_static_pub_node, tf2_static_pub_node2, octomap_node, gp_node]) #, rviz2_node]) diff --git a/global_planner/launch/global_planner_depth_camera_rs2_light_node.launch.py b/global_planner/launch/global_planner_depth_camera_rs2_light_node.launch.py new file mode 100644 index 000000000..d601ce273 --- /dev/null +++ b/global_planner/launch/global_planner_depth_camera_rs2_light_node.launch.py @@ -0,0 +1,136 @@ +#!/usr/bin/env python + +import os.path as osp + +from launch import LaunchDescription, logging +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import \ + PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import ThisLaunchFileDir +from launch.substitutions import LaunchConfiguration, PythonExpression + +def declare_configurable_parameters(parameters): + return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters] + +def set_configurable_parameters(parameters): + return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters]) + +def generate_launch_description(): + + camera_frame_name = 'camera_frame' # Realsense camera frame name + + rs2_node_param = [{'frame_name': camera_frame_name}, + {'depth_width': 480}, + {'depth_height': 270}, + {'depth_fps': 5}] + + realsense2_node = Node(package='rs2_light_wrapper', + executable='rs2_light_node', + parameters = rs2_node_param, + output='screen', + ) + + tf2_static_pub_node = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_depth_camera', + arguments=['0', '0', '0', + '-1.57', '0', '-1.57', + 'local_origin_odom', camera_frame_name], + output='screen') + + tf2_static_pub_node2 = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_frd_ned', + arguments=['0', '0', '0', + '1.57', '0', '3.14', + 'base_frame', 'base_frame_ned'], + output='screen') + + gp_params = {'frame_id': 'base_frame', + 'position_mode': 'local_position', + 'pointcloud_topics': ['/rs2_pc'], + 'start_pos_x': 0.0, + 'start_pos_y': 0.0, + 'start_pos_z': 2.0, + 'min_altitude': 1, + 'max_altitude': 5, + 'max_cell_risk': 0.5, + 'smooth_factor': 10.0, + 'vert_to_hor_cost': 1.0, + 'risk_factor': 500.0, + 'neighbor_risk_flow': 1.0, + 'explore_penalty': 0.005, + 'up_cost': 3.0, + 'down_cost': 1.0, + 'search_time': 0.5, + 'min_overestimate_factor': 1.03, + 'max_overestimate_factor': 2.0, + 'risk_threshold_risk_based_speedup': 0.5, + 'default_speed': 1.0, + 'max_speed': 2.0, + 'max_iterations': 1000, + 'goal_is_blocked': False, + 'current_cell_blocked': False, + 'goal_must_be_free': True, + 'use_current_yaw': True, + 'use_risk_heuristics': True, + 'use_speedup_heuristics': True, + 'use_risk_based_speedup': False} + + # Remapping rules for using other types of topic name instead of default PubSubTopic names + agent_id = 14 + gp_remap = [('/VehicleAttitude_PubSubTopic', '/agent{}/vehicle_attitude'.format(agent_id)), + ('/VehicleLocalPosition_PubSubTopic', '/agent{}/vehicle_local_position'.format(agent_id)), + ('/VehicleGlobalPosition_PubSubTopic','/agent{}/vehicle_global_position'.format(agent_id)), + ('/VehicleStatus_PubSubTopic', '/agent{}/vehicle_status'.format(agent_id)), + ('/VehicleCommand_PubSubTopic','/agent{}/vehicle_command'.format(agent_id))] + + gp_node = Node(package='global_planner', + executable='global_planner_node', + output='screen', + remappings = gp_remap, + parameters=[gp_params]) + + octomap_params = {'resolution': 0.5, + 'frame_id': 'base_frame', + 'base_frame_id': 'base_frame', + 'height_map': True, + 'colored_map': False, + 'color_factor': 0.8, + 'filter_ground': True, + 'filter_speckles': False, + 'compress_map': True, + 'incremental_2D_projection': False, + 'sensor_model/min_range': 0.5, + 'sensor_model/max_range': 5.0, + 'sensor_model/hit': 0.7, + 'sensor_model/miss': 0.4, + 'sensor_model/min': 0.2, + 'sensor_model/max': 0.7, + 'pointcloud_max_x': 50.0, + 'pointcloud_max_y': 50.0, + 'pointcloud_max_z': 50.0, + 'pointcloud_min_x': -50.0, + 'pointcloud_min_y': -50.0, + 'pointcloud_min_z': -50.0, + 'occupancy_min_z': 0.3, + 'color/r': 0.0, + 'color/g': 0.0, + 'color/b': 1.0, + 'color/a': 1.0, + 'color_free/r': 0.0, + 'color_free/g': 0.0, + 'color_free/b': 1.0, + 'color_free/a': 1.0, + 'publish_free_space': False, + } + + octomap_node = Node(package='octomap_server2', + executable='octomap_server', + output='log', + parameters=[octomap_params]) + return LaunchDescription([realsense2_node, tf2_static_pub_node, tf2_static_pub_node2, octomap_node, gp_node]) #, rviz2_node]) diff --git a/global_planner/launch/global_planner_octomap.launch b/global_planner/launch/global_planner_octomap.launch deleted file mode 100644 index ad5893ede..000000000 --- a/global_planner/launch/global_planner_octomap.launch +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - $(arg pointcloud_topics) - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/global_planner/launch/global_planner_sitl_3cam.launch b/global_planner/launch/global_planner_sitl_3cam.launch deleted file mode 100644 index d6cbc2030..000000000 --- a/global_planner/launch/global_planner_sitl_3cam.launch +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/global_planner/launch/global_planner_stereo.launch b/global_planner/launch/global_planner_stereo.launch deleted file mode 100644 index 088bd1a52..000000000 --- a/global_planner/launch/global_planner_stereo.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/global_planner/msg/PathWithRiskMsg.msg b/global_planner/msg/PathWithRiskMsg.msg index 837aa4140..bd4c1296d 100644 --- a/global_planner/msg/PathWithRiskMsg.msg +++ b/global_planner/msg/PathWithRiskMsg.msg @@ -1,3 +1,3 @@ -Header header +std_msgs/Header header geometry_msgs/PoseStamped[] poses float64[] risks \ No newline at end of file diff --git a/global_planner/package.xml b/global_planner/package.xml index e292ce58b..7f9620801 100644 --- a/global_planner/package.xml +++ b/global_planner/package.xml @@ -1,79 +1,36 @@ - + + global_planner - 0.0.0 - Avoidance module doing global planning. - - - - - pixhawk - - - - - + 1.0.0 + PX4 Avoidance global planner ROS2 version + Dongoo Lee TODO + ament_cmake + rosidl_default_generators + + ros_environment + rclcpp + px4_msgs + std_msgs + geometry_msgs + sensor_msgs + avoidance + tf2 + tf2_ros + pcl + octomap + octomap_msgs + avoidance_msgs + visualization_msgs + + builtin_interfaces + rosidl_default_runtime + + rosidl_interface_packages - - - - - - - - - - - - - - - - - - - - - - - - catkin - - dynamic_reconfigure - message_generation - roscpp - rospy - std_msgs - geometry_msgs - sensor_msgs - octomap - octomap_msgs - tf - pcl_ros - mavros - mavros_extras - avoidance - - - dynamic_reconfigure - message_runtime - roscpp - rospy - std_msgs - geometry_msgs - sensor_msgs - octomap - octomap_msgs - tf - pcl_ros - mavros - mavros_extras - avoidance - - - + ament_cmake diff --git a/global_planner/resource/global_planner.rviz b/global_planner/resource/global_planner.rviz index 2eb0f8185..0cdb7c282 100644 --- a/global_planner/resource/global_planner.rviz +++ b/global_planner/resource/global_planner.rviz @@ -1,487 +1,215 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Path2 - Splitter Ratio: 0.5 - Tree Height: 571 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /world - Name: World - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /drone - Name: Drone - Namespaces: - "": true - Queue Size: 100 - Value: true - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 36 - Reference Frame: - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /path_actual - Name: path_actual - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /path_waypoint - Name: path_waypoint - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.100000001 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: false - Head Length: 0.300000012 - Head Radius: 0.100000001 - Name: Vehicle pose - Shaft Length: 1 - Shaft Radius: 0.0500000007 - Shape: Arrow - Topic: /mavros/local_position/pose - Unreliable: false - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /current_setpoint - Name: currenSetpoint - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /selected_marker - Name: Selected direction - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /extended_marker - Name: ExtendedMarker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /rejected_marker - Name: Detected object - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /blocked_marker - Name: Blocked direction - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /candidates_marker - Name: Possible direction - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /goal_position - Name: Goal - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Axes - Enabled: false - Length: 1 - Name: fcu - Radius: 0.100000001 - Reference Frame: fcu - Value: false - - Class: rviz/Axes - Enabled: true - Length: 1 - Name: local_origin - Radius: 0.100000001 - Reference Frame: local_origin - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Local pointcloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.100000001 - Style: Flat Squares - Topic: /local_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: true - Max Color: 255; 0; 255 - Max Intensity: 4096 - Min Color: 255; 0; 255 - Min Intensity: 0 - Name: Reprojected_points - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 5 - Size (m): 0.100000001 - Style: Points - Topic: /reprojected_points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /camera/rgb/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /complete_tree - Name: Tree - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /tree_path - Name: TreePath - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /initial_height - Name: InitialHeight - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /take_off_pose - Name: TakeOff - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /original_waypoint - Name: WpOriginal - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /smoothed_waypoint - Name: WpSmoothed - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: false - Marker Topic: /adapted_waypoint - Name: WpAdapted - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /closest_point - Name: Offtrack Closest Point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /deg60_point - Name: Offtrack degrees 60 Point - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /occupied_cells_vis_array - Name: MarkerArray - Namespaces: - map: true - Queue Size: 100 - Value: true - - Alpha: 0.699999988 - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: /projected_map - Unreliable: false - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Lines - Line Width: 0.0299999993 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: /actual_path - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Lines - Line Width: 0.0299999993 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: /global_temp_path - Unreliable: false - Value: true - - Alpha: 1 - Class: rviz/PointStamped - Color: 204; 41; 204 - Enabled: true - History Length: 1 - Name: PointStamped - Radius: 0.200000003 - Topic: /global_temp_goal - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 89; 120; 76 - Default Light: true - Fixed Frame: local_origin - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 17.7176418 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.0648926124 - Y: -0.0846519172 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.204795957 - Target Frame: fcu - Value: ThirdPersonFollower (rviz) - Yaw: 2.2342782 - Saved: - - Class: rviz/ThirdPersonFollower - Distance: 10 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.00999999978 - Pitch: 0.220398411 - Target Frame: camera_link - Value: ThirdPersonFollower (rviz) - Yaw: 4.69858599 - - Class: rviz/Orbit - Distance: 49.4510117 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 5.36913252 - Y: -1.03582704 - Z: -0.00413707877 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Orbit - Near Clip Distance: 0.00999999978 - Pitch: 1.56979632 - Target Frame: world - Value: Orbit (rviz) - Yaw: 3.12039614 -Window Geometry: - Displays: - collapsed: false - Height: 1416 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000004e4fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073020000087b000004fd00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a00000275000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002b5000002690000001600fffffffb0000000a0049006d00610067006500000002ce000000c000000000000000000000000100000132000004f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000004f20000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bf0000004afc0100000002fb0000000800540069006d00650100000000000009bf0000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000084f000004e400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 2495 - X: 65 - Y: 24 +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Path1 + - /Path2 + Splitter Ratio: 0.5 + Tree Height: 623 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /actual_path + Value: true + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /drone + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupied_cells_vis_array + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_temp_path + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /world + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 18.897754669189453 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4603973627090454 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.965398073196411 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002f8000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002f8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000354000002f800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 709 + Y: 66 diff --git a/global_planner/resource/parameter_file b/global_planner/resource/parameter_file deleted file mode 100644 index 966d3ad44..000000000 --- a/global_planner/resource/parameter_file +++ /dev/null @@ -1,7 +0,0 @@ -smooth_factor_ 10.0 -risk_factor_ 500.0 -up_cost_ 300.0 -explore_penalty 0.005 -max_cell_risk_ 0.2 -neighbor_risk_flow_ 1.0 -goal_alt_inc 0.0 diff --git a/global_planner/resource/px4_config.yaml b/global_planner/resource/px4_config.yaml deleted file mode 100644 index 330158b3b..000000000 --- a/global_planner/resource/px4_config.yaml +++ /dev/null @@ -1,177 +0,0 @@ -# Common configuration for PX4 autopilot -# -# node: -startup_px4_usb_quirk: true - -# --- system plugins --- - -# sys_status & sys_time connection options -conn: - heartbeat_rate: 1.0 # send hertbeat rate in Hertz - timeout: 10.0 # hertbeat timeout in seconds - timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) - system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) - -# sys_status -sys: - min_voltage: 10.0 # diagnostics min voltage - disable_diag: false # disable all sys_status diagnostics, except heartbeat - -# sys_time -time: - time_ref_source: "fcu" # time_reference source - timesync_avg_alpha: 0.6 # timesync averaging factor - -# --- mavros plugins (alphabetical order) --- - -# 3dr_radio -tdr_radio: - low_rssi: 40 # raw rssi lower level for diagnostics - -# actuator_control -# None - -# command -cmd: - use_comp_id_system_control: false # quirk for some old FCUs - -# dummy -# None - -# ftp -# None - -# global_position -global_position: - frame_id: "fcu" # pose and fix frame_id - rot_covariance: 99999.0 # covariance for attitude? - tf: - send: false # send TF? - frame_id: "local_origin" # TF frame_id - child_frame_id: "fcu_utm" # TF child_frame_id - -# imu_pub -imu: - frame_id: "fcu" - # need find actual values - linear_acceleration_stdev: 0.0003 - angular_velocity_stdev: !degrees 0.02 - orientation_stdev: 1.0 - magnetic_stdev: 0.0 - -# local_position -local_position: - frame_id: "local_origin" - tf: - send: true - frame_id: "local_origin" - child_frame_id: "fcu" - send_fcu: false - -# param -# None, used for FCU params - -# rc_io -# None - -# safety_area -safety_area: - p1: {x: 1.0, y: 1.0, z: 1.0} - p2: {x: -1.0, y: -1.0, z: -1.0} - -# setpoint_accel -setpoint_accel: - send_force: false - -# setpoint_attitude -setpoint_attitude: - reverse_throttle: false # allow reversed throttle - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "attitude" - rate_limit: 10.0 - -# setpoint_position -setpoint_position: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "setpoint" - rate_limit: 50.0 - -# setpoint_velocity -# None - -# vfr_hud -# None - -# waypoint -mission: - pull_after_gcs: true # update mission if gcs updates - -# --- mavros extras plugins (same order) --- - -# distance_sensor (PX4 only) -distance_sensor: - hrlv_ez4_pub: - id: 0 - frame_id: "hrlv_ez4_sonar" - orientation: ROLL_180 # RPY:{180.0, 0.0, 0.0} - field_of_view: 0.0 # XXX TODO - send_tf: true - sensor_position: {x: 0.0, y: 0.0, z: -0.1} - lidarlite_pub: - id: 1 - frame_id: "lidarlite_laser" - orientation: ROLL_180 - field_of_view: 0.0 # XXX TODO - send_tf: true - sensor_position: {x: 0.0, y: 0.0, z: -0.1} - sonar_1_sub: - subscriber: true - id: 2 - orientation: ROLL_180 - laser_1_sub: - subscriber: true - id: 3 - orientation: ROLL_180 - -## Currently available orientations: -# Check http://wiki.ros.org/mavros/Enumerations -## - -# image_pub -image: - frame_id: "px4flow" - -# mocap_pose_estimate -mocap: - # select mocap source - use_tf: true # ~mocap/tf - use_pose: false # ~mocap/pose - -# px4flow -px4flow: - frame_id: "px4flow" - ranger_fov: !degrees 0.0 # XXX TODO - ranger_min_range: 0.3 # meters - ranger_max_range: 5.0 # meters - -# vision_pose_estimate -vision_pose: - tf: - listen: false # enable tf listener (disable topic subscribers) - frame_id: "local_origin" - child_frame_id: "vision" - rate_limit: 10.0 - -# vision_speed_estimate -vision_speed: - listen_twist: false - -# vibration -vibration: - frame_id: "vibration" - -# vim:set ts=2 sw=2 et: diff --git a/global_planner/resource/random_goals b/global_planner/resource/random_goals deleted file mode 100644 index e69de29bb..000000000 diff --git a/global_planner/resource/sample_output_from_mock_data b/global_planner/resource/sample_output_from_mock_data deleted file mode 100644 index a33a6efe3..000000000 --- a/global_planner/resource/sample_output_from_mock_data +++ /dev/null @@ -1,13 +0,0 @@ -[ INFO] [1467203468.623922075]: No goal file given. -[ INFO] [1467203478.608638174]: ========== Set goal : (8,4,3) ========== -[ INFO] [1467203478.608707527]: Start planning path. -[ INFO] [1467203478.608757630]: OctoMap memory usage: 0.010 MB -[ INFO] [1467203478.608901955]: Planning a path from (0,2,1) to (8,4,3) -[ INFO] [1467203478.608933362]: currPos: 0.50,2.50,1.50 s: 0.50,2.50,1.50 -Average iteration time: 0.162512 ms -overEstimateFactor: 2.00, numIter: 43 path cost: 126.23 (cost: 126.23, dist: 30.83, risk: 75.22, smooth: 20.00) -Average iteration time: 0.132180 ms -overEstimateFactor: 1.25, numIter: 50 path cost: 126.23 (cost: 126.23, dist: 30.83, risk: 75.22, smooth: 20.00) -Average iteration time: 0.145964 ms -overEstimateFactor: 1.06, numIter: 55 path cost: 126.23 (cost: 126.23, dist: 30.83, risk: 75.22, smooth: 20.00) -(0.50, 2.50, 2.50) -> (0.50, 2.50, 3.50) -> (0.50, 2.50, 4.50) -> (0.50, 2.50, 5.50) -> (0.50, 2.50, 6.50) -> (0.50, 2.50, 7.50) -> (1.50, 3.50, 7.50) -> (2.50, 4.50, 7.50) -> (3.50, 4.50, 7.50) -> (4.50, 4.50, 7.50) -> (5.50, 4.50, 7.50) -> (6.50, 4.50, 7.50) -> (7.50, 4.50, 7.50) -> (8.50, 4.50, 7.50) -> (8.50, 4.50, 6.50) -> (8.50, 4.50, 5.50) -> (8.50, 4.50, 4.50) -> (8.50, 4.50, 3.50) -> diff --git a/global_planner/src/library/cell.cpp b/global_planner/src/library/cell.cpp index 80aeacb0b..7b1aea8f8 100644 --- a/global_planner/src/library/cell.cpp +++ b/global_planner/src/library/cell.cpp @@ -6,7 +6,7 @@ Cell::Cell() = default; Cell::Cell(std::tuple new_tuple) : tpl_(new_tuple) {} Cell::Cell(double x, double y, double z) : tpl_(floor(x / CELL_SCALE), floor(y / CELL_SCALE), floor(z / CELL_SCALE)) {} Cell::Cell(double x, double y) : Cell(x, y, 0.0) {} -Cell::Cell(geometry_msgs::Point point) : Cell(point.x, point.y, point.z) {} +Cell::Cell(geometry_msgs::msg::Point point) : Cell(point.x, point.y, point.z) {} int Cell::xIndex() const { return std::get<0>(tpl_); } int Cell::yIndex() const { return std::get<1>(tpl_); } @@ -16,8 +16,8 @@ double Cell::xPos() const { return CELL_SCALE * (xIndex() + 0.5); } double Cell::yPos() const { return CELL_SCALE * (yIndex() + 0.5); } double Cell::zPos() const { return CELL_SCALE * (zIndex() + 0.5); } -geometry_msgs::Point Cell::toPoint() const { - geometry_msgs::Point point; +geometry_msgs::msg::Point Cell::toPoint() const { + geometry_msgs::msg::Point point; point.x = xPos(); point.y = yPos(); point.z = zPos(); @@ -67,7 +67,9 @@ std::vector Cell::getFlowNeighbors(int radius) const { std::vector cells; auto ceilDistance = [](int radius, int x, int y) { auto sqr = [](int i) { return double(i * i); }; - return static_cast(std::ceil(std::sqrt(sqr(radius) - sqr(x) - sqr(y)))); + auto sq_value = sqr(radius) - sqr(x) - sqr(y); + if (sq_value > 0) return static_cast(std::ceil(std::sqrt(sq_value))); + return 0; }; for (int x = -radius; x <= radius; x++) { int y_radius = ceilDistance(radius, x, 0); @@ -109,4 +111,4 @@ std::string Cell::asString() const { return s; } -} // namespace global_planner +} // namespace global_planner \ No newline at end of file diff --git a/global_planner/src/library/global_planner.cpp b/global_planner/src/library/global_planner.cpp index 5f65e4fd8..f83eaa2c7 100644 --- a/global_planner/src/library/global_planner.cpp +++ b/global_planner/src/library/global_planner.cpp @@ -27,9 +27,11 @@ void GlobalPlanner::calculateAccumulatedHeightPrior() { } // Updates the current pose and keeps track of the path back -void GlobalPlanner::setPose(const geometry_msgs::PoseStamped& new_pose) { +void GlobalPlanner::setPose(const geometry_msgs::msg::PoseStamped new_pose, const double yaw) { curr_pos_ = new_pose.pose.position; - curr_yaw_ = tf::getYaw(new_pose.pose.orientation); + // curr_yaw_ = tf2::getYaw(new_pose->pose.orientation); + curr_yaw_ = yaw; // get Yaw directly from px4_msgs::msg::VehicleLocalPosition. + Cell curr_cell = Cell(curr_pos_); if (!going_back_ && (path_back_.empty() || curr_cell != path_back_.back())) { // Keep track of where we have been, add current position to path_back_ if @@ -154,10 +156,10 @@ double GlobalPlanner::getSingleCellRisk(const Cell& cell) { return post_prob; } // No obstacle spotted (all measurements hint towards it being free) - return expore_penalty_ * post_prob; + return explore_penalty_ * post_prob; } // No measurements at all - return expore_penalty_ * getAltPrior(cell); // Risk for unexplored cells + return explore_penalty_ * getAltPrior(cell); // Risk for unexplored cells } double GlobalPlanner::getAltPrior(const Cell& cell) { @@ -182,12 +184,18 @@ double GlobalPlanner::getRisk(const Cell& cell) { if (risk_cache_.find(cell) != risk_cache_.end()) { return risk_cache_[cell]; } - double risk = getSingleCellRisk(cell); int radius = static_cast(std::ceil(robot_radius_ / octree_resolution_)); + // printf("cell (%.2lf %.2lf %.2lf) robot_radius_ : %d\n", cell.xPos(), cell.yPos(), cell.zPos(), robot_radius_); + int risk_cell_count = 1; for (const Cell& neighbor : cell.getFlowNeighbors(radius)) { + // printf("neighbor (%.2lf %.2lf %.2lf) risk : %.3lf\n", neighbor.xPos(), neighbor.yPos(), neighbor.zPos(), + // getSingleCellRisk(neighbor)); + risk_cell_count++; risk += neighbor_risk_flow_ * getSingleCellRisk(neighbor); } + // printf("-------------------------------------------------------------------------\n"); + risk = risk / risk_cell_count; // get averaged risk risk_cache_[cell] = risk; return risk; @@ -204,9 +212,9 @@ double GlobalPlanner::getRisk(const Node& node) { // Returns the risk of the quadratic Bezier curve defined by poses // TODO: think about this -double GlobalPlanner::getRiskOfCurve(const std::vector& msg) { +double GlobalPlanner::getRiskOfCurve(const std::vector& msg) { if (msg.size() != 3) { - ROS_INFO("Bezier msg must have 3 points"); + // ROS_INFO("Bezier msg must have 3 points"); return -1; } @@ -252,7 +260,7 @@ double GlobalPlanner::riskHeuristic(const Cell& u, const Cell& goal) { if (u == goal) { return 0.0; } - double unexplored_risk = (1.0 + 6.0 * neighbor_risk_flow_) * expore_penalty_ * risk_factor_; + double unexplored_risk = (1.0 + 6.0 * neighbor_risk_flow_) * explore_penalty_ * risk_factor_; double xy_dist = u.diagDistance2D(goal) - 1.0; // XY distance excluding the goal cell double xy_risk = xy_dist * unexplored_risk * getAltPrior(u); double z_risk = @@ -271,7 +279,7 @@ double GlobalPlanner::riskHeuristicReverseCache(const Cell& u, const Cell& goal) return 0.0; } double dist_to_bubble = std::max(0.0, u.diagDistance3D(goal) - bubble_radius_); - double unexplored_risk = (1.0 + 6.0 * neighbor_risk_flow_) * expore_penalty_ * risk_factor_; + double unexplored_risk = (1.0 + 6.0 * neighbor_risk_flow_) * explore_penalty_ * risk_factor_; double heuristic = bubble_cost_ + dist_to_bubble * unexplored_risk * getAltPrior(u); // bubble_risk_cache_[u] = heuristic; return heuristic; @@ -332,18 +340,29 @@ double GlobalPlanner::getHeuristic(const Node& u, const Cell& goal) { return heuristic; } -geometry_msgs::PoseStamped GlobalPlanner::createPoseMsg(const Cell& cell, double yaw) { - geometry_msgs::PoseStamped pose_msg; +geometry_msgs::msg::PoseStamped GlobalPlanner::createPoseMsg(const Cell& cell, double yaw) { + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.header.frame_id = frame_id_; pose_msg.pose.position = cell.toPoint(); - pose_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); + // pose_msg.pose.orientation = tf2::createQuaternionMsgFromYaw(yaw); // tf version + + // tf2 version + tf2::Quaternion quaternion; + quaternion.setRPY(0, 0, yaw); + geometry_msgs::msg::Quaternion qOri; + qOri.x = quaternion.x(); + qOri.y = quaternion.y(); + qOri.z = quaternion.z(); + qOri.w = quaternion.w(); + pose_msg.pose.orientation = qOri; + return pose_msg; } -nav_msgs::Path GlobalPlanner::getPathMsg() { return getPathMsg(curr_path_); } +nav_msgs::msg::Path GlobalPlanner::getPathMsg() { return getPathMsg(curr_path_); } -nav_msgs::Path GlobalPlanner::getPathMsg(const std::vector& path) { - nav_msgs::Path path_msg; +nav_msgs::msg::Path GlobalPlanner::getPathMsg(const std::vector& path) { + nav_msgs::msg::Path path_msg; path_msg.header.frame_id = frame_id_; if (path.size() == 0) { @@ -366,18 +385,18 @@ nav_msgs::Path GlobalPlanner::getPathMsg(const std::vector& path) { return path_msg; } -PathWithRiskMsg GlobalPlanner::getPathWithRiskMsg() { - nav_msgs::Path path_msg = getPathMsg(); - PathWithRiskMsg risk_msg; - risk_msg.header = path_msg.header; - risk_msg.poses = path_msg.poses; +// avoidance_msgs::msg::PathWithRiskMsg GlobalPlanner::getPathWithRiskMsg() { +// nav_msgs::msg::Path path_msg = getPathMsg(); +// avoidance_msgs::msg::PathWithRiskMsg risk_msg; +// risk_msg.header = path_msg.header; +// risk_msg.poses = path_msg.poses; - for (const auto& pose : path_msg.poses) { - double risk = getRisk(Cell(pose.pose.position)); - risk_msg.risks.push_back(risk); - } - return risk_msg; -} +// for (const auto& pose : path_msg.poses) { +// double risk = getRisk(Cell(pose.pose.position)); +// risk_msg.risks.push_back(risk); +// } +// return risk_msg; +// } // Returns details of the cost of the path PathInfo GlobalPlanner::getPathInfo(const std::vector& path) { @@ -421,9 +440,9 @@ bool GlobalPlanner::findPath(std::vector& path) { Cell parent_of_s = s; // Ignore the current yaw } - ROS_INFO("Planning a path from %s to %s", s.asString().c_str(), t.asString().c_str()); - ROS_INFO("curr_pos_: %2.2f,%2.2f,%2.2f\t s: %2.2f,%2.2f,%2.2f", curr_pos_.x, curr_pos_.y, curr_pos_.z, s.xPos(), - s.yPos(), s.zPos()); + // ROS_INFO("Planning a path from %s to %s", s.asString().c_str(), t.asString().c_str()); + // ROS_INFO("curr_pos_: %2.2f,%2.2f,%2.2f\t s: %2.2f,%2.2f,%2.2f", curr_pos_.x, curr_pos_.y, curr_pos_.z, s.xPos(), + // s.yPos(), s.zPos()); bool found_path = false; double best_path_cost = INFINITY; @@ -483,15 +502,18 @@ bool GlobalPlanner::getGlobalPath() { Cell s = Cell(curr_pos_); Cell t = Cell(goal_pos_); current_cell_blocked_ = isOccupied(s); - + // printf("getGlobalPath!! from (%.3lf %.3lf %.3lf) -> (%.3lf %.3lf %.3lf) // current_cell_blocked_ : %d\n", + // curr_pos_.x, curr_pos_.y, curr_pos_.z, + // goal_pos_.xPos(), goal_pos_.yPos(), goal_pos_.zPos(), current_cell_blocked_); if (goal_must_be_free_ && getRisk(t) > max_cell_risk_) { // If goal is occupied, no path is published - ROS_INFO("Goal position is occupied"); + // ROS_INFO("Goal position is occupied"); goal_is_blocked_ = true; + // printf("goal_is_blocked!! getRisk(t) : %lf, max_cell_risk_ : %lf\n", getRisk(t), max_cell_risk_); return false; } else if (current_cell_blocked_) { // If current position is occupied the way back is published - ROS_INFO("Current position is occupied, going back."); + // ROS_INFO("Current position is occupied, going back."); // goBack(); // return true; return false; @@ -500,7 +522,7 @@ bool GlobalPlanner::getGlobalPath() { std::vector path; if (!findPath(path)) { double goal_risk = getRisk(t); - ROS_INFO(" Failed to find a path, risk of t: %3.2f", goal_risk); + // printf(" Failed to find a path, risk of t: %3.2f", goal_risk); goal_is_blocked_ = true; return false; } @@ -512,7 +534,7 @@ bool GlobalPlanner::getGlobalPath() { // Sets the current path to be the path back until a safe cell is reached // Then the mission can be tried again or a new mission can be set void GlobalPlanner::goBack() { - ROS_INFO(" GO BACK "); + // ROS_INFO(" GO BACK "); going_back_ = true; std::vector new_path = path_back_; std::reverse(new_path.begin(), new_path.end()); @@ -536,4 +558,4 @@ void GlobalPlanner::stop() { void GlobalPlanner::setRobotRadius(double radius) { robot_radius_ = radius; } -} // namespace global_planner +} // namespace global_planner \ No newline at end of file diff --git a/global_planner/src/library/node.cpp b/global_planner/src/library/node.cpp index 6f631f038..f37bb6ad3 100644 --- a/global_planner/src/library/node.cpp +++ b/global_planner/src/library/node.cpp @@ -81,4 +81,4 @@ std::string Node::asString() const { return s; } -} // namespace global_planner +} // namespace global_planner \ No newline at end of file diff --git a/global_planner/src/nodes/global_planner_node.cpp b/global_planner/src/nodes/global_planner_node.cpp index d724362db..030714b3e 100644 --- a/global_planner/src/nodes/global_planner_node.cpp +++ b/global_planner/src/nodes/global_planner_node.cpp @@ -2,105 +2,151 @@ namespace global_planner { -GlobalPlannerNode::GlobalPlannerNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) - : nh_(nh), - nh_private_(nh_private), - avoidance_node_(nh, nh_private), - cmdloop_dt_(0.1), - plannerloop_dt_(1.0), - mapupdate_dt_(0.2), - start_yaw_(0.0) { - // Set up Dynamic Reconfigure Server - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&GlobalPlannerNode::dynamicReconfigureCallback, this, _1, _2); - server_.setCallback(f); +GlobalPlannerNode::GlobalPlannerNode() + : Node("global_planner_node"), gp_cmdloop_dt_(200ms), gp_plannerloop_dt_(1000ms), start_yaw_(0.0) { + RCLCPP_INFO_ONCE(this->get_logger(), "GlobalPlannerNode STARTED!"); #ifndef DISABLE_SIMULATION - world_visualizer_.reset(new avoidance::WorldVisualizer(nh_, ros::this_node::getName())); + world_visualizer_ = std::make_shared(); + world_visualizer_executor_.add_node(world_visualizer_); #endif - avoidance_node_.init(); // Read Ros parameters readParams(); // Subscribers - octomap_full_sub_ = nh_.subscribe("/octomap_full", 1, &GlobalPlannerNode::octomapFullCallback, this); - ground_truth_sub_ = nh_.subscribe("/mavros/local_position/pose", 1, &GlobalPlannerNode::positionCallback, this); - velocity_sub_ = nh_.subscribe("/mavros/local_position/velocity", 1, &GlobalPlannerNode::velocityCallback, this); - clicked_point_sub_ = nh_.subscribe("/clicked_point", 1, &GlobalPlannerNode::clickedPointCallback, this); - move_base_simple_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &GlobalPlannerNode::moveBaseSimpleCallback, this); - fcu_input_sub_ = nh_.subscribe("/mavros/trajectory/desired", 1, &GlobalPlannerNode::fcuInputGoalCallback, this); + rclcpp::QoS qos_default = rclcpp::SystemDefaultsQoS(); + rclcpp::QoS qos_best_effort = rclcpp::QoS(5).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); + + octomap_full_sub_ = this->create_subscription( + "/octomap_full", qos_best_effort, std::bind(&GlobalPlannerNode::octomapFullCallback, this, _1)); + attitude_sub_ = this->create_subscription( + "/VehicleAttitude_PubSubTopic", qos_best_effort, std::bind(&GlobalPlannerNode::attitudeCallback, this, _1)); + clicked_point_sub_ = this->create_subscription( + "/clicked_point", qos_default, std::bind(&GlobalPlannerNode::clickedPointCallback, this, _1)); + move_base_simple_sub_ = this->create_subscription( + "/goal_pose", qos_default, std::bind(&GlobalPlannerNode::moveBaseSimpleCallback, this, _1)); + local_position_sub_ = this->create_subscription( + "/VehicleLocalPosition_PubSubTopic", qos_best_effort, + std::bind(&GlobalPlannerNode::localPositionCallback, this, _1)); + global_position_sub_ = this->create_subscription( + "/VehicleGlobalPosition_PubSubTopic", qos_best_effort, + std::bind(&GlobalPlannerNode::globalPositionCallback, this, _1)); + status_sub_ = this->create_subscription( + "/VehicleStatus_PubSubTopic", qos_best_effort, + std::bind(&GlobalPlannerNode::vehicleStatusCallback, this, _1)); // Publishers - global_temp_path_pub_ = nh_.advertise("/global_temp_path", 10); - actual_path_pub_ = nh_.advertise("/actual_path", 10); - smooth_path_pub_ = nh_.advertise("/smooth_path", 10); - global_goal_pub_ = nh_.advertise("/global_goal", 10); - global_temp_goal_pub_ = nh_.advertise("/global_temp_goal", 10); - explored_cells_pub_ = nh_.advertise("/explored_cells", 10); - mavros_waypoint_publisher_ = nh_.advertise("/mavros/setpoint_position/local", 10); - mavros_obstacle_free_path_pub_ = nh_.advertise("/mavros/trajectory/generated", 10); - current_waypoint_publisher_ = nh_.advertise("/current_setpoint", 10); - pointcloud_pub_ = nh_.advertise("/cloud_in", 10); + global_temp_path_pub_ = this->create_publisher("/global_temp_path", 10); + actual_path_pub_ = this->create_publisher("/actual_path", 10); + smooth_path_pub_ = this->create_publisher("/smooth_path", 10); + global_goal_pub_ = this->create_publisher("/global_goal", 10); + global_temp_goal_pub_ = this->create_publisher("/global_temp_goal", 10); + explored_cells_pub_ = this->create_publisher("/explored_cells", 10); + vehicle_command_pub_ = this->create_publisher("/VehicleCommand_PubSubTopic", 10); + current_waypoint_publisher_ = this->create_publisher("/current_setpoint", 10); + pointcloud_pub_ = this->create_publisher("/cloud_in", 10); + + transform_broadcaster_ = std::make_shared(this); actual_path_.header.frame_id = frame_id_; - ros::TimerOptions cmdlooptimer_options(ros::Duration(cmdloop_dt_), - boost::bind(&GlobalPlannerNode::cmdLoopCallback, this, _1), &cmdloop_queue_); - cmdloop_timer_ = nh_.createTimer(cmdlooptimer_options); - - cmdloop_spinner_.reset(new ros::AsyncSpinner(1, &cmdloop_queue_)); - cmdloop_spinner_->start(); - - ros::TimerOptions plannerlooptimer_options(ros::Duration(plannerloop_dt_), - boost::bind(&GlobalPlannerNode::plannerLoopCallback, this, _1), - &plannerloop_queue_); - plannerloop_timer_ = nh_.createTimer(plannerlooptimer_options); - - plannerloop_spinner_.reset(new ros::AsyncSpinner(1, &plannerloop_queue_)); - plannerloop_spinner_->start(); + gp_cmdloop_timer_ = this->create_wall_timer(gp_cmdloop_dt_, [&]() { cmdLoopCallback(); }); + gp_plannerloop_timer_ = this->create_wall_timer(gp_plannerloop_dt_, [&]() { plannerLoopCallback(); }); current_goal_.header.frame_id = frame_id_; current_goal_.pose.position = start_pos_; - current_goal_.pose.orientation = tf::createQuaternionMsgFromYaw(start_yaw_); + current_goal_.pose.orientation = avoidance::createQuaternionMsgFromYaw(start_yaw_); last_goal_ = current_goal_; - speed_ = 5.0; + tf_buffer_ = std::make_shared(get_clock()); + auto timer_interface = + std::make_shared(this->get_node_base_interface(), this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); - start_time_ = ros::Time::now(); + start_time_ = rclcpp::Clock().now(); } GlobalPlannerNode::~GlobalPlannerNode() {} -// Read Ros parameters void GlobalPlannerNode::readParams() { std::vector camera_topics; - nh_.param("start_pos_x", start_pos_.x, 0.5); - nh_.param("start_pos_y", start_pos_.y, 0.5); - nh_.param("start_pos_z", start_pos_.z, 3.5); - nh_.param("frame_id", frame_id_, "/local_origin"); - nh_.getParam("pointcloud_topics", camera_topics); + frame_id_ = this->declare_parameter("frame_id", "/base_frame"); + start_pos_.x = this->declare_parameter("start_pos_x", 0.0); + start_pos_.y = this->declare_parameter("start_pos_y", 0.0); + start_pos_.z = this->declare_parameter("start_pos_z", 3.0); + global_planner_.min_altitude_ = this->declare_parameter("min_altitude", global_planner_.min_altitude_); + global_planner_.max_altitude_ = this->declare_parameter("max_altitude", global_planner_.max_altitude_); + global_planner_.max_cell_risk_ = this->declare_parameter("max_cell_risk", global_planner_.max_cell_risk_); + global_planner_.smooth_factor_ = this->declare_parameter("smooth_factor", global_planner_.smooth_factor_); + global_planner_.vert_to_hor_cost_ = this->declare_parameter("vert_to_hor_cost", global_planner_.vert_to_hor_cost_); + global_planner_.risk_factor_ = this->declare_parameter("risk_factor", global_planner_.risk_factor_); + global_planner_.neighbor_risk_flow_ = + this->declare_parameter("neighbor_risk_flow", global_planner_.neighbor_risk_flow_); + global_planner_.explore_penalty_ = this->declare_parameter("explore_penalty", global_planner_.explore_penalty_); + global_planner_.up_cost_ = this->declare_parameter("up_cost", global_planner_.up_cost_); + global_planner_.down_cost_ = this->declare_parameter("down_cost", global_planner_.down_cost_); + global_planner_.search_time_ = this->declare_parameter("search_time", global_planner_.search_time_); + global_planner_.min_overestimate_factor_ = + this->declare_parameter("min_overestimate_factor", global_planner_.min_overestimate_factor_); + global_planner_.max_overestimate_factor_ = + this->declare_parameter("max_overestimate_factor", global_planner_.max_overestimate_factor_); + global_planner_.risk_threshold_risk_based_speedup_ = + this->declare_parameter("risk_threshold_risk_based_speedup", global_planner_.risk_threshold_risk_based_speedup_); + global_planner_.default_speed_ = this->declare_parameter("default_speed", global_planner_.default_speed_); + global_planner_.max_speed_ = this->declare_parameter("max_speed", global_planner_.max_speed_); + global_planner_.max_iterations_ = this->declare_parameter("max_iterations", global_planner_.max_iterations_); + global_planner_.goal_is_blocked_ = this->declare_parameter("goal_is_blocked", global_planner_.goal_is_blocked_); + global_planner_.current_cell_blocked_ = + this->declare_parameter("current_cell_blocked", global_planner_.current_cell_blocked_); + global_planner_.goal_must_be_free_ = this->declare_parameter("goal_must_be_free", global_planner_.goal_must_be_free_); + global_planner_.use_current_yaw_ = this->declare_parameter("use_current_yaw", global_planner_.use_current_yaw_); + global_planner_.use_risk_heuristics_ = + this->declare_parameter("use_risk_heuristics", global_planner_.use_risk_heuristics_); + global_planner_.use_speedup_heuristics_ = + this->declare_parameter("use_speedup_heuristics", global_planner_.use_speedup_heuristics_); + global_planner_.use_risk_based_speedup_ = + this->declare_parameter("use_risk_based_speedup", global_planner_.use_risk_based_speedup_); + global_planner_.position_mode_ = this->declare_parameter("position_mode", global_planner_.position_mode_); + + camera_topics = this->declare_parameter("pointcloud_topics", camera_topics); + // camera_topics.push_back("/camera/points"); initializeCameraSubscribers(camera_topics); global_planner_.goal_pos_ = GoalCell(start_pos_.x, start_pos_.y, start_pos_.z); double robot_radius; - nh_.param("robot_radius", robot_radius, 0.5); + this->get_parameter_or("robot_radius", robot_radius, 1.0); global_planner_.setFrame(frame_id_); global_planner_.setRobotRadius(robot_radius); + + // Position for PX4 sitl default location + global_planner_.ref_point_.latitude = 47.3977508; + global_planner_.ref_point_.longitude = 8.5456073; + global_planner_.ref_point_.altitude = 488.10101318359375; + + global_planner_.ref_point_.latitude = + this->declare_parameter("ref_point_lat", global_planner_.ref_point_.latitude); + global_planner_.ref_point_.longitude = + this->declare_parameter("ref_point_long", global_planner_.ref_point_.longitude); + global_planner_.ref_point_.altitude = + this->declare_parameter("ref_point_alt", global_planner_.ref_point_.altitude); } void GlobalPlannerNode::initializeCameraSubscribers(std::vector& camera_topics) { cameras_.resize(camera_topics.size()); + rclcpp::QoS qos_best_effort = rclcpp::QoS(5).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); for (size_t i = 0; i < camera_topics.size(); i++) { - cameras_[i].pointcloud_sub_ = nh_.subscribe(camera_topics[i], 1, &GlobalPlannerNode::depthCameraCallback, this); + cameras_[i].pointcloud_sub_ = this->create_subscription( + camera_topics[i], qos_best_effort, std::bind(&GlobalPlannerNode::depthCameraCallback, this, _1)); } } // Sets a new goal, plans a path to it and publishes some info void GlobalPlannerNode::setNewGoal(const GoalCell& goal) { - ROS_INFO("========== Set goal : %s ==========", goal.asString().c_str()); + RCLCPP_INFO(this->get_logger(), "========== Set goal : %s ==========", goal.asString().c_str()); global_planner_.setGoal(goal); publishGoal(goal); } @@ -114,7 +160,7 @@ void GlobalPlannerNode::popNextGoal() { setNewGoal(new_goal); } else if (global_planner_.goal_is_blocked_) { // Goal is blocked but there is no other goal in waypoints_, just stop - ROS_INFO(" STOP "); + // RCLCPP_INFO(this->get_logger(), " STOP "); global_planner_.stop(); } } @@ -123,20 +169,21 @@ void GlobalPlannerNode::popNextGoal() { void GlobalPlannerNode::planPath() { std::clock_t start_time = std::clock(); if (global_planner_.octree_) { - ROS_INFO("OctoMap memory usage: %2.3f MB", global_planner_.octree_->memoryUsage() / 1000000.0); + RCLCPP_INFO(this->get_logger(), "OctoMap memory usage: %2.3f MB", global_planner_.octree_->memoryUsage() / + 1000000.0); } bool found_path = global_planner_.getGlobalPath(); if (!found_path) { // TODO: popNextGoal(), instead of checking if goal_is_blocked in - // positionCallback? - ROS_INFO("Failed to find a path"); + // localPositionCallback? + // RCLCPP_INFO(this->get_logger(), "Failed to find a path"); } else if (global_planner_.overestimate_factor_ > 1.05) { // The path is not good enough, set an intermediate goal on the path setIntermediateGoal(); } - printf("Total time: %2.2f ms \n", (std::clock() - start_time) / (double)(CLOCKS_PER_SEC / 1000)); + // printf("Total time: %2.2f ms \n", (std::clock() - start_time) / (double)(CLOCKS_PER_SEC / 1000)); } // Sets a temporary goal on the path to the current goal @@ -150,62 +197,131 @@ void GlobalPlannerNode::setIntermediateGoal() { } } -void GlobalPlannerNode::dynamicReconfigureCallback(global_planner::GlobalPlannerNodeConfig& config, uint32_t level) { - // global_planner_ - global_planner_.min_altitude_ = config.min_altitude_; - global_planner_.max_altitude_ = config.max_altitude_; - global_planner_.max_cell_risk_ = config.max_cell_risk_; - global_planner_.smooth_factor_ = config.smooth_factor_; - global_planner_.vert_to_hor_cost_ = config.vert_to_hor_cost_; - global_planner_.risk_factor_ = config.risk_factor_; - global_planner_.neighbor_risk_flow_ = config.neighbor_risk_flow_; - global_planner_.expore_penalty_ = config.expore_penalty_; - global_planner_.up_cost_ = config.up_cost_; - global_planner_.down_cost_ = config.down_cost_; - global_planner_.search_time_ = config.search_time_; - global_planner_.min_overestimate_factor_ = config.min_overestimate_factor_; - global_planner_.max_overestimate_factor_ = config.max_overestimate_factor_; - global_planner_.max_iterations_ = config.max_iterations_; - global_planner_.goal_must_be_free_ = config.goal_must_be_free_; - global_planner_.use_current_yaw_ = config.use_current_yaw_; - global_planner_.use_risk_heuristics_ = config.use_risk_heuristics_; - global_planner_.use_speedup_heuristics_ = config.use_speedup_heuristics_; - - // global_planner_node - clicked_goal_alt_ = config.clicked_goal_alt_; - clicked_goal_radius_ = config.clicked_goal_radius_; - simplify_iterations_ = config.simplify_iterations_; - simplify_margin_ = config.simplify_margin_; - - // cell - if (level == 2) { - CELL_SCALE = config.CELL_SCALE; - } +void GlobalPlannerNode::attitudeCallback(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) { + tf2::Quaternion q(msg->q[0], msg->q[1], msg->q[2], msg->q[3]); + double yaw, pitch, roll; + tf2::getEulerYPR(q, yaw, pitch, roll); + tf2::Quaternion q_NED; + q_NED.setRPY(yaw + 3.14, -pitch, -roll - 3.141592); + geometry_msgs::msg::Quaternion quat_geomsg; + tf2::convert(q_NED, quat_geomsg); + + geometry_msgs::msg::TransformStamped tfmsg; + tfmsg.header.stamp = rclcpp::Clock().now(); + tfmsg.header.frame_id = "local_origin"; + tfmsg.child_frame_id = "local_origin_odom"; + tfmsg.transform.rotation = quat_geomsg; + transform_broadcaster_->sendTransform(tfmsg); +} + +// Sets the current position and checks if the current goal has been reached +void GlobalPlannerNode::localPositionCallback(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) { + if (global_planner_.position_mode_.compare("local_position") == 0) { + geometry_msgs::msg::TransformStamped tfmsg; + tfmsg.header.stamp = rclcpp::Clock().now(); + tfmsg.header.frame_id = "base_frame_ned"; + tfmsg.child_frame_id = "local_origin"; + tfmsg.transform.translation.x = msg->x; + tfmsg.transform.translation.y = msg->y; + tfmsg.transform.translation.z = msg->z; + transform_broadcaster_->sendTransform(tfmsg); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = msg->x; + pose.pose.position.y = msg->y; + pose.pose.position.z = msg->z; + pose.pose.orientation = avoidance::createQuaternionMsgFromYaw(msg->yaw); + + geometry_msgs::msg::PoseStamped transformed_pose = avoidance::transformNEDandENU(pose); + + // Update position + last_pos_ = transformed_pose; + global_planner_.setPose(transformed_pose, msg->yaw); + + // Update velocity (considering NED to ENU transformation) + geometry_msgs::msg::Vector3 vel; + vel.x = msg->vy; + vel.y = msg->vx; + vel.z = -(msg->vz); + global_planner_.curr_vel_ = vel; + + // Check if a new goal is needed + if (num_local_pos_msg_++ % 10 == 0) { + last_pos_.header.frame_id = frame_id_; + actual_path_.poses.push_back(last_pos_); + actual_path_pub_->publish(actual_path_); + } - // node - if (level == 4) { - SPEEDNODE_RADIUS = config.SPEEDNODE_RADIUS; - global_planner_.default_node_type_ = config.default_node_type_; + position_received_ = true; + + // Check if we are close enough to current goal to get the next part of the + // path + if (path_.size() > 0 && isCloseToGoal()) { + double yaw1 = tf2::getYaw(current_goal_.pose.orientation); + double yaw2 = tf2::getYaw(last_pos_.pose.orientation); + double yaw_diff = std::abs(yaw2 - yaw1); + // Transform yaw_diff to [0, 2*pi] + yaw_diff -= std::floor(yaw_diff / (2 * M_PI)) * (2 * M_PI); + double max_yaw_diff = M_PI / 1.0; + if (yaw_diff < max_yaw_diff || yaw_diff > 2 * M_PI - max_yaw_diff) { + // If we are facing the right direction, then pop the first point of the + // path + last_goal_ = current_goal_; + current_goal_ = path_[0]; + path_.erase(path_.begin()); + } + } } -} -void GlobalPlannerNode::velocityCallback(const geometry_msgs::TwistStamped& msg) { - global_planner_.curr_vel_ = msg.twist.linear; + // Update velocity (considering NED to ENU transformation) + geometry_msgs::msg::Vector3 vel; + vel.x = msg->vy; + vel.y = msg->vx; + vel.z = -(msg->vz); + global_planner_.curr_vel_ = vel; } -// Sets the current position and checks if the current goal has been reached -void GlobalPlannerNode::positionCallback(const geometry_msgs::PoseStamped& msg) { +void GlobalPlannerNode::globalPositionCallback(const px4_msgs::msg::VehicleGlobalPosition::SharedPtr msg) { + geographic_msgs::msg::GeoPoint cur_point; + cur_point.latitude = msg->lat; + cur_point.longitude = msg->lon; + cur_point.altitude = msg->alt; + + if (set_init_pos_ == false) { + global_planner_.ref_point_.latitude = msg->lat; + global_planner_.ref_point_.longitude = msg->lon; + global_planner_.ref_point_.altitude = msg->alt; + set_init_pos_ = true; + } + + geometry_msgs::msg::Point local_pos = LLH2NED(global_planner_.ref_point_, cur_point); + + geometry_msgs::msg::TransformStamped tfmsg; + tfmsg.header.stamp = rclcpp::Clock().now(); + tfmsg.header.frame_id = "base_frame_ned"; + tfmsg.child_frame_id = "local_origin"; + tfmsg.transform.translation.x = local_pos.x; + tfmsg.transform.translation.y = local_pos.y; + tfmsg.transform.translation.z = local_pos.z; + transform_broadcaster_->sendTransform(tfmsg); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = local_pos.x; + pose.pose.position.y = local_pos.y; + pose.pose.position.z = local_pos.z; + pose.pose.orientation = avoidance::createQuaternionMsgFromYaw(0); + + geometry_msgs::msg::PoseStamped transformed_pose = avoidance::transformNEDandENU(pose); + // Update position - last_pos_ = msg; - global_planner_.setPose(last_pos_); + last_pos_ = transformed_pose; + global_planner_.setPose(transformed_pose, 0); // Check if a new goal is needed - if (num_pos_msg_++ % 10 == 0) { - // Keep track of and publish the actual travel trajectory - // ROS_INFO("Travelled path extended"); + if (num_global_pos_msg_++ % 10 == 0) { last_pos_.header.frame_id = frame_id_; actual_path_.poses.push_back(last_pos_); - actual_path_pub_.publish(actual_path_); + actual_path_pub_->publish(actual_path_); } position_received_ = true; @@ -213,9 +329,8 @@ void GlobalPlannerNode::positionCallback(const geometry_msgs::PoseStamped& msg) // Check if we are close enough to current goal to get the next part of the // path if (path_.size() > 0 && isCloseToGoal()) { - // TODO: get yawdiff(yaw1, yaw2) - double yaw1 = tf::getYaw(current_goal_.pose.orientation); - double yaw2 = tf::getYaw(last_pos_.pose.orientation); + double yaw1 = tf2::getYaw(current_goal_.pose.orientation); + double yaw2 = tf2::getYaw(last_pos_.pose.orientation); double yaw_diff = std::abs(yaw2 - yaw1); // Transform yaw_diff to [0, 2*pi] yaw_diff -= std::floor(yaw_diff / (2 * M_PI)) * (2 * M_PI); @@ -230,77 +345,56 @@ void GlobalPlannerNode::positionCallback(const geometry_msgs::PoseStamped& msg) } } -void GlobalPlannerNode::clickedPointCallback(const geometry_msgs::PointStamped& msg) { - printPointInfo(msg.point.x, msg.point.y, msg.point.z); +void GlobalPlannerNode::vehicleStatusCallback(const px4_msgs::msg::VehicleStatus::SharedPtr msg) { + last_vehicle_status_.timestamp = msg->timestamp; + last_vehicle_status_.nav_state = msg->nav_state; + last_vehicle_status_.nav_state_timestamp = msg->nav_state_timestamp; + last_vehicle_status_.system_id = msg->system_id; + last_vehicle_status_.component_id = msg->component_id; +} + +void GlobalPlannerNode::clickedPointCallback(const geometry_msgs::msg::PointStamped::SharedPtr msg) { + printPointInfo(msg->point.x, msg->point.y, msg->point.z); - geometry_msgs::PoseStamped pose; - pose.header = msg.header; - pose.pose.position = msg.point; + geometry_msgs::msg::PoseStamped pose; + pose.header = msg->header; + pose.pose.position = msg->point; pose.pose.position.z = global_planner_.curr_pos_.z; last_clicked_points.push_back(pose); } -void GlobalPlannerNode::moveBaseSimpleCallback(const geometry_msgs::PoseStamped& msg) { - setNewGoal(GoalCell(msg.pose.position.x, msg.pose.position.y, clicked_goal_alt_, clicked_goal_radius_)); -} - -void GlobalPlannerNode::fcuInputGoalCallback(const mavros_msgs::Trajectory& msg) { - const GoalCell new_goal = GoalCell(msg.point_2.position.x, msg.point_2.position.y, msg.point_2.position.z, 1.0); - if (msg.point_valid[1] == true && ((std::fabs(global_planner_.goal_pos_.xPos() - new_goal.xPos()) > 0.001) || - (std::fabs(global_planner_.goal_pos_.yPos() - new_goal.yPos()) > 0.001))) { - setNewGoal(new_goal); - } +void GlobalPlannerNode::moveBaseSimpleCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + setNewGoal(GoalCell(msg->pose.position.x, msg->pose.position.y, 3.0)); } -// Check if the current path is blocked -void GlobalPlannerNode::octomapFullCallback(const octomap_msgs::Octomap& msg) { +// // Check if the current path is blocked +void GlobalPlannerNode::octomapFullCallback(const octomap_msgs::msg::Octomap::SharedPtr msg) { std::lock_guard lock(mutex_); - ros::Time current = ros::Time::now(); + rclcpp::Time current = rclcpp::Clock().now(); // Update map at a fixed rate. This is useful on setting replanning rates for the planner. - if ((current - last_wp_time_).toSec() < mapupdate_dt_) { + if ((current - last_wp_time_).seconds() < mapupdate_dt_) { return; } - last_wp_time_ = ros::Time::now(); + last_wp_time_ = rclcpp::Clock().now(); - octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(msg); + octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(*msg); global_planner_.updateFullOctomap(tree); } // Go through obstacle points and store them -void GlobalPlannerNode::depthCameraCallback(const sensor_msgs::PointCloud2& msg) { - try { - // Transform msg from camera frame to world frame - ros::Time now = ros::Time::now(); - listener_.waitForTransform(frame_id_, "/camera_link", now, ros::Duration(5.0)); - tf::StampedTransform transform; - listener_.lookupTransform(frame_id_, "/camera_link", now, transform); - sensor_msgs::PointCloud2 transformed_msg; - pcl_ros::transformPointCloud(frame_id_, transform, msg, transformed_msg); - pcl::PointCloud cloud; // Easier to loop through pcl::PointCloud - pcl::fromROSMsg(transformed_msg, cloud); - - // Store the obstacle points - for (const auto& p : cloud) { - if (!std::isnan(p.x)) { - // TODO: Not all points end up here - Cell occupied_cell(p.x, p.y, p.z); - global_planner_.occupied_.insert(occupied_cell); - } - } - pointcloud_pub_.publish(msg); - } catch (tf::TransformException const& ex) { - ROS_DEBUG("%s", ex.what()); - ROS_WARN("Transformation not available (%s to /camera_link", frame_id_); - } +void GlobalPlannerNode::depthCameraCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + msg->header.stamp = rclcpp::Clock().now(); + msg->header.frame_id = "camera_frame"; + pointcloud_pub_->publish(*msg); } -void GlobalPlannerNode::setCurrentPath(const std::vector& poses) { +void GlobalPlannerNode::setCurrentPath(const std::vector& poses) { path_.clear(); if (poses.size() < 2) { - ROS_INFO(" Received empty path\n"); + // RCLCPP_INFO(this->get_logger(), " Received empty path\n"); return; } last_goal_ = poses[0]; @@ -311,20 +405,24 @@ void GlobalPlannerNode::setCurrentPath(const std::vector lock(mutex_); bool is_in_goal = global_planner_.goal_pos_.withinPositionRadius(global_planner_.curr_pos_); if (is_in_goal || global_planner_.goal_is_blocked_) { @@ -335,10 +433,10 @@ void GlobalPlannerNode::plannerLoopCallback(const ros::TimerEvent& event) { // Print and publish info if (is_in_goal && !waypoints_.empty()) { - ROS_INFO("Reached current goal %s, %d goals left\n\n", global_planner_.goal_pos_.asString().c_str(), - (int)waypoints_.size()); - ROS_INFO("Actual travel distance: %2.2f \t Actual energy usage: %2.2f", pathLength(actual_path_), - pathEnergy(actual_path_, global_planner_.up_cost_)); + RCLCPP_INFO(this->get_logger(), "Reached current goal %s, %d goals left\n\n", + global_planner_.goal_pos_.asString().c_str(), (int)waypoints_.size()); + RCLCPP_INFO(this->get_logger(), "Actual travel distance: %2.2f \t Actual energy usage: %2.2f", + pathLength(actual_path_), pathEnergy(actual_path_, global_planner_.up_cost_)); } publishPath(); @@ -346,31 +444,32 @@ void GlobalPlannerNode::plannerLoopCallback(const ros::TimerEvent& event) { // Publish the position of goal void GlobalPlannerNode::publishGoal(const GoalCell& goal) { - geometry_msgs::PointStamped pointMsg; + geometry_msgs::msg::PointStamped pointMsg; pointMsg.header.frame_id = frame_id_; pointMsg.point = goal.toPoint(); // Always publish as temporary to remove any obsolete temporary path - global_temp_goal_pub_.publish(pointMsg); + global_temp_goal_pub_->publish(pointMsg); if (!goal.is_temporary_) { - global_goal_pub_.publish(pointMsg); + global_goal_pub_->publish(pointMsg); } } // Publish the current path void GlobalPlannerNode::publishPath() { auto path_msg = global_planner_.getPathMsg(); - PathWithRiskMsg risk_msg = global_planner_.getPathWithRiskMsg(); + // PathWithRiskMsg risk_msg = global_planner_.getPathWithRiskMsg(); // Always publish as temporary to remove any obsolete temporary path - global_temp_path_pub_.publish(path_msg); + global_temp_path_pub_->publish(path_msg); setCurrentPath(path_msg.poses); - smooth_path_pub_.publish(smoothPath(path_msg)); - - auto simple_path = simplifyPath(&global_planner_, global_planner_.curr_path_, simplify_iterations_, simplify_margin_); - auto simple_path_msg = global_planner_.getPathMsg(simple_path); - global_temp_path_pub_.publish(simple_path_msg); - setCurrentPath(simple_path_msg.poses); - smooth_path_pub_.publish(smoothPath(simple_path_msg)); + smooth_path_pub_->publish(smoothPath(path_msg)); + + // auto simple_path = simplifyPath(&global_planner_, global_planner_.curr_path_, simplify_iterations_, + // simplify_margin_); + // auto simple_path_msg = global_planner_.getPathMsg(simple_path); + // global_temp_path_pub_->publish(simple_path_msg); + // setCurrentPath(simple_path_msg.poses); + // smooth_path_pub_->publish(smoothPath(simple_path_msg)); } // Prints information about the point, mostly the risk of the containing cell @@ -380,32 +479,59 @@ void GlobalPlannerNode::printPointInfo(double x, double y, double z) { } void GlobalPlannerNode::publishSetpoint() { - // Vector pointing from current position to the current goal - tf::Vector3 vec = toTfVector3(subtractPoints(current_goal_.pose.position, last_pos_.pose.position)); - // If we are less than 1.0 away, then we should stop at the goal - double new_len = vec.length() < 1.0 ? vec.length() : speed_; - vec.normalize(); - vec *= new_len; - - auto setpoint = current_goal_; // The intermediate position sent to Mavros - setpoint.pose.position.x = last_pos_.pose.position.x + vec.getX(); - setpoint.pose.position.y = last_pos_.pose.position.y + vec.getY(); - setpoint.pose.position.z = last_pos_.pose.position.z + vec.getZ(); - - // Publish setpoint for vizualization - current_waypoint_publisher_.publish(setpoint); - - // Publish setpoint to Mavros - mavros_waypoint_publisher_.publish(setpoint); - mavros_msgs::Trajectory obst_free_path = {}; - geometry_msgs::Twist velocity_setpoint{}; - velocity_setpoint.linear.x = NAN; - velocity_setpoint.linear.y = NAN; - velocity_setpoint.linear.z = NAN; - avoidance::transformToTrajectory(obst_free_path, setpoint, velocity_setpoint); - mavros_obstacle_free_path_pub_.publish(obst_free_path); + // For the safty reason, publishing setpoint should work only if navigation state is AUTO LOITER mode. + if (last_vehicle_status_.nav_state == px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_LOITER) { + // Vector pointing from current position to the current goal + tf2::Vector3 vec = toTfVector3(subtractPoints(current_goal_.pose.position, last_pos_.pose.position)); + + // If we are less than 1.0 away, then we should stop at the goal + if (vec.length() < 1.0) + return; + // double new_len = vec.length() < 1.0 ? vec.length() : global_planner_.default_speed_; + vec.normalize(); + vec *= global_planner_.default_speed_; + + // RCLCPP_INFO(this->get_logger(), "Current goal : %lf %lf %lf", + // current_goal_.pose.position.x, current_goal_.pose.position.y, current_goal_.pose.position.z); + // RCLCPP_INFO(this->get_logger(), "Last pos : %lf %lf %lf", + // last_pos_.pose.position.x, last_pos_.pose.position.y, last_pos_.pose.position.z); + // RCLCPP_INFO(this->get_logger(), "Vec : %lf %lf %lf", + // vec.x(), vec.y(), vec.z()); + + // To reduce noisy command, ignore small vector values of x,y,z. + if (std::abs(vec.x()) <= 0.1) + vec.setX(0); + if (std::abs(vec.y()) <= 0.1) + vec.setY(0); + if (std::abs(vec.z()) <= 0.1) + vec.setZ(0); + + auto setpoint = current_goal_; // The intermediate position sent to Mavros + setpoint.pose.position.x = last_pos_.pose.position.x + vec.getX(); + setpoint.pose.position.y = last_pos_.pose.position.y + vec.getY(); + setpoint.pose.position.z = last_pos_.pose.position.z + vec.getZ(); + + geometry_msgs::msg::PoseStamped NED_setpoint = avoidance::transformNEDandENU(setpoint); + // Publish setpoint for vizualization + // current_waypoint_publisher_->publish(setpoint); + geographic_msgs::msg::GeoPoint setpoint_geopoint = NED2LLH(global_planner_.ref_point_, NED_setpoint.pose.position); + auto reposition_cmd = px4_msgs::msg::VehicleCommand(); + reposition_cmd.target_system = last_vehicle_status_.system_id; + reposition_cmd.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_REPOSITION; + reposition_cmd.param1 = -1.0; + reposition_cmd.param2 = 1.0; + reposition_cmd.param3 = 0.0; + + double yaw = atan2(vec.getX(), vec.getY()); + reposition_cmd.param4 = yaw; // yaw + reposition_cmd.param5 = setpoint_geopoint.latitude; + reposition_cmd.param6 = setpoint_geopoint.longitude; + reposition_cmd.param7 = setpoint_geopoint.altitude; + reposition_cmd.from_external = true; + vehicle_command_pub_->publish(reposition_cmd); + } } -bool GlobalPlannerNode::isCloseToGoal() { return distance(current_goal_, last_pos_) < 1.5; } +bool GlobalPlannerNode::isCloseToGoal() { return distance(current_goal_, last_pos_) < global_planner_.default_speed_ * 1.5; } } // namespace global_planner diff --git a/global_planner/src/nodes/global_planner_node_main.cpp b/global_planner/src/nodes/global_planner_node_main.cpp index 5c892ae5d..5eb4dccbc 100644 --- a/global_planner/src/nodes/global_planner_node_main.cpp +++ b/global_planner/src/nodes/global_planner_node_main.cpp @@ -1,13 +1,9 @@ #include "global_planner/global_planner_node.h" +#include int main(int argc, char** argv) { - ros::init(argc, argv, "global_planner_node"); - - ros::NodeHandle nh("~"); - ros::NodeHandle nh_private(""); - - global_planner::GlobalPlannerNode global_planner_node(nh, nh_private); - - ros::spin(); + rclcpp::init(argc, argv); + auto global_planner_nd = std::make_shared(); + rclcpp::spin(global_planner_nd); return 0; } diff --git a/global_planner/src/nodes/mock_data_node.cpp b/global_planner/src/nodes/mock_data_node.cpp deleted file mode 100644 index d6a109641..000000000 --- a/global_planner/src/nodes/mock_data_node.cpp +++ /dev/null @@ -1,129 +0,0 @@ -#include "mock_data_node.h" - -namespace global_planner { - -MockDataNode::MockDataNode() { - ros::NodeHandle nh; - - path_sub_ = nh.subscribe("/global_path", 1, &MockDataNode::receivePath, this); - - depth_points_pub_ = nh.advertise("/camera/depth/points", 10); - local_position_pub_ = nh.advertise("/mavros/local_position/pose", 10); - global_goal_pub_ = nh.advertise("/clicked_point", 10); - - createWall(5, 5, 6); - - int num_loops = 0; - ros::Rate rate(1); - while (ros::ok()) { - if (num_loops++ == 10) { - sendClickedPoint(); - } - sendMockData(); - rate.sleep(); - ros::spinOnce(); - } -} - -MockDataNode::~MockDataNode() {} - -void MockDataNode::createWall(int dist, int width, int height) { - points_.clear(); - for (int i = -width; i <= width; ++i) { - for (int j = 0; j <= height; ++j) { - points_.push_back(dist + 0.5); - points_.push_back(i + 0.5); - points_.push_back(j + 0.5); - } - } -} - -void MockDataNode::sendClickedPoint() { - geometry_msgs::PointStamped msg; - msg.header.frame_id = "/world"; - msg.point.x = 8.5; - msg.point.y = 4.5; - msg.point.z = 1.5; - global_goal_pub_.publish(msg); -} - -void MockDataNode::receivePath(const nav_msgs::Path& msg) { - for (auto p : msg.poses) { - double x = p.pose.position.x; - double y = p.pose.position.y; - double z = p.pose.position.z; - printf("(%2.2f, %2.2f, %2.2f) -> ", x, y, z); - } - printf("\n\n"); -} - -void MockDataNode::sendMockData() { - // Create a PointCloud2 - sensor_msgs::PointCloud2 cloud_msg; - cloud_msg.header.frame_id = "/world"; - // Fill some internals of the PoinCloud2 like the header/width/height ... - cloud_msg.height = 1; - cloud_msg.width = 4; - // Set the point fields to xyzrgb and resize the vector with the following - // command 4 is for the number of added fields. Each come in triplet: the name - // of the PointField, the number of occurences of the type in the PointField, - // the type of the PointField - sensor_msgs::PointCloud2Modifier modifier(cloud_msg); - modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, "rgb", 1, sensor_msgs::PointField::FLOAT32); - // For convenience and the xyz, rgb, rgba fields, you can also use the - // following overloaded function. You have to be aware that the following - // function does add extra padding for backward compatibility though so it is - // definitely the solution of choice for PointXYZ and PointXYZRGB 2 is for the - // number of fields to add - modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); - // You can then reserve / resize as usual - modifier.resize(100); - - // Define some raw data we'll put in the PointCloud2 - int n = points_.size() / 3; - uint8_t color_data[] = {40, 200, 120}; - - // Define the iterators. When doing so, you define the Field you would like to - // iterate upon and the type of you would like returned: it is not necessary - // the type of the PointField as sometimes you pack data in another type (e.g. - // 3 uchar + 1 uchar for RGB are packed in a float) - sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); - // Even though the r,g,b,a fields do not exist (it's usually rgb, rgba), you - // can create iterators for those: they will handle data packing for you (in - // little endian RGB is packed as *,R,G,B in a float and RGBA as A,R,G,B) - sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); - sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); - sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); - // Fill the PointCloud2 - for (size_t i = 0; i < n; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) { - *iter_x = points_[3 * i + 0]; - *iter_y = points_[3 * i + 1]; - *iter_z = points_[3 * i + 2]; - *iter_r = color_data[0]; - *iter_g = color_data[1]; - *iter_b = color_data[2]; - } - - depth_points_pub_.publish(cloud_msg); - - // Send position - geometry_msgs::PoseStamped pos; - pos.header.frame_id = "/world"; - pos.pose.position.x = 0.5; - pos.pose.position.y = 2.5; - pos.pose.position.z = 1.5; - pos.pose.orientation.w = 1.0; - local_position_pub_.publish(pos); -} - -} // namespace global_planner - -int main(int argc, char** argv) { - ros::init(argc, argv, "mock_data_node"); - global_planner::MockDataNode mock_data_node; - ros::spin(); - return 0; -} diff --git a/global_planner/src/nodes/mock_data_node.h b/global_planner/src/nodes/mock_data_node.h deleted file mode 100644 index c5f8885df..000000000 --- a/global_planner/src/nodes/mock_data_node.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef GLOBAL_PLANNER_MOCK_DATA_NODE_H -#define GLOBAL_PLANNER_MOCK_DATA_NODE_H - -#include -#include -#include -#include - -#include -#include - -#include "global_planner/common.h" // hasSameYawAndAltitude - -namespace global_planner { - -class MockDataNode { - public: - MockDataNode(); - ~MockDataNode(); - void createWall(int dist, int width, int height); - void sendClickedPoint(); - void receivePath(const nav_msgs::Path& msg); - void sendMockData(); - - std::vector points_{5.5, -0.5, 0.5, 5.5, 0.5, 0.5, 5.5, 1.5, 0.5, 5.5, -0.5, 1.5, 5.5, 0.5, - 1.5, 5.5, 1.5, 1.5, 5.5, -0.5, 2.5, 5.5, 0.5, 2.5, 5.5, 1.5, 2.5}; - - private: - ros::Subscriber path_sub_; - - ros::Publisher local_position_pub_; - ros::Publisher depth_points_pub_; - ros::Publisher global_goal_pub_; -}; - -} // namespace global_planner - -#endif // GLOBAL_PLANNER_MOCK_DATA_NODE_H diff --git a/global_planner/test/main.cpp b/global_planner/test/main.cpp deleted file mode 100644 index 4d820af77..000000000 --- a/global_planner/test/main.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include - -int main(int argc, char **argv) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/global_planner/test/test_example.cpp b/global_planner/test/test_example.cpp deleted file mode 100644 index 14bcdeabe..000000000 --- a/global_planner/test/test_example.cpp +++ /dev/null @@ -1,77 +0,0 @@ -#include - -#include -using std::string; - -class StringTest : public ::testing::Test { - public: - string actualString; - string wrongString; - string expectString; - - char *actualValTrue; - char *actualValFalse; - char *expectVal; - - // Use this method to set up any state that you need for all of your tests - void SetUp() override { - actualString = "hello gtest"; - wrongString = "hello world"; - expectString = "hello gtest"; - - actualValTrue = new char[actualString.size() + 1]; - strncpy(actualValTrue, actualString.c_str(), actualString.size() + 1); - - actualValFalse = new char[wrongString.size() + 1]; - strncpy(actualValFalse, wrongString.c_str(), wrongString.size() + 1); - - expectVal = new char[expectString.size() + 1]; - strncpy(expectVal, expectString.c_str(), expectString.size() + 1); - } - - // Use this method to clean up any memory, network etc. after each test - void TearDown() override { - delete[] actualValTrue; - delete[] actualValFalse; - delete[] expectVal; - } -}; - -// Example code we are testing: -namespace myNormalCode { - -void reverseInPlace(string &toReverse) { - // NB! this only works for ASCII - for (int i = 0, j = toReverse.size() - 1; i < j; i++, j--) { - char tmp = toReverse[i]; - toReverse[i] = toReverse[j]; - toReverse[j] = tmp; - } -} -} - -TEST_F(StringTest, StrEqual) { - // GIVEN: two strings that are the same - - // THEN: we expect them to be equal - EXPECT_STREQ(actualString.c_str(), expectString.c_str()); -} - -TEST_F(StringTest, CStrNotEqual) { - // GIVEN: two char* that are NOT the same - - // THEN: we expect them to be not equal - EXPECT_STRNE(expectVal, actualValFalse); -} - -TEST_F(StringTest, testReverse) { - // GIVEN: a string, and a manually reversed string as well - string toReverse = actualString; - const string expectedReversed = "tsetg olleh"; - - // WHEN: we reverse the string - myNormalCode::reverseInPlace(toReverse); - - // THEN: they should be the same - EXPECT_STREQ(expectedReversed.c_str(), toReverse.c_str()); -} diff --git a/octomap_msgs b/octomap_msgs new file mode 160000 index 000000000..65d0b3b81 --- /dev/null +++ b/octomap_msgs @@ -0,0 +1 @@ +Subproject commit 65d0b3b81f890b5dc3bf531abe0e8c12ef3858e2 diff --git a/octomap_server2 b/octomap_server2 new file mode 160000 index 000000000..09d63ed0e --- /dev/null +++ b/octomap_server2 @@ -0,0 +1 @@ +Subproject commit 09d63ed0e3804689572110c363109a0d1167dd69 diff --git a/pcl_msgs b/pcl_msgs new file mode 160000 index 000000000..16c4174b4 --- /dev/null +++ b/pcl_msgs @@ -0,0 +1 @@ +Subproject commit 16c4174b4903982c8b031672c8cfcdd16a7430dc diff --git a/perception_pcl b/perception_pcl new file mode 160000 index 000000000..135a2c29e --- /dev/null +++ b/perception_pcl @@ -0,0 +1 @@ +Subproject commit 135a2c29e3f46904f586e963cc4a4ec3fe594a9c