ntnu-arl / gbplanner_ros Goto Github PK

View Code? Open in Web Editor NEW

641.0 641.0 146.0 11.63 MB

Graph-based Exploration Planner for Subterranean Environments

License: BSD 3-Clause "New" or "Revised" License

CMake 1.00% C++ 95.88% C 3.13%

gbplanner_ros's People

Contributors

Stargazers

Watchers

Forkers

kazimbalti kaidwang arghyachatterjee abelvdavid walid19 algprasad kongan aerialrob mihirdharmadhikari kfritchman46 michaelfyang jdr-lib buaaalf jinqiang lonfr zxw610 aseligmann pluto-project-it lyk125 nghiajenius-dev ibrahiminfinite ugv-environment-explorer kinggreat24 imaduddinamajid xijunke goldenminerlmg congleetea preetijain7681 rogarg njust-chenl zdkhit girror navigation-lab mfkiwl collector-m qycqycqyc oblivione yapbenzet widemeadows daoran agauto lucianzhong tech2morrow nmboyd sg774 ozi-forks turtlezhong xepost metavai sunghowoo chengwei920412 kostas-alexis namdinhrobotics dejavusgg honglouyemeng tolgakarakurt derektan95 illusion-1404 sarah-banyan loulansuiye edwinstudy icanchen zy-cuhk shakeebbb ompugao zzhmx sanderfu lbaron980810 yanlonglin tops666 usmanmaqbool jesusmartin86 telemething lshengjian boazvig csermac whw-create vaibhav2181 xudong0401 tuonii aloharobotics gaeunyim lijian8 nkhedekar huynam1802 marteruud huiyulin jxx315 vortexmath zhaoziyie sindrebk yinghaojia98 civy-1014 taylorvd yuliangzhong lyciss mr-d-self-driving hotelzululima georgesheslow jh142857

gbplanner_ros's Issues

Change all submodules git repos url due to github's new requirments on security

gbplanner_ros/packages_ssh.rosinstall

Line 4 in ca12791

uri: [email protected]:catkin/catkin_simple.git

All [email protected] ssh URLs should be changed with their https equivalents due to new security policies

Input pointcloud queue getting too long!

When i try to run a demo, i met the error as follows,

QObject::connect: Cannot queue arguments of type 'QVector'
(Make sure 'QVector' is registered using qRegisterMetaType().)
QObject::connect: Cannot queue arguments of type 'QVector'
(Make sure 'QVector' is registered using qRegisterMetaType().)
[ERROR] [1638882589.949854752, 60.176000000]: Input pointcloud queue getting too long! Dropping some pointclouds. Either unable to look up transform timestamps or the processing is taking too long.
[ERROR] [1638882627.590240752, 120.192000000]: Input pointcloud queue getting too long! Dropping some pointclouds. Either unable to look up transform timestamps or the processing is taking too long.
[ERROR] [1638882664.958607277, 180.300000000]: Input pointcloud queue getting too long! Dropping some pointclouds. Either unable to look up transform timestamps or the processing is taking too long.

Who met the same error before? Thanks a lot.

Using GBP in our own uav simulator

Hi I am currently trying to get the GBP Planner to work in our own UAV simulator and I am running into a couple of issues.
We have our own controller so I am converting the trajectory message for our controller.

Does it matter if I use smb or rmf configs for gbp? (I am using and editing the original gbp launch file but removed the gazebo portion).
I am using our own UAV simulator and pass in the odometry topic and point cloud topic into the planner (using a realsense camera for pc). Am I missing any other parameters I should changing?
The trajectory generated is only where the drone is spawned and never allows the drone to progress forward.
Why is this happening?

Any help would be greatly appreciated.
Attached below are my two launch files:
sim1.launch.pdf
sim2.launch.pdf

Plan to Waypoint Function cannot work

Hello,I am trying the Plan to Waypoint function in the UI which you mentioned in

https://github.com/ntnu-arl/gbplanner_ros/wiki/Demo#5-plan-to-waypoint-using-the-global-graph.

I followed your instructions:First,I set the goal point using the 2D nav goal button in rviz interface.And then,I clicked the Plan to Waypoint button in the UI.
From the terminal output,I can see that the goal point has been set.However,the planning path doesn't go to goal point.

Could you tell me why? I am struggling with solving this problem for a long time,and still have no idea.

What is the GBPlanner Control?

Hello, I am looking at your open source project: NTNU Autonomous Robots Lab. I followed the above tutorial to run the Aerial Robot Demo. When I input: "roslaunch gbplanner rmf_sim.launch", the rviz interface is displayed, but no The point cloud is displayed,and the error message is :" Error:Could not find library corresponding to plugin gbplanner_ui/GBPlanner Control.Make sure the plugin description XML file has the correct name of the library and that the library actually exists. ", but I can't find the GBPlanner Control, how can I solve it. thanks

Integration with other SLAM Algorithms

Hi @MihirDharmadhikari,

I wonder if this graph-based planner can work with other types of SLAM algorithms such as RTABMap? It has the flexibility to allow us to map the scene with RGBD input, while localizing using 3D LiDAR input. It has the ability to output octomaps incrementally that is local to the robot's SLAM pose, similar to submaps in VoxGraph. This seems to be compatible with this package as reference to the GBPlanner1's README.md.

However, please be aware that the planner requires several dependencies; for example:

Mapping framework: either Voxblox or Octomap
Planner-Control interface package: pci_mav
MAV simulation: RotorS, LiDAR simulator

By extension, how about RTABMap's potential integration with COHORT? Would be amazing to see how we can integrate this exploratory planner to existing ROS frameworks that is used by many others. (I am currently using RTABMap for localization, and move_base to send navigation goals)

Regards,
Derek

Integrating the planner with different UAV platfrom (not available in rotors_gazebo)

I am using a different UAV platform in GAZEBO that is equipped only with a Lidar and a depth camera and uses px4. My system provides the following topics 1) odometry (localization) 2) point cloud (from Lidar) 3)images. When I run my system, the drone takes off and waits for a goal position. I have my own controller that is responsible for moving the drone from Point A to Point B.

I was wondering 1) how can I integrate the planner with my system?

I tried to modify the rmf_sim.launch file to make it work with my system as shown. Kindly let me know 2)what to change in the rmf_sim.launch file to make it work?

3) Is there any documentation to guide us through the process of adding a new drone?

Also, 4) what is the topic that is used to send a goal position to the drone or path? Should I change it from the code or from the launch file?


<?xml version="1.0"?>
<launch>
  <!-- All settings -->
  <arg name="robot_name" default="uav1"/>
  <arg name="gazebo_gui_en" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="rviz_en" default="true" />
  <arg name="launch_prefix" default=""/> <!-- gdb -ex run //args -->
  
  <param name="use_sim_time" value="$(arg use_sim_time)"/>

<!--%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
  <arg name="odometry_topic" default="/uav1/aloam/odom"/>
<!--%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
  <!-- Config files -->
  <arg name="gbplanner_config_file" default="$(find gbplanner)/config/$(arg robot_name)/gbplanner_config.yaml"/>
  <arg name="pci_file" default="$(find gbplanner)/config/$(arg robot_name)/planner_control_interface_sim_config.yaml"/>
  <arg name="voxblox_config_file" default="$(find gbplanner)/config/$(arg robot_name)/voxblox_sim_config.yaml"/>
  <arg name="map_config_file" default="$(arg voxblox_config_file)"/>
  <arg name="world_file" default="$(find planner_gazebo_sim)/worlds/darpa_subt_final_circuit.world"/>

  <group ns="$(arg robot_name)">
    <node name="img_throttler" type="throttle" pkg="topic_tools" args="messages vi_sensor/camera_depth/depth/points 5 vi_sensor/camera_depth/depth/points_throttled" />
    <node name="odo_throttler" type="throttle" pkg="topic_tools" args="messages ground_truth/odometry 100 ground_truth/odometry_throttled" />


    <!-- Position controller  -->
    <node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
      <rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg robot_name).yaml" />
      <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg robot_name).yaml" />
      <remap from="odometry" to="$(arg odometry_topic)" />
    </node>
  </group>


  <node name="pose_throttler" type="throttle" pkg="topic_tools" args="messages $(arg robot_name)/ground_truth/pose_with_covariance 10 /msf_core/pose" />
  <!-- Graph based planning -->
  <node pkg="gbplanner" type="gbplanner_node" name="gbplanner_node" output="screen" launch-prefix="$(arg launch_prefix)">
    <remap from="odometry" to="$(arg odometry_topic)" />
    <remap from="/pointcloud" to="/uav1/os_cloud_nodelet/points_processed" />
    <rosparam command="load" file="$(arg gbplanner_config_file)" />
    <rosparam command="load" file="$(arg map_config_file)" />
  </node>

  <!-- Planner and Control Interface -->
  <node pkg="pci_general" type="pci_general_ros_node" name="pci_general_ros_node" output="screen">

    <remap from="planner_server" to="gbplanner" />
    <remap from="planner_homing_server" to="gbplanner/homing" />
    <remap from="odometry" to="$(arg odometry_topic)"/>
    <rosparam command="load" file="$(arg pci_file)" />
  </node>

  <node pkg="rviz" type="rviz" name="gbplanner_ui" output="screen" args="-d $(find gbplanner)/config/rviz/rmf_obelix.rviz"/>

</launch>

Issue with reading xacro file in UAV demo on ROS-Melodic

I had an issue where the UAV demo did not want to launch. It turned out that ROS-melodic had an error with xacro. The maintainer of the xacro package is aware of this and has triggered a release, but it is not yet synced.

As a fix I ended up changing to the the testing repository of ROS-Melodic. Here a newer version of xacro is available, which I simply downloaded/updated through "sudo apt-get ros-melodic-xacro".

The release that was triggered by the xacro maintainer has yet to be synced due to a lot of regression in melodic.

Please see the links for more context:
ethz-asl/rotors_simulator#695 (comment)
ros/xacro#316 (comment)
ros/rosdistro#32236
http://wiki.ros.org/TestingRepository

Error received:
RLException: while processing /home/daniel/gbplanner2_ws/src/sim/rotors_simulator/rotors_gazebo/launch/spawn_mav.launch: Invalid <param> tag: Cannot load command parameter [robot_description]: command [['/opt/ros/melodic/lib/xacro/xacro', '/home/daniel/gbplanner2_ws/src/sim/rotors_simulator/rotors_description/urdf/rmf_obelix.gazebo', 'enable_logging:=false', 'enable_ground_truth:=true', 'enable_mavlink_interface:=false', 'log_file:=rmf_obelix', 'wait_to_record_bag:=false', 'mav_name:=rmf_obelix', 'namespace:=rmf_obelix']] returned with code [2].

Param xml is <param command=" $(find xacro)/xacro '$(arg model)' enable_logging:=$(arg enable_logging) enable_ground_truth:=$(arg enable_ground_truth) enable_mavlink_interface:=$(arg enable_mavlink_interface) log_file:=$(arg log_file) wait_to_record_bag:=$(arg wait_to_record_bag) mav_name:=$(arg mav_name) namespace:=$(arg namespace)" name="robot_description"/>

ground robots

Hello,
Does this repository also support ground robot without legs?
Thanks

I want to change the range of the sensor.

I am using a LiDAR sensor tilted 30 degrees forward for ground recognition. Therefore, I want to select the sensor range as KLidar and adjust the angle in the GBPlanner. Can I make this change in param.cc?

Which node subscribe the “/input_pointcloud”？

Hi @tungdanganh @MihirDharmadhikari ,
Thank you very much for your work. I've read both your papers and the youtube video detailing the entire exploration algorithm. When I try your released code, in the ground robot sim demo, I found this in smb_sim.launch
<node pkg="gbplanner" type="gbplanner_node" name="gbplanner_node" output="screen" launch-prefix="$(arg launch_prefix)">
<remap from="odometry" to="$(arg odometry_topic)" />
<remap from="/pointcloud" to="/input_pointcloud" />
<rosparam command="load" file="$(arg gbplanner_config_file)" />
<rosparam command="load" file="$(arg map_config_file)" />
</node>

I can't find the subscriber of /point_cloud in gbplanner.cpp or rrg.cpp, but when I use rostopic node info /gbplanner_node I got this:

Node [/gbplanner_node]
Publications:

/freespace_pointcloud [sensor_msgs/PointCloud2]
/gbp_time_log [std_msgs/Float32MultiArray]
/gbplanner_node/mesh [voxblox_msgs/Mesh]
/gbplanner_node/occupied_nodes [visualization_msgs/MarkerArray]
/gbplanner_node/surface_pointcloud [sensor_msgs/PointCloud2]
/gbplanner_node/tsdf_map_out [voxblox_msgs/Layer]
/gbplanner_node/tsdf_pointcloud [sensor_msgs/PointCloud2]
/gbplanner_node/tsdf_slice [sensor_msgs/PointCloud2]
/planner_control_interface/msg/reset [std_msgs/Bool]
/rosout [rosgraph_msgs/Log]
/tf [tf2_msgs/TFMessage]
/vis/alternate_path [visualization_msgs/MarkerArray]
/vis/best_planning_paths [visualization_msgs/MarkerArray]
/vis/blind_mod_path [visualization_msgs/MarkerArray]
/vis/geofence [visualization_msgs/MarkerArray]
/vis/hyperplanes [visualization_msgs/MarkerArray]
/vis/imp_ref_path [visualization_msgs/MarkerArray]
/vis/mod_path [visualization_msgs/MarkerArray]
/vis/negative_edges [visualization_msgs/MarkerArray]
/vis/no_gain_zone [visualization_msgs/MarkerArray]
/vis/occupied_pcl [sensor_msgs/PointCloud2]
/vis/planning_failed [visualization_msgs/MarkerArray]
/vis/planning_global_graph [visualization_msgs/MarkerArray]
/vis/planning_global_path [visualization_msgs/MarkerArray]
/vis/planning_graph [visualization_msgs/MarkerArray]
/vis/planning_homing_path [visualization_msgs/MarkerArray]
/vis/planning_projected_graph [visualization_msgs/MarkerArray]
/vis/planning_workspace [visualization_msgs/MarkerArray]
/vis/ray_casting [visualization_msgs/MarkerArray]
/vis/ref_path [visualization_msgs/MarkerArray]
/vis/robot_state [visualization_msgs/MarkerArray]
/vis/sampler [visualization_msgs/MarkerArray]
/vis/sensor_fov [visualization_msgs/MarkerArray]
/vis/shortest_path_clustering [visualization_msgs/MarkerArray]
/vis/shortest_paths [visualization_msgs/MarkerArray]
/vis/state_history [visualization_msgs/MarkerArray]
/vis/volumetric_gains [visualization_msgs/MarkerArray]

Subscriptions:

/clock [rosgraph_msgs/Clock]
/freespace_pointcloud [sensor_msgs/PointCloud2]
/gbplanner_node/tsdf_map_in [unknown type]
/ground_truth/state [nav_msgs/Odometry]
/input_pointcloud [sensor_msgs/PointCloud2]
/planner_control_interface/stop_request [std_msgs/Bool]
/pose [unknown type]
/pose_stamped [unknown type]
/robot_status [unknown type]
/semantic_location [planner_semantic_msgs/SemanticPoint]
/tf [tf2_msgs/TFMessage]
/tf_static [tf2_msgs/TFMessage]
/traversability_estimation/untraversable_polygon [unknown type]

which means the gbplaner_node subscribe the /input_pointcloud. and I only found in the [/maping/voxblox/voxblox_ros/src/tsdf_server.cc], the node subscribe the pointcloud.
So I am confused about the gbplanner node only contains

src/gbplanner.cpp
src/rrg.cpp
src/gbplanner_rviz.cpp
in CMakeLists.txt.
So How does the gbplanner_node link to voxblox_ros?

Backward direction

Hi, this project it's really cool but i have one problem, i set up gbplanner on real ugv and my problem is that the real robot goes 80% of the time in backward. How can modify this for the robot drive in forward direction?

Unable to set goal pose and get trajectory

@MihirDharmadhikari Thanks for the excellent contributions.

Currently we are trying to deploy this package with real robot (SPOT) and currently able to get it work as it shown in the image.
We are using custom lidar odom package.

What does those highlighted bounding box indicates ?

What does this red box indicates ?

Would you please let us know ! how to set goal pose and get trajectory for the specific goal pose.

Appreciate any response.

Build bug

Hi, thank you for sharing this great work! I would like to try on my machine, but I keep getting error when I run catkin build. After I searched, this should be a error due to C++, so I ran catkin build -DCMAKE_CXX_STANDARD=17. Still showing the same error. DO you have any idea why is that? Appreciate your help!

Integrating the gbplanner2 with another agv platform?

Hi, I am trying to using an agv with a lidar on it. So I tried to modified the smb_sim.launch like below. It turn out that, Voxblox had results on RViz, but planner did not work when I click the "start planner" button.

And, when I tried to use original module(smb.urdf.xacro), it worked and command trajectory was shown on Rviz . So I am wandering how to integrating the planner.

Looking forward to your reply.

<launch>
  <arg name="robot_name" default="agv"/>
  <arg name="gazebo_gui_en" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="rviz_en" default="true" />
  <arg name="launch_prefix" default=""/> <!-- gdb -ex run //args -->
  <param name="use_sim_time" value="$(arg use_sim_time)"/>

  <!-- Config files -->
  <arg name="gbplanner_config_file" default="$(find gbplanner)/config/$(arg robot_name)/gbplanner_config.yaml"/>
  <arg name="pci_file" default="$(find gbplanner)/config/$(arg robot_name)/planner_control_interface_sim_config.yaml"/>
  <arg name="voxblox_config_file" default="$(find gbplanner)/config/$(arg robot_name)/voxblox_sim_config.yaml"/>
  <arg name="map_config_file" default="$(arg voxblox_config_file)"/>

    <arg name = "model_xacro" default = "$(find scout_gazebo)/urdf/base.xacro" />
    <param name="robot_description" command="$(find xacro)/xacro '$(arg model_xacro)'" />
   <-- <param name="robot_description" command="$(find xacro)/xacro '$(find smb_description)/urdf/smb.urdf.xacro'" />-->

    <!-- Launch  the robot state publisher -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

    <arg name="world_file" default="$(find gazebo_cave_world)/worlds/cave_world.world"/>
    <include file="$(find gazebo_ros)/launch/empty_world.launch"> 
		<arg name="world_name" value="$(arg world_file)"/>
		<arg name="paused" value="false"/>
		<arg name="use_sim_time" value="true"/>
		<arg name="gui" value="$(arg gazebo_gui_en)"/>
		<arg name="headless" value="false"/>
		<arg name="debug" value="false"/>
        <arg name="verbose" value="false"/>
    </include>

    <node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model scout -param robot_description -x 10 -y -20 -z 0.5 -Y 3.1415926"  />  
    <node name="message_to_tf" pkg="smb_gazebo" type="message_to_tf" output="screen">
      <param name="odometry_topic" value="ground_truth/state"/>
      <param name="frame_id" value="world"/>
    </node>
    <node name="odom_to_world_tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /world /odom 10"/>
  
  <arg name="odometry_topic" default="/ground_truth/state"/>
  <node pkg="topic_tools" type="relay" name="vlp_relay" args="/velodyne_points /input_pointcloud" />
  <node name="pose_throttler" type="throttle" pkg="topic_tools" args="messages $(arg robot_name)/ground_truth/pose_with_covariance 10 /msf_core/pose" />

  <!-- Graph based planning -->
  <node pkg="gbplanner" type="gbplanner_node" name="gbplanner_node" output="screen" launch-prefix="$(arg launch_prefix)">
    <remap from="odometry" to="$(arg odometry_topic)" />
    <remap from="/pointcloud" to="/input_pointcloud" />
    <rosparam command="load" file="$(arg gbplanner_config_file)" />
    <rosparam command="load" file="$(arg map_config_file)" />
  </node>

  <!-- Planner and Control Interface -->
  <node pkg="pci_general" type="pci_general_ros_node" name="pci_general_ros_node" output="screen">
    <remap from="command/trajectory" to="$(arg robot_name)/command/trajectory" />
    <remap from="planner_server" to="gbplanner" />
    <remap from="planner_homing_server" to="gbplanner/homing" />
    <remap from="odometry" to="$(arg odometry_topic)"/>
    <rosparam command="load" file="$(arg pci_file)" />
  </node>

  <node pkg="rviz" type="rviz" name="gbplanner_ui" output="screen" args="-d $(find gbplanner)/config/rviz/agv.rviz"/>
</launch>

Is it necessary to have the "max_ground_height"

As explained in the following code comment, it seems that the "max_ground_height" param is there to project "ground_height", "groud_dist" and "robot_height" to the same reference level surface. Why is this necessary?
// Get the point on the ground at a heigh of // planning_params_.max_ground_height projected_edge_pt(2) -= (ground_height - planning_params_.max_ground_height);

Planner service failed with real robot

Hello,

With your continuous support, we were able to test this planner with bag_file and everything working as expected.

But, when i tried to test module with real robot (SPOT robot), getting error in like below.

[ INFO] [1711142413.262462715]: getLocalPointcloud time: 0.019490
[ERROR] [1711142413.262526028]: Exception thrown while processing service call: bool pcl::PCA<PointT>::initCompute() [with PointT = pcl::PointXYZI] in /usr/include/pcl-1.10/pcl/common/impl/pca.hpp @ 58 : [pcl::PCA::initCompute] number of points < 3
[ERROR] [1711142413.262815389]: Service call failed: service [/gbplanner] responded with an error: bool pcl::PCA<PointT>::initCompute() [with PointT = pcl::PointXYZI] in /usr/include/pcl-1.10/pcl/common/impl/pca.hpp @ 58 : [pcl::PCA::initCompute] number of points < 3
[ERROR] [1711142413.262846490]: Planner service failed

Also, In rviz error with marker visulization as below:

Any suggestions please !

I have some question about generate path.

Hello,
Thank you for your open source contribution.

Currently, we are testing mapping indoors and outdoors using your package.

So far, there have been no problems, and we are getting good results.

One question I have is about the performance in different environments. While it creates good paths in spaces like tunnels, in large room-shaped spaces like factories, despite all the surrounding point cloud data being collected, the search does not terminate.

I have been studying the papers and the code, but I wanted to ask this question.

Thank you.
Best regard

How to save the map.into file?

Hello, sincerely thank you for your work, very useful to me! I use the robot to explore and create a map, how do I save this map data into the file? Does your algorithm provide a function to save the map data? Looking forward to your suggestions! Thanks again for your contributions!

Wrong subscript order in voxblox_alt_impl.cpp

Hello,
Thank you very much for your work. When I was reading the code I noticed that the order of the subscript of starting_points in gbplanner_ros/planner_common/src/voxblox/voxblox_alt_impl.cpp seems to be written in reverse order.

The starting_points(height, width) is declared on line 569.
But the subscript is starting_points(width, height), as shown below.
It seems that the order of all starting_points in the file may need to be swapped.

In addition, I think the range of x should be [0, width - 1), instead of [0, width) (line 606).
Otherwise starting_points(x + 1, y) will also encounter the problem of out-of-bounds, such as line 693.
Please have a check.

Best,
Zezhou

pci_mav package crashes on using gcc-8 and g++-8

Using gcc-8 and g++-8 causes the pci_mav package to crash after executing 1 planning iteration.

Using gcc-7 and g++-7 does not cause this error to show up.

[ WARN] [1612929626.567301264, 26.350000000]: PCI: Ready to trigger the planner.
terminate called after throwing an instance of 'std::bad_alloc'
  what():  std::bad_alloc
[pci_mav_ros_node-16] process has died [pid 21673, exit code -6, cmd /home/arl/gbplanner_ws/devel/lib/pci_mav/pci_mav_ros_node command/trajectory:=m100/command/trajectory planner_server:=gbplanner planner_homing_server:=gbplanner/homing odometry:=m100/ground_truth/odometry_throttled __name:=pci_mav_ros_node __log:=/home/arl/.ros/log/6f136fca-6b54-11eb-8bb6-7085c2cb6201/pci_mav_ros_node-16.log].
log file: /home/arl/.ros/log/6f136fca-6b54-11eb-8bb6-7085c2cb6201/pci_mav_ros_node-16*.log

Having some troubles when run the gbplanner2 with real Lidar databag

Hello,I'm trying to run gbplanner2 with a real UAV and Lidar sensor(ouster)
So I changed the odometry topic name and lidar topic name in rmf_sim.launch file as follows:

<arg name="odometry_topic" default="/aft_mapped_to_init"/>
<remap from="/pointcloud" to="/os_cloud_node/points"/>

Then,rviz shows valid samples are limited in a small cuboid as follows:

and PCI shows no errors as follows:

I also tried to modify some parameters in three config files,for example,BoundedSpaceParams-Local,RobotParams-size,tsdf_voxel_size,tsdf_voxel_per_side,edge_length_min/max.However,it does no help.

Could you please tell me the reason why? I'm so desperate wondering why.
Thanks!

I connected my custom simulation drone to a planner to generate a TSDF point cloud, but it is created slightly differently.

hi.

First, I was very impressed by your excellent planner. Thank you for your research.

I have modified the provided file to update odometry and point cloud data, and I have also adjusted the world tf. Below is the launch file.

<launch>
  <!-- All settings -->
  <arg name="robot_name" default="rmf_obelix"/>
  <arg name="gazebo_gui_en" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="rviz_en" default="true" />
  <arg name="launch_prefix" default=""/> <!-- gdb -ex run //args -->
  <param name="use_sim_time" value="$(arg use_sim_time)"/>

  <!-- Config files -->
  <arg name="gbplanner_config_file" default="$(find gbplanner)/config/$(arg robot_name)/gbplanner_config.yaml"/>
  <arg name="pci_file" default="$(find gbplanner)/config/$(arg robot_name)/planner_control_interface_sim_config.yaml"/>
  <arg name="voxblox_config_file" default="$(find gbplanner)/config/$(arg robot_name)/voxblox_sim_config.yaml"/>
  <arg name="map_config_file" default="$(arg voxblox_config_file)"/>
  <!-- <arg name="world_file" default="$(find planner_gazebo_sim)/worlds/darpa_cave_01.world"/> -->
  <!--<arg name="world_file" default="$(find planner_gazebo_sim)/worlds/darpa_subt_final_circuit.world"/>-->

  <!-- Static TF -->
  <node pkg="tf" type="static_transform_publisher" name="tf_fastlio2" args="0 0 0 0 0 0 camera_init world 1" />
  
  
  
  <node pkg="topic_tools" type="relay" name="mid360_relay" args="/cloud_registered /input_pointcloud"/>

  <arg name="odometry_topic" default="/Odometry"/>
  
  
  <!-- Graph based planning -->
  <node pkg="gbplanner" type="gbplanner_node" name="gbplanner_node" output="screen" launch-prefix="$(arg launch_prefix)">
    <remap from="odometry" to="$(arg odometry_topic)" />
    <remap from="/pointcloud" to="/input_pointcloud" />
    <rosparam command="load" file="$(arg gbplanner_config_file)" />
    <rosparam command="load" file="$(arg map_config_file)" />
  </node>

  <!-- Planner and Control Interface -->
  <node pkg="pci_general" type="pci_general_ros_node" name="pci_general_ros_node" output="screen">
    <remap from="command/trajectory" to="$(arg robot_name)/command/trajectory" />
    <remap from="planner_server" to="gbplanner" />
    <remap from="planner_homing_server" to="gbplanner/homing" />
    <remap from="odometry" to="$(arg odometry_topic)"/>
    <rosparam command="load" file="$(arg pci_file)" />
  </node>

  <node pkg="rviz" type="rviz" name="gbplanner_ui" output="screen" args="-d $(find gbplanner)/config/rviz/rmf_obelix.rviz"/>

</launch>

Everything seemed to be working well, but I found that some TSDF point clouds were not being generated.

my tsdf pointcloud

original gbplanner rmf

It seems that the TSDF between the outer wall and the drone is not being generated when the drone exits from within the wall. It appears to consistently generate point clouds only from the origin.

In my opinion, I believe that TSDF should be generated from the sensor center in the world frame, but it seems to be generated from the origin in the world frame.

As a result, in complex environments, when the drone moves behind obstacles, some TSDF is not generated, leading to suboptimal exploration paths

What do you think could be the issue? I would appreciate any assistance or insights.

Thanks!

Can we run path exploration with the PCD map ?

Hello,

First of all thank you so much sharing such a very nice repository for path planning.
I have followed the tutorial and tested with simulation environment and everything working fine.
After going through videos and tutorials, i have few question, i would greatly appreciate your response.

Can we explore path in our custom prebuilt PCD map ? without being
Do we need to rely voxblox ? Can we use any alternative mapping library !

Currently, i am working with robot for underground mining and we are using lidar map matching algorithm for localization
and publishing /odometry topic. We are looking for using this package for path exploration in the pcd map.

Any inputs greatly appreciated.

Regards,
Ajay

Using RGB-D Camera

Hi. Thanks for sharing this great repo.

I think this repo is really similar with MBPlanner from your lab.

So, the same issue is happening here.
Using RGB-D Camera with "kCamera" SensorParams in config files,
It continuously returns "No positive gain was found"

[ WARN] [1607462735.649574133, 16.540000000]: [PLANNER_ERROR] No positive gain was found.
[ WARN] [1607462735.649621397, 16.540000000]: Planner returned an empty path
[ INFO] [1607462736.207201124, 17.040000000]: PlannerControlInterface: Running Planner (kAuto)
[ INFO] [1607462736.218772441, 17.052000000]: Planning iteration 3
[ INFO] [1607462736.267533763, 17.100000000]: Set bound mode: kExtendedBound
[ INFO] [1607462736.268022768, 17.100000000]: Current box contains Occupied voxels.

I think this repo also has to be fixe as this pull request of MBPlanner.
As I made a same issue there, and got fixed from the first author of the paper here

Thank you in advance.

Exploration using a realsense D435i

First of all, thank you so much for your great work!

I have been using it on my simulations using an OS64 and it works phenomenally!
The thing is, I'm currently trying to get it to work using only the depth point cloud from a realsense camera installed in front of the UAV.
However, I have noticed that the TSDF map it generates does not fill the scan with free space unless there is a clear obstacle in front of it.
I'll explain: when I fly, let's say, inside a tunnel, some sort of cone of free space is generated from the sensor POV, but this free space is only close to the walls of that tunnel, so when I execute the exploration, the paths generated are always too close to the walls, as it cannot expand the graph in the "middle". You can see what I'm talking about in the images below:

As you can see, the tunnel is not being filled with free space, something that happens not when using an OS64 for instance.
I'm guessing this is more related to voxblox itself, so i'll leave here the parameters i'm using:

world_frame: map
use_tf_transforms: True
use_freespace_pointcloud: True
tsdf_voxels_per_side: 16
tsdf_voxel_size: 0.20
max_ray_length_m: 15.0
truncation_distance: 0.6
voxel_carving_enabled: True
use_sparsity_compensation_factor: True
sparsity_compensation_factor: 100.0
color_mode: "colors"
verbose: False
update_mesh_every_n_sec: 1.0
mesh_min_weight: 1e-4
slice_level: 1.0
method: "merged"
integration_order_mode: "sorted"
max_consecutive_ray_collisions: 0
publish_slices: False
publish_pointclouds: True
allow_clear: True
pointcloud_queue_size: 10
min_time_between_msgs_sec: 0.0
#max_block_distance_from_body: FLT_MAX
publish_tsdf_info: False
publish_slices: False
publish_traversable: True
traversability_radius: 0.4
enable_icp: False
icp_refine_roll_pitch: False
accumulate_icp_corrections: True
timestamp_tolerance_sec: 0.001 #0.001
publish_tsdf_map: True
publish_esdf_map: True
esdf_max_distance_m: 2.0
clear_sphere_for_planning: False
clear_sphere_radius: 0.8
use_const_weight: False
use_weight_dropoff: True
use_symmetric_weight_drop_off: False
clearing_ray_weight_factor: 0.01
weight_ray_by_range: False
min_ray_length_m: 0.5
occupancy_min_distance_voxel_size_factor: 1.0

Any help would be really appreciated! Thanks in advance!

How to deal with dyanmic objects in the environments.

Hi, the moving object (like people)would easily remain on the borderlines of point clouds when I generated the TSDF data. The result of this is that when the person walks away, the robot thinks that there is still an obstacle, which in turn affects the path of autonomous exploration. Is there a way to solve this problem? Thanks!

gbplanner_node crash, planner service failed.

Hello,

I built the package on ROS Noetic and experienced a crash, here is the output:

`roslaunch gbplanner smb_sim.launch
... logging to /home/antoan/.ros/log/c189b720-726c-11ed-9f3d-035f770cbc42/roslaunch-Ronin-736510.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://Ronin:38123/

SUMMARY
========

PARAMETERS
 * /gazebo/enable_ros_network: True
 * /gbplanner_node/AdaptiveObbParams/bounding_box_size_max: 35
 * /gbplanner_node/AdaptiveObbParams/local_pointcloud_range: 50.0
 * /gbplanner_node/AdaptiveObbParams/type: kPca
 * /gbplanner_node/BoundedSpaceParams/Global/max_val: [1000.0, 1000.0, ...
 * /gbplanner_node/BoundedSpaceParams/Global/min_val: [-1000.0, -1000.0...
 * /gbplanner_node/BoundedSpaceParams/Global/type: kCuboid
 * /gbplanner_node/BoundedSpaceParams/Local/max_extension: [20.0, 20.0, 20.0]
 * /gbplanner_node/BoundedSpaceParams/Local/max_val: [15.0, 15.0, 3.0]
 * /gbplanner_node/BoundedSpaceParams/Local/min_extension: [-20.0, -20.0, -2...
 * /gbplanner_node/BoundedSpaceParams/Local/min_val: [-15.0, -15.0, -3.0]
 * /gbplanner_node/BoundedSpaceParams/Local/type: kCuboid
 * /gbplanner_node/BoundedSpaceParams/LocalAdaptiveExp/max_val: [10.0, 10.0, 0.75]
 * /gbplanner_node/BoundedSpaceParams/LocalAdaptiveExp/min_val: [-10.0, -10.0, -0...
 * /gbplanner_node/BoundedSpaceParams/LocalAdaptiveExp/type: kCuboid
 * /gbplanner_node/BoundedSpaceParams/LocalSearch/max_val: [50.0, 50.0, 10.0]
 * /gbplanner_node/BoundedSpaceParams/LocalSearch/min_val: [-50.0, -50.0, -1...
 * /gbplanner_node/BoundedSpaceParams/LocalSearch/type: kCuboid
 * /gbplanner_node/GeofenceParams/AreaList: ['GLeft', 'GBack'...
 * /gbplanner_node/GeofenceParams/GBack/center: [-3.0, 0.0, 0.0]
 * /gbplanner_node/GeofenceParams/GBack/size: [2.0, 6.0, 0.0]
 * /gbplanner_node/GeofenceParams/GLeft/center: [0.0, 4.0, 0.0]
 * /gbplanner_node/GeofenceParams/GLeft/size: [6.0, 2.0, 0.0]
 * /gbplanner_node/GeofenceParams/GRight/center: [0.0, -4.0, 0.0]
 * /gbplanner_node/GeofenceParams/GRight/size: [6.0, 2.0, 0.0]
 * /gbplanner_node/NoGainZones/g1/max_val: [2.0, 50.0, 20.0]
 * /gbplanner_node/NoGainZones/g1/min_val: [-50.0, -50.0, -2...
 * /gbplanner_node/NoGainZones/g1/type: kCuboid
 * /gbplanner_node/PlanningParams/augment_free_voxels_time: 1
 * /gbplanner_node/PlanningParams/auto_global_planner_enable: True
 * /gbplanner_node/PlanningParams/auto_homing_enable: True
 * /gbplanner_node/PlanningParams/bound_mode: kExtremeBound
 * /gbplanner_node/PlanningParams/cluster_vertices_for_gain: True
 * /gbplanner_node/PlanningParams/clustering_radius: 0.5
 * /gbplanner_node/PlanningParams/edge_length_max: 3.0
 * /gbplanner_node/PlanningParams/edge_length_min: 0.1
 * /gbplanner_node/PlanningParams/edge_overshoot: 0.0
 * /gbplanner_node/PlanningParams/exp_gain_voxel_size: 0.8
 * /gbplanner_node/PlanningParams/exp_sensor_list: ['VLP16']
 * /gbplanner_node/PlanningParams/free_frustum_before_planning: False
 * /gbplanner_node/PlanningParams/free_voxel_gain: 0.0
 * /gbplanner_node/PlanningParams/freespace_cloud_enable: False
 * /gbplanner_node/PlanningParams/geofence_checking_enable: True
 * /gbplanner_node/PlanningParams/global_frame_id: world
 * /gbplanner_node/PlanningParams/global_path_inclination_check: True
 * /gbplanner_node/PlanningParams/go_home_if_fully_explored: True
 * /gbplanner_node/PlanningParams/hanging_vertex_penalty: 5.0
 * /gbplanner_node/PlanningParams/homing_backward: False
 * /gbplanner_node/PlanningParams/interpolate_projection_distance: False
 * /gbplanner_node/PlanningParams/leafs_only_for_volumetric_gain: False
 * /gbplanner_node/PlanningParams/max_ground_height: 0.7
 * /gbplanner_node/PlanningParams/max_inclination: 0.4363323129985824
 * /gbplanner_node/PlanningParams/nearest_range: 2.5
 * /gbplanner_node/PlanningParams/nearest_range_max: 3.0
 * /gbplanner_node/PlanningParams/nearest_range_min: 0.1
 * /gbplanner_node/PlanningParams/no_gain_zones_list: ['g1']
 * /gbplanner_node/PlanningParams/nonuniform_ray_cast: True
 * /gbplanner_node/PlanningParams/num_edges_max: 5000
 * /gbplanner_node/PlanningParams/num_loops_cutoff: 2000
 * /gbplanner_node/PlanningParams/num_loops_max: 20000
 * /gbplanner_node/PlanningParams/num_vertices_max: 500
 * /gbplanner_node/PlanningParams/occupied_voxel_gain: 0.0
 * /gbplanner_node/PlanningParams/path_direction_penalty: 0.3
 * /gbplanner_node/PlanningParams/path_interpolation_distance: 0.5
 * /gbplanner_node/PlanningParams/path_length_penalty: 0.07
 * /gbplanner_node/PlanningParams/path_safety_enhance_enable: True
 * /gbplanner_node/PlanningParams/ray_cast_step_size_multiplier: 1.0
 * /gbplanner_node/PlanningParams/relaxed_corridor_multiplier: 1.5
 * /gbplanner_node/PlanningParams/robot_height: 0.6
 * /gbplanner_node/PlanningParams/rr_mode: kGraph
 * /gbplanner_node/PlanningParams/time_budget_limit: 3600
 * /gbplanner_node/PlanningParams/traverse_length_max: 6.0
 * /gbplanner_node/PlanningParams/traverse_time_max: 40.0
 * /gbplanner_node/PlanningParams/type: kAdaptiveExploration
 * /gbplanner_node/PlanningParams/unknown_voxel_gain: 60.0
 * /gbplanner_node/PlanningParams/use_current_state: True
 * /gbplanner_node/PlanningParams/use_ray_model_for_volumetric_gain: True
 * /gbplanner_node/PlanningParams/yaw_tangent_correction: True
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/Heading/max_val: 3.141592653589793
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/Heading/min_val: -3.141592653589793
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/Heading/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/Heading/sample_mode: kManual
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/X/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/X/sample_mode: kLocal
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/Y/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/Y/sample_mode: kLocal
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/Z/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForAdaptiveExp/Z/sample_mode: kLocal
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/Heading/max_val: 3.141592653589793
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/Heading/min_val: -3.141592653589793
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/Heading/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/Heading/sample_mode: kManual
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/X/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/X/sample_mode: kLocal
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/Y/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/Y/sample_mode: kLocal
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/Z/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForExploration/Z/sample_mode: kLocal
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/Heading/max_val: 3.141592653589793
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/Heading/min_val: -3.141592653589793
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/Heading/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/Heading/sample_mode: kManual
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/X/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/X/sample_mode: kLocal
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/Y/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/Y/sample_mode: kLocal
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/Z/pdf_type: kUniform
 * /gbplanner_node/RandomSamplerParams/SamplerForSearching/Z/sample_mode: kLocal
 * /gbplanner_node/RobotParams/bound_mode: kExtendedBound
 * /gbplanner_node/RobotParams/center_offset: [0.0, 0.0, 0.0]
 * /gbplanner_node/RobotParams/relax_ratio: 0.5
 * /gbplanner_node/RobotParams/safety_extension: [2.0, 3.0, 0.2]
 * /gbplanner_node/RobotParams/size: [0.8, 0.8, 0.2]
 * /gbplanner_node/RobotParams/size_extension: [0.2, 0.2, 0.2]
 * /gbplanner_node/RobotParams/size_extension_min: [0.0, 0.0, 0.0]
 * /gbplanner_node/RobotParams/type: kGroundRobot
 * /gbplanner_node/SensorParams/VLP16/center_offset: [0.0, 0.0, 0.0]
 * /gbplanner_node/SensorParams/VLP16/fov: [6.28318530717958...
 * /gbplanner_node/SensorParams/VLP16/frontier_percentage_threshold: 0.05
 * /gbplanner_node/SensorParams/VLP16/max_range: 20.0
 * /gbplanner_node/SensorParams/VLP16/resolution: [0.08726646259971...
 * /gbplanner_node/SensorParams/VLP16/rotations: [0.0, 0.0, 0.0]
 * /gbplanner_node/SensorParams/VLP16/type: kLidar
 * /gbplanner_node/SensorParams/sensor_list: ['VLP16']
 * /gbplanner_node/accumulate_icp_corrections: True
 * /gbplanner_node/allow_clear: True
 * /gbplanner_node/clear_sphere_for_planning: False
 * /gbplanner_node/clear_sphere_radius: 0.8
 * /gbplanner_node/clearing_ray_weight_factor: 0.01
 * /gbplanner_node/color_mode: colors
 * /gbplanner_node/enable_icp: False
 * /gbplanner_node/esdf_max_distance_m: 2.0
 * /gbplanner_node/icp_refine_roll_pitch: False
 * /gbplanner_node/integration_order_mode: sorted
 * /gbplanner_node/max_consecutive_ray_collisions: 0
 * /gbplanner_node/max_ray_length_m: 50.0
 * /gbplanner_node/max_weight: 200
 * /gbplanner_node/mesh_min_weight: 1e-4
 * /gbplanner_node/method: fast
 * /gbplanner_node/min_ray_length_m: 0.5
 * /gbplanner_node/min_time_between_msgs_sec: 0.0
 * /gbplanner_node/occupancy_min_distance_voxel_size_factor: 1.0
 * /gbplanner_node/pointcloud_queue_size: 1000
 * /gbplanner_node/publish_esdf_map: True
 * /gbplanner_node/publish_pointclouds: True
 * /gbplanner_node/publish_slices: False
 * /gbplanner_node/publish_traversable: True
 * /gbplanner_node/publish_tsdf_info: False
 * /gbplanner_node/publish_tsdf_map: True
 * /gbplanner_node/slice_level: 1.0
 * /gbplanner_node/sparsity_compensation_factor: 100.0
 * /gbplanner_node/timestamp_tolerance_sec: 0.001
 * /gbplanner_node/traversability_radius: 0.4
 * /gbplanner_node/truncation_distance: 0.6
 * /gbplanner_node/tsdf_voxel_size: 0.2
 * /gbplanner_node/tsdf_voxels_per_side: 16
 * /gbplanner_node/update_mesh_every_n_sec: 0.5
 * /gbplanner_node/use_const_weight: False
 * /gbplanner_node/use_freespace_pointcloud: True
 * /gbplanner_node/use_sparsity_compensation_factor: True
 * /gbplanner_node/use_symmetric_weight_drop_off: False
 * /gbplanner_node/use_tf_transforms: True
 * /gbplanner_node/use_weight_dropoff: True
 * /gbplanner_node/verbose: False
 * /gbplanner_node/voxel_carving_enabled: True
 * /gbplanner_node/weight_ray_by_range: False
 * /gbplanner_node/world_frame: world
 * /message_to_tf/frame_id: world
 * /message_to_tf/odometry_topic: ground_truth/state
 * /pci_general_ros_node/RobotDynamics/dt: 0.05
 * /pci_general_ros_node/RobotDynamics/v_homing_max: 0.3
 * /pci_general_ros_node/RobotDynamics/v_init_max: 0.3
 * /pci_general_ros_node/RobotDynamics/v_max: 0.3
 * /pci_general_ros_node/RobotDynamics/yaw_rate_max: 0.5
 * /pci_general_ros_node/init_motion/x_forward: 3.0
 * /pci_general_ros_node/init_motion/z_drop: 1.7
 * /pci_general_ros_node/init_motion/z_takeoff: 2.5
 * /pci_general_ros_node/init_motion_enable: False
 * /pci_general_ros_node/output_type: kAction
 * /pci_general_ros_node/planner_trigger_lead_time: 0.0
 * /pci_general_ros_node/robot_type: kGround
 * /pci_general_ros_node/run_mode: kSim
 * /pci_general_ros_node/smooth_heading_enable: True
 * /pci_general_ros_node/trigger_mode: kManual
 * /robot_description: <?xml version="1....
 * /rosdistro: noetic
 * /rosversion: 1.15.14
 * /smb_front_left_wheel_velocity_controller/base_frame_id: base_link
 * /smb_front_left_wheel_velocity_controller/cmd_vel_timeout: 0.25
 * /smb_front_left_wheel_velocity_controller/has_acceleration_limits: True
 * /smb_front_left_wheel_velocity_controller/has_velocity_limits: True
 * /smb_front_left_wheel_velocity_controller/joint: LF_WHEEL
 * /smb_front_left_wheel_velocity_controller/max_acceleration: 3.0
 * /smb_front_left_wheel_velocity_controller/max_velocity: 3.0
 * /smb_front_left_wheel_velocity_controller/pid/d: 10.0
 * /smb_front_left_wheel_velocity_controller/pid/i: 0.01
 * /smb_front_left_wheel_velocity_controller/pid/p: 100.0
 * /smb_front_left_wheel_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_front_left_wheel_velocity_controller/publish_rate: 50
 * /smb_front_left_wheel_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_front_left_wheel_velocity_controller/type: velocity_controll...
 * /smb_front_left_wheel_velocity_controller/velocity_rolling_window_size: 2
 * /smb_front_right_wheel_velocity_controller/base_frame_id: base_link
 * /smb_front_right_wheel_velocity_controller/cmd_vel_timeout: 0.25
 * /smb_front_right_wheel_velocity_controller/has_acceleration_limits: True
 * /smb_front_right_wheel_velocity_controller/has_velocity_limits: True
 * /smb_front_right_wheel_velocity_controller/joint: RF_WHEEL
 * /smb_front_right_wheel_velocity_controller/max_acceleration: 3.0
 * /smb_front_right_wheel_velocity_controller/max_velocity: 3.0
 * /smb_front_right_wheel_velocity_controller/pid/d: 10.0
 * /smb_front_right_wheel_velocity_controller/pid/i: 0.01
 * /smb_front_right_wheel_velocity_controller/pid/p: 100.0
 * /smb_front_right_wheel_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_front_right_wheel_velocity_controller/publish_rate: 50
 * /smb_front_right_wheel_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_front_right_wheel_velocity_controller/type: velocity_controll...
 * /smb_front_right_wheel_velocity_controller/velocity_rolling_window_size: 2
 * /smb_joint_publisher/publish_rate: 50
 * /smb_joint_publisher/type: joint_state_contr...
 * /smb_path_tracker_node/critical_angle: 0.52
 * /smb_path_tracker_node/desired_vel: 0.5
 * /smb_path_tracker_node/look_ahead_distance: 0.5
 * /smb_path_tracker_node/look_ahead_error_margin: 0.2
 * /smb_path_tracker_node/max_ang_vel: 0.5
 * /smb_rear_left_wheel_velocity_controller/base_frame_id: base_link
 * /smb_rear_left_wheel_velocity_controller/cmd_vel_timeout: 0.25
 * /smb_rear_left_wheel_velocity_controller/has_acceleration_limits: True
 * /smb_rear_left_wheel_velocity_controller/has_velocity_limits: True
 * /smb_rear_left_wheel_velocity_controller/joint: LH_WHEEL
 * /smb_rear_left_wheel_velocity_controller/max_acceleration: 3.0
 * /smb_rear_left_wheel_velocity_controller/max_velocity: 3.0
 * /smb_rear_left_wheel_velocity_controller/pid/d: 10.0
 * /smb_rear_left_wheel_velocity_controller/pid/i: 0.01
 * /smb_rear_left_wheel_velocity_controller/pid/p: 100.0
 * /smb_rear_left_wheel_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_rear_left_wheel_velocity_controller/publish_rate: 50
 * /smb_rear_left_wheel_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_rear_left_wheel_velocity_controller/type: velocity_controll...
 * /smb_rear_left_wheel_velocity_controller/velocity_rolling_window_size: 2
 * /smb_rear_right_wheel_velocity_controller/base_frame_id: base_link
 * /smb_rear_right_wheel_velocity_controller/cmd_vel_timeout: 0.25
 * /smb_rear_right_wheel_velocity_controller/has_acceleration_limits: True
 * /smb_rear_right_wheel_velocity_controller/has_velocity_limits: True
 * /smb_rear_right_wheel_velocity_controller/joint: RH_WHEEL
 * /smb_rear_right_wheel_velocity_controller/max_acceleration: 3.0
 * /smb_rear_right_wheel_velocity_controller/max_velocity: 3.0
 * /smb_rear_right_wheel_velocity_controller/pid/d: 10.0
 * /smb_rear_right_wheel_velocity_controller/pid/i: 0.01
 * /smb_rear_right_wheel_velocity_controller/pid/p: 100.0
 * /smb_rear_right_wheel_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_rear_right_wheel_velocity_controller/publish_rate: 50
 * /smb_rear_right_wheel_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_rear_right_wheel_velocity_controller/type: velocity_controll...
 * /smb_rear_right_wheel_velocity_controller/velocity_rolling_window_size: 2
 * /smb_teleop_twist_joy_node/max_velocity_angular: 1.5
 * /smb_teleop_twist_joy_node/max_velocity_linear: 1
 * /smb_velocity_controller/angular/z/has_acceleration_limits: True
 * /smb_velocity_controller/angular/z/has_velocity_limits: True
 * /smb_velocity_controller/angular/z/max_acceleration: 2
 * /smb_velocity_controller/angular/z/max_velocity: 1
 * /smb_velocity_controller/base_frame_id: base_link
 * /smb_velocity_controller/cmd_vel_timeout: 0.25
 * /smb_velocity_controller/enable_odom_tf: False
 * /smb_velocity_controller/estimate_velocity_from_position: False
 * /smb_velocity_controller/left_wheel: ['LF_WHEEL', 'LH_...
 * /smb_velocity_controller/linear/x/has_acceleration_limits: True
 * /smb_velocity_controller/linear/x/has_velocity_limits: True
 * /smb_velocity_controller/linear/x/max_acceleration: 3
 * /smb_velocity_controller/linear/x/max_velocity: 1.5
 * /smb_velocity_controller/pid/d: 10.0
 * /smb_velocity_controller/pid/i: 0.01
 * /smb_velocity_controller/pid/p: 100.0
 * /smb_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_velocity_controller/publish_rate: 50
 * /smb_velocity_controller/right_wheel: ['RF_WHEEL', 'RH_...
 * /smb_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /smb_velocity_controller/type: diff_drive_contro...
 * /smb_velocity_controller/velocity_rolling_window_size: 2
 * /smb_velocity_controller/wheel_radius_multiplier: 1.0
 * /smb_velocity_controller/wheel_separation: 0.6604
 * /smb_velocity_controller/wheel_separation_multiplier: 1.0
 * /twist_mux/locks: [{'name': 'e_stop...
 * /twist_mux/topics: [{'name': 'joy', ...
 * /use_sim_time: True

NODES
  /
    controller_spawner (controller_manager/spawner)
    front_lidar_relay (topic_tools/relay)
    gazebo (gazebo_ros/gzserver)
    gbplanner_node (gbplanner/gbplanner_node)
    gbplanner_ui (rviz/rviz)
    joy_node (joy/joy_node)
    message_to_tf (smb_gazebo/message_to_tf)
    odom_to_world_tf (tf/static_transform_publisher)
    pci_general_ros_node (pci_general/pci_general_ros_node)
    pose_throttler (topic_tools/throttle)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    smb_path_tracker_node (smb_path_tracker/smb_path_tracker_ros_node)
    smb_teleop_twist_joy_node (smb_teleop_twist_joy/smb_teleop_twist_joy_node)
    spawn_smb_model (gazebo_ros/spawn_model)
    tf_53 (tf/static_transform_publisher)
    twist_marker_server (interactive_marker_twist_server/marker_server)
    twist_mux (twist_mux/twist_mux)
    vlp_relay (topic_tools/relay)

auto-starting new master
process[master]: started with pid [736558]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to c189b720-726c-11ed-9f3d-035f770cbc42
process[rosout-1]: started with pid [736587]
started core service [/rosout]
process[tf_53-2]: started with pid [736594]
process[gazebo-3]: started with pid [736595]
process[spawn_smb_model-4]: started with pid [736598]
process[robot_state_publisher-5]: started with pid [736601]
process[message_to_tf-6]: started with pid [736607]
process[odom_to_world_tf-7]: started with pid [736608]
process[controller_spawner-8]: started with pid [736609]
process[twist_mux-9]: started with pid [736618]
process[twist_marker_server-10]: started with pid [736624]
process[joy_node-11]: started with pid [736626]
process[smb_teleop_twist_joy_node-12]: started with pid [736632]
process[smb_path_tracker_node-13]: started with pid [736639]
process[vlp_relay-14]: started with pid [736664]
process[front_lidar_relay-15]: started with pid [736667]
process[pose_throttler-16]: started with pid [736673]
process[gbplanner_node-17]: started with pid [736674]
process[pci_general_ros_node-18]: started with pid [736682]
[ INFO] [1670004695.483485043]: [twist_marker_server] Initialized.
process[gbplanner_ui-19]: started with pid [736684]
[ERROR] [1670004695.492176116]: Couldn't open joystick /dev/input/js0. Will retry every second.
[ INFO] [1670004695.643547166]: rviz version 1.14.19
[ INFO] [1670004695.643586960]: compiled against Qt version 5.12.8
[ INFO] [1670004695.643597614]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1670004695.651919564]: Forcing OpenGl version 0.
/gbplanner_node/SensorParams/VLP16 0, 0, 0
[ INFO] [1670004695.857957564]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1670004695.858948939]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1670004695.984320833]: Stereo is NOT SUPPORTED
[ INFO] [1670004695.984363630]: OpenGL device: NVIDIA GeForce RTX 3060 Laptop GPU/PCIe/SSE2
[ INFO] [1670004695.984373301]: OpenGl version: 4.6 (GLSL 4.6).
[ INFO] [1670004696.247503641]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1670004696.254953679]: Physics dynamic reconfigure ready.
Service [///shininess] is not valid.
Topic [default///base_link/camera1/image] is not valid.
[ INFO] [1670004698.391252063, 251.830000000]: Lidar laser plugin ready, 32 lasers
Topic [default///base_link/camera1/image] is not valid.
[ INFO] [1670004698.493617727, 251.830000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
Topic [default///base_link/camera1/image] is not valid.
[ INFO] [1670004698.495975652, 251.830000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1670004698.496052921, 251.830000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1670004698.496730382, 251.830000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1670004698.498986617, 251.830000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1670004698.499652560, 251.830000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
Topic [///joint_cmd] is not valid.
Service [///joint_cmd_req] is not valid.
[ INFO] [1670004698.524273546, 251.830000000]: Lidar GPU laser plugin ready, 16 lasers
[ INFO] [1670004698.609773848, 251.830000000]: Loading gazebo_ros_control plugin
[ INFO] [1670004698.609839667, 251.830000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1670004698.610737562, 251.830000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1670004698.721298764, 251.830000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/LF_WHEEL
[ERROR] [1670004698.721904971, 251.830000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/RF_WHEEL
[ERROR] [1670004698.722408338, 251.830000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/LH_WHEEL
[ERROR] [1670004698.722860239, 251.830000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/RH_WHEEL
[ INFO] [1670004698.727314585, 251.830000000]: Loaded gazebo_ros_control.
[ INFO] [1670004698.767677752, 251.850000000]: Controller state will be published at 50Hz.
[ INFO] [1670004698.768504302, 251.850000000]: Wheel separation will be multiplied by 1.
[ INFO] [1670004698.769068173, 251.860000000]: Left wheel radius will be multiplied by 1.
[ INFO] [1670004698.769096530, 251.860000000]: Right wheel radius will be multiplied by 1.
[ INFO] [1670004698.769386021, 251.860000000]: Velocity rolling window size of 2.
[ INFO] [1670004698.769896670, 251.860000000]: Velocity commands will be considered old if they are older than 0.25s.
[ INFO] [1670004698.770574253, 251.860000000]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1670004698.771141669, 251.860000000]: Base frame_id set to base_link
[ INFO] [1670004698.771462786, 251.860000000]: Odometry frame_id set to odom
[ INFO] [1670004698.772069996, 251.860000000]: Publishing to tf is disabled
[ INFO] [1670004698.779347315, 251.870000000]: Odometry params : wheel separation 0.6604, left wheel radius 0.19, right wheel radius 0.19
[ INFO] [1670004698.780605674, 251.870000000]: Adding left wheel with joint name: LF_WHEEL and right wheel with joint name: RF_WHEEL
[ INFO] [1670004698.780643537, 251.870000000]: Adding left wheel with joint name: LH_WHEEL and right wheel with joint name: RH_WHEEL
[ INFO] [1670004698.789939136, 251.880000000]: Dynamic Reconfigure:
DynamicParams:
	Odometry parameters:
		left wheel radius multiplier: 1
		right wheel radius multiplier: 1
		wheel separation multiplier: 1
	Publication parameters:
		Publish executed velocity command: disabled
		Publication rate: 50
		Publish frame odom on tf: disabled
[spawn_smb_model-4] process has finished cleanly
log file: /home/antoan/.ros/log/c189b720-726c-11ed-9f3d-035f770cbc42/spawn_smb_model-4*.log
[ INFO] [1670004752.476194525, 300.780000000]: Setting goal: Frame:world, Position(6.063, 0.310, 0.000), Orientation(0.000, 0.000, 0.346, 0.938) = Angle: 0.707

[ INFO] [1670004766.275938144, 313.280000000]: Setting goal: Frame:world, Position(6.227, 0.409, 0.000), Orientation(0.000, 0.000, -0.085, 0.996) = Angle: -0.171

[ INFO] [1670004784.759061604, 329.990000000]: Planning iteration 0
[ INFO] [1670004796.096076254, 340.400000000]: Planning iteration 1
[ERROR] [1670004801.100860797, 344.950000000]: Input pointcloud queue getting too long! Dropping some pointclouds. Either unable to look up transform timestamps or the processing is taking too long.
[ INFO] [1670004808.750320954, 352.000000000]: Planning iteration 2
[ INFO] [1670004830.968320079, 372.220000000]: Planning iteration 3
[ INFO] [1670004843.933968872, 384.090000000]: Planning iteration 4
[ INFO] [1670004856.066563520, 395.140000000]: Setting goal: Frame:world, Position(2.224, -0.193, 0.000), Orientation(0.000, 0.000, 0.002, 1.000) = Angle: 0.004

[ INFO] [1670004857.855551417, 396.800000000]: Planning iteration 5
[ERROR] [1670004869.408217314, 407.320000000]: Input pointcloud queue getting too long! Dropping some pointclouds. Either unable to look up transform timestamps or the processing is taking too long.
[ INFO] [1670004879.497433612, 416.470000000]: Planning iteration 6
[ INFO] [1670004892.457615414, 428.370000000]: Planning iteration 7
[ INFO] [1670004904.951218133, 439.840000000]: Planning iteration 8
[ INFO] [1670004920.757421893, 454.260000000]: Setting goal: Frame:world, Position(-4.752, -1.237, 0.000), Orientation(0.000, 0.000, -1.000, 0.009) = Angle: -3.123

[ INFO] [1670004932.165040894, 464.560000000]: SMB Path Tracker: Reached end of path
[ INFO] [1670004940.822883235, 472.400000000]: Planning iteration 9

gbplanner_node: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:364: Eigen::DenseCoeffsBase<Derived, 1>::Scalar& Eigen::DenseCoeffsBase<Derived, 1>::operator()(Eigen::Index, Eigen::Index) [with Derived = Eigen::Matrix<double, -1, -1>; Eigen::DenseCoeffsBase<Derived, 1>::Scalar = double; Eigen::Index = long int]: Assertion `row >= 0 && row < rows() && col >= 0 && col < cols()' failed.
*** Aborted at 1670004946 (unix time) try "date -d @1670004946" if you are using GNU date ***
PC: @     0x7f0eae24e00b gsignal
*** SIGABRT (@0x3e8000b3da2) received by PID 736674 (TID 0x7f0ea2980840) from PID 736674; stack trace: ***
    @     0x7f0eb001b781 google::(anonymous namespace)::FailureSignalHandler()
    @     0x7f0eae24e090 (unknown)
    @     0x7f0eae24e00b gsignal
    @     0x7f0eae22d859 abort
    @     0x7f0eae22d729 (unknown)
    @     0x7f0eae23efd6 __assert_fail
    @     0x7f0eaf47bc11 Eigen::DenseCoeffsBase<>::operator()()
    @     0x7f0eaf47a234 _ZN17MapManagerVoxbloxIN7voxblox10TsdfServerENS0_9TsdfVoxelEE13getScanStatusERN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEERSt6vectorIS6_SaIS6_EERSt5tupleIJiiiEERS8_ISt4pairIS6_N10MapManager11VoxelStatusEESaISI_EER16SensorParamsBase
    @     0x7f0eafbc3017 explorer::Rrg::computeVolumetricGainRayModel()
    @     0x7f0eafbba192 explorer::Rrg::expandGlobalGraphTimerCallback()
    @     0x7f0eafc310fb boost::_mfi::mf1<>::operator()()
    @     0x7f0eafc29371 boost::_bi::list2<>::operator()<>()
    @     0x7f0eafc2229d boost::_bi::bind_t<>::operator()<>()
    @     0x7f0eafc1ab2e boost::detail::function::void_function_obj_invoker1<>::invoke()
    @     0x7f0eaff07608 ros::TimerManager<>::TimerQueueCallback::call()
    @     0x7f0eaff29172 ros::CallbackQueue::callOneCB()
    @     0x7f0eaff2a883 ros::CallbackQueue::callAvailable()
    @     0x7f0eaff7dfcf ros::SingleThreadedSpinner::spin()
    @     0x7f0eaff6621f ros::spin()
    @     0x557a276b8a3a main
    @     0x7f0eae22f083 __libc_start_main
    @     0x557a276b871e _start
[ERROR] [1670004946.273309155, 477.380000000]: Planner service failed
[gbplanner_node-17] process has died [pid 736674, exit code -6, cmd /home/antoan/dev/gbplanner2_ws/devel/lib/gbplanner/gbplanner_node odometry:=/ground_truth/state /pointcloud:=/input_pointcloud __name:=gbplanner_node __log:=/home/antoan/.ros/log/c189b720-726c-11ed-9f3d-035f770cbc42/gbplanner_node-17.log].
log file: /home/antoan/.ros/log/c189b720-726c-11ed-9f3d-035f770cbc42/gbplanner_node-17*.log
[ INFO] [1670004946.806100546, 477.890000000]: Planning iteration 9
[ERROR] [1670004946.806156020, 477.890000000]: Planner service failed
`

Exploration Demo Not Working

Hi,
I have encountered issue when running demo simulations on ROS Melodic (Ubuntu 18.04) following instructions
After clicking "Initialization" and "Start Planner" in order,
the exploration planner never leads vehicle to out of starting location.
It just goes back and forth in starting area
This behavior is similar in case of 'smb_sim' too.

How can I make exploration running?

Only console output to note for both cases
[GBPLANNER] Very low local gain. Triggering global planner

I'm using latest gbplanner2 branch of this repository,
resolved few compile issue other than that it is default as is.

Case - $ roslaunch gbplanner rmf_sim.launch after 400 secs of run

Case - $ roslaunch gbplanner smb_sim.launch after 400 secs of run

command interface using px4

I am trying to use this planner with px4 as I would like to simulate a drone I will later build.
I was looking for the actual commands sent to the drone, which are usually passed as mavros_msgs/AttitudeTarget or as trajectory_msgs/MultiDOFJointTrajectoryPoint.
Analyzing the topic /rmf_obelix/command/trajectory I found that there a message of the latter type is published in it, but at a very slow rate and probably not for the purpose at which such messages are normally published, so I have a doubt about it. Is it used to keep a rough track of

/rmf_obelix/command/motor_speed instead publishes a message of type mav_msgs/Actuators direclty to gazebo, which I understand is the used way to move the drone.

now, is there a way to get the planner to output one this type of messages among the two mentioned above?

Planning with legged robots

Good evening,

Thank you very much for your work. I've read both your papers and the youtube video detailing the entire exploration algorithm. It is stated that this autonomous exploration algorithm can also worked for legged robots, even though the demonstrations were mainly for quadcopters. I wonder if there are specific parameters for me to set if I were to want the RRT local planner to generate paths only tangential to the ground (which the legged robot can walk on), instead of paths that are midair that are aimed more for quadcopters? Thanks!

libvolumetric_map_base.so: undefined reference to `cv::Mat::updateContinuityFlag()'

Hi, I followed your tutorial, made a gbplanner_ws workspace & tried to complie it (with and without octomapping). However, in both cases, the commands seem like having a mismatch in opencv version or something similar.
I follwed the following path:

git clone https://github.com/unr-arl/gbplanner_ws
cd gbplanner_ws
wstool init
wstool merge packages_https.rosinstall
wstool update
catkin config -DCMAKE_BUILD_TYPE=Release
catkin build

I am using Ubuntu 18.04 with ROS melodic running and an opencv version of 3.4.8. Here is the log file:
build.make.003.log.

Error displayed on command prompt:

arghya@arghya-Erazer-X7849-MD60379:~/gbplanner_ws$ catkin build -DCMAKE_BUILD_TYPE=Release gbplanner_ros
-------------------------------------------------------------------------------------------------------
Profile:                     default
Extending:          [cached] /home/arghya/catkin_ws/devel:/home/arghya/ws_moveit/devel:/opt/ros/melodic
Workspace:                   /home/arghya/gbplanner_ws
-------------------------------------------------------------------------------------------------------
Build Space:        [exists] /home/arghya/gbplanner_ws/build
Devel Space:        [exists] /home/arghya/gbplanner_ws/devel
Install Space:      [unused] /home/arghya/gbplanner_ws/install
Log Space:          [exists] /home/arghya/gbplanner_ws/logs
Source Space:       [exists] /home/arghya/gbplanner_ws/src
DESTDIR:            [unused] None
-------------------------------------------------------------------------------------------------------
Devel Space Layout:          linked
Install Space Layout:        None
-------------------------------------------------------------------------------------------------------
Additional CMake Args:       -DCMAKE_BUILD_TYPE=Release -DCMAKE_BUILD_TYPE=Release -DUSE_OCTOMAP=1
Additional Make Args:        None
Additional catkin Make Args: None
Internal Make Job Server:    True
Cache Job Environments:      False
-------------------------------------------------------------------------------------------------------
Whitelisted Packages:        None
Blacklisted Packages:        None
-------------------------------------------------------------------------------------------------------
Workspace configuration appears valid.

NOTE: Forcing CMake to run for each package.
-------------------------------------------------------------------------------------------------------
[build] Found '41' packages in 0.0 seconds.                                                                                                             
[build] Package table is up to date.                                                                                                                    
Starting  >>> catkin_simple                                                                                                                             
Starting  >>> planner_gazebo_sim                                                                                                                        
Starting  >>> planner_msgs                                                                                                                              
Finished  <<< planner_gazebo_sim                       [ 0.9 seconds ]                                                                                  
Finished  <<< catkin_simple                            [ 1.0 seconds ]                                                                                  
Starting  >>> eigen_catkin                                                                                                                              
Starting  >>> gflags_catkin                                                                                                                             
Starting  >>> kdtree                                                                                                                                    
Starting  >>> volumetric_msgs                                                                                                                           
Starting  >>> voxblox_msgs                                                                                                                              
Finished  <<< kdtree                                   [ 1.2 seconds ]                                                                                  
Finished  <<< gflags_catkin                            [ 1.2 seconds ]                                                                                  
Finished  <<< eigen_catkin                             [ 1.2 seconds ]                                                                                  
Starting  >>> glog_catkin                                                                                                                               
Finished  <<< planner_msgs                             [ 2.6 seconds ]                                                                                  
Starting  >>> planner_control_interface                                                                                                                 
Finished  <<< voxblox_msgs                             [ 1.8 seconds ]                                                                                  
Finished  <<< volumetric_msgs                          [ 1.8 seconds ]                                                                                  
Finished  <<< glog_catkin                              [ 1.0 seconds ]                                                                                  
Starting  >>> eigen_checks                                                                                                                              
Finished  <<< planner_control_interface                [ 1.5 seconds ]                                                                                  
Finished  <<< eigen_checks                             [ 1.0 seconds ]                                                                                  
Starting  >>> minkindr                                                                                                                                  
Finished  <<< minkindr                                 [ 0.7 seconds ]                                                                                  
Starting  >>> minkindr_conversions                                                                                                                      
Starting  >>> volumetric_map_base                                                                                                                       
Starting  >>> voxblox                                                                                                                                   
________________________________________________________________________________________________________________________________________________________
Warnings   << voxblox:cmake /home/arghya/gbplanner_ws/logs/voxblox/build.cmake.003.log                                                                  
INSTALL TARGETS - target voxblox_proto has PUBLIC_HEADER files but no PUBLIC_HEADER DESTINATION.
CMake Warning at /opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:418 (message):
  catkin_package() include dir '/home/arghya/gbplanner_ws/build/voxblox'
  should be placed in the devel space instead of the build space
Call Stack (most recent call first):
  /opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
  /home/arghya/gbplanner_ws/devel/share/catkin_simple/cmake/catkin_simple-extras.cmake:214 (catkin_package)
  CMakeLists.txt:173 (cs_export)


cd /home/arghya/gbplanner_ws/build/voxblox; catkin build --get-env voxblox | catkin env -si  /usr/bin/cmake /home/arghya/gbplanner_ws/src/mapping/voxblox/voxblox --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/arghya/gbplanner_ws/devel/.private/voxblox -DCMAKE_INSTALL_PREFIX=/home/arghya/gbplanner_ws/install -DCMAKE_BUILD_TYPE=Release -DCMAKE_BUILD_TYPE=Release -DUSE_OCTOMAP=1; cd -
........................................................................................................................................................
Finished  <<< minkindr_conversions                     [ 1.1 seconds ]                                                                                  
Finished  <<< voxblox                                  [ 1.2 seconds ]                                                                                  
Starting  >>> voxblox_rviz_plugin                                                                                                                       
Finished  <<< volumetric_map_base                      [ 2.7 seconds ]                                                                                  
Starting  >>> octomap_world                                                                                                                             
Finished  <<< voxblox_rviz_plugin                      [ 1.9 seconds ]                                                                                  
Starting  >>> voxblox_ros                                                                                                                               
Finished  <<< voxblox_ros                              [ 3.5 seconds ]                                                                                  
________________________________________________________________________________________________________________________________________________________
Errors     << octomap_world:make /home/arghya/gbplanner_ws/logs/octomap_world/build.make.003.log                                                        
/home/arghya/gbplanner_ws/devel/.private/volumetric_map_base/lib/libvolumetric_map_base.so: undefined reference to `cv::Mat::updateContinuityFlag()'
collect2: error: ld returned 1 exit status
make[2]: *** [/home/arghya/gbplanner_ws/devel/.private/octomap_world/lib/octomap_world/octomap_manager] Error 1
make[1]: *** [CMakeFiles/octomap_manager.dir/all] Error 2
make: *** [all] Error 2
cd /home/arghya/gbplanner_ws/build/octomap_world; catkin build --get-env octomap_world | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -
........................................................................................................................................................
Failed     << octomap_world:make                       [ Exited with code 2 ]                                                                           
Failed    <<< octomap_world                            [ 4.7 seconds ]                                                                                  
Abandoned <<< gbplanner                                [ Unrelated job failed ]                                                                         
Abandoned <<< gbplanner_ros                            [ Unrelated job failed ]                                                                         
[build] Summary: 17 of 20 packages succeeded.                                                                                                           
[build]   Ignored:   21 packages were skipped or are blacklisted.                                                                                       
[build]   Warnings:  1 packages succeeded with warnings.                                                                                                
[build]   Abandoned: 2 packages were abandoned.                                                                                                         
[build]   Failed:    1 packages failed.                                                                                                                 
[build] Runtime: 13.2 seconds total.

D435i integration with gbplanner sim

HI,
I am trying to integrate a D435i with the gbplanner sim. I have already tried changing the FOV and resolution in gbplanner_config.yaml as you had suggested in a earlier post.
SensorParams:
sensor_list: ["OS064"]
OS064:
type: kCamera
CameraType: kFixed
max_range: 2.0
center_offset: [0.0, 0.0, 0.0]
rotations: [0, 0, 0]
fov: [rad(85pi/180), rad(60pi/180)]
resolution: [rad(7pi/180), rad(7pi/180)]
frontier_percentage_threshold: 0.04
The simulation runs without issues but the point cloud viz on rviz is still a 360 degree FOV depth map like:

How do i proceed to include D435i with the simulations for testing.

Exploration in Outdoor Environments

Hi,

I've studied both your papers and your codebase for awhile now. Amazing work here! I've 2 questions:

(1) Performance in outdoor environments
From my understanding, the algorithm works on choosing paths with highest exploration gain, which is a function of volumetric gain, while minimizing heading changes and path distance.

I would like to ask if your algorithm has been tested in outdoor environments? For instance, an area with many buildings (e.g. University campus, residential complex), or nature areas (e.g. forest)? How efficient is the path taken by the robot in mapping such an environment with many possible branching paths?

(2) Multi-Agent Exploration
Can your algorithm be used by multiple robots to perform concurrent exploration? Perhaps the map can be shared in real time between the robots for faster exploration completion, and the different robots can take charge of exploring different areas?

Thanks alot,
Derek

gazebo 9 world process died & librotors_gazebo_multirotor_base_plugin.so: undefined symbol

I am using Ubuntu 20.04 with ROS Noetic to run this package. I was able to run the ground robot simulation successfully. However, the drone simulation was not. I followed the instructions in the readme file. I installed the models in ./gazebo/models folder. Here is the output of the terminal.

/gbplanner_node/SensorParams/OS064 0, 0, 0
[INFO] [1656051334.597383, 0.000000]: Loading model XML from ros parameter robot_description
[INFO] [1656051334.602214, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1656051334.834050629]: Stereo is NOT SUPPORTED
[ INFO] [1656051334.834121696]: OpenGL device: Quadro T2000 with Max-Q Design/PCIe/SSE2
[ INFO] [1656051334.834148694]: OpenGl version: 4.6 (GLSL 4.6).
[ INFO] [1656051335.044938827]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1656051335.046637313]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1656051335.781775331]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[INFO] [1656051335.807447, 0.000000]: Calling service /gazebo/spawn_urdf_model
[INFO] [1656051368.293094, 875.948000]: Spawn status: SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name rmf_obelix
[ERROR] [1656051368.294511, 875.948000]: Spawn service failed. Exiting.
[ INFO] [1656051368.301889779, 875.948000000]: Physics dynamic reconfigure ready.
[rmf_obelix/spawn_rmf_obelix-12] process has died [pid 515779, exit code 1, cmd /opt/ros/noetic/lib/gazebo_ros/spawn_model -param robot_description -urdf -x 0.0 -y 0.0 -z 0.5 -model rmf_obelix __name:=spawn_rmf_obelix __log:=/home/tii/.ros/log/0dc60e52-f385-11ec-bc0b-586c255f72c4/rmf_obelix-spawn_rmf_obelix-12.log].
log file: /home/tii/.ros/log/0dc60e52-f385-11ec-bc0b-586c255f72c4/rmf_obelix-spawn_rmf_obelix-12*.log
[ INFO] [1656051371.453124837, 875.948000000]: Lidar laser plugin ready, 32 lasers
gzserver: symbol lookup error: /home/tii/gbplanner2_ws/devel/lib/librotors_gazebo_multirotor_base_plugin.so: undefined symbol: _ZN14gz_sensor_msgs9ActuatorsC1Ev
[gazebo-9] process has died [pid 515762, exit code 127, cmd /opt/ros/noetic/lib/gazebo_ros/gzserver -e ode /home/tii/gbplanner2_ws/src/exploration/gbplanner_ros/planner_gazebo_sim/worlds/darpa_subt_final_circuit.world __name:=gazebo __log:=/home/tii/.ros/log/0dc60e52-f385-11ec-bc0b-586c255f72c4/gazebo-9.log].
log file: /home/tii/.ros/log/0dc60e52-f385-11ec-bc0b-586c255f72c4/gazebo-9*.log

How can I fix this error
gbplanner2_ws/devel/lib/librotors_gazebo_multirotor_base_plugin.so: undefined symbol: _ZN14gz_sensor_msgs9ActuatorsC1Ev

and this issue

cmd /opt/ros/noetic/lib/gazebo_ros/gzserver -e ode /home/tii/gbplanner2_ws/src/exploration/gbplanner_ros/planner_gazebo_sim/worlds/darpa_subt_final_circuit.world __name:=gazebo __log:=/home/tii/.ros/log/065c621e-f38b-11ec-ba36-586c255f72c4/gazebo-9.log].
log file: /home/tii/.ros/log/065c621e-f38b-11ec-ba36-586c255f72c4/gazebo-9*.log

Kindly assist.

protobuf issues

Build is failing with this and other errors that are similar. error: no matching function for call to ‘google::protobuf::internal::ArenaStringPtr::GetNoArena(const string*) const’
I looked this up and it seems its a version mismatch from what I have on my machine and the version that you used to develop the software. Which version of protobuf did you use ?

How to modify the launch file to integrate into my robot system

Hi,

Thank you very much for sharing this awesome planner! I want to integrate this to my own robot system and I have already red the issue https://github.com/ntnu-arl/gbplanner_ros/issues/36 here, its very helpful. How can I modify the launch file? You mean I only need the pci and gbplanner package in the launch file, so this is my launch file, is this correct?

Thank you very much!

Multiple MAVs explore the same environment using GBplanner separately

Hi @MihirDharmadhikari
Recently, I'm trying to add another MAV in simulation and then two MAVs can explore the same environment separately(from different starting point).
The link of launch file which I'm using is as follows:
https://github.com/Rye450/gbplanner_test/blob/1583caafbd70fc93400b224723fe6b64b8da025d/byl_two.launch

However,the belowing terminal output shows that the second MAV seems to plan much slower than the first one:

The planner of the first MAV has already planned for 100 times,while the planner of the second MAV has only planned for 5 times.
The second MAV stays still all the time while the first MAV has already moved for a long distance in gazebo as follows:

I have already checked the tf tree,there are transformations between the sensor frame of two MAVs and the fixed frame.

Could you tell me what's wrong with my launch file?

Looking forward to your reply!

Add Custom Ground Model and d435i Realsense Camera

Hello, I want to integrate my own robot into the system. And I also have realsense2_camera. I also want to get my point cloud data accordingly. For this I updated the /input_pointcloud locations for my camera. I also gave my model in xacro format. But I couldn't get the output in RVIZ environment. Can you give information about these? Can I run d435i realsense_camera on the system? And do I need to do anything external for my model other than giving the path to my model file in the add model part of your project?

When homing, the robot doesn't return exactly to the starting position.

I assume the position after initialization is the starting position for the planner. The robot will only return to a position approximately half to one meter down the path from the starting position. I guess that's where the first node of the planning graph is and that the starting position is not part of the planning graph, thus not on the homing path?
I will look into the code. In the meantime, if someone has any idea of the cause, please let me know.

Uninitialized quaternion, assuming identity

I tried using the gbplanner in a different simulation environment and a different UAV robot. The input to the gbplanner is the point cloud topic and the odom topic. While the output is the set of points (trajectory). I created a node to read all the trajectory points and send them to the controller of my drone. The drone was able to follow the path. However, the path generated is too close to the wall, and the drone sometimes hits the walls. In addition, I got the following warnings and errors in RVIZ for both the best and the ref paths and other tools:
Uninitialized quaternion, assuming identity

Here is the output from RVIZ:

Also, When I pressed Go home. It created the correct path as visualized, but it landed in a different place as shown in the following Image.

I can provide the terminal output as well.

Kindly let me know how to fix or enhance the performance of the planner on a different drone type.

How to deploy code to a physical quadruped robot

Hello,think you for your work.
Now I want to deploy it on a physical robot, and I want to know how to modify it？
Can you give me some advise?

Custom Maps for indoor simulations

Hi,
I am trying to simulate the gbplanner sim for a custom indoor environment that I have created with Gazebo. I made the world file model and replaced the world file name in the launch file. The Gazebo sim and drone spawn correctly but nothing comes up in rviz and the planner interface is unresponsive and getting the following error:
"put pointcloud queue getting too long! Dropping some pointclouds. Either unable to look up transform timestamps or the processing is taking too long."
the following is the gazebo environment created [name modified to .txt to upload]

hey
I was able to get it working with the custom world. I had missed the ros_gazebo_plugin command.

ntnu-arl / gbplanner_ros Goto Github PK

gbplanner_ros's People

Contributors

Stargazers

Watchers

Forkers

gbplanner_ros's Issues

Recommend Projects

Recommend Topics

Recommend Org