Unable to set goal pose and get trajectory

ajay1606 commented 4 months ago

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

MihirDharmadhikari commented 4 months ago

Hi @ajay1606 ,

The highlighted box (the polygon) is called a no-go-zone or geofence. The planner will not span the graph in this area. If you want to disable or change it you can do it in the config file here:

This parameter will enable or disable the use of this functionality.
You can set a custom geofence here.

Regarding the red box. This is the no-gain-zone. No voxels in this volume will be counted for the volumetric gain calculation. You can disable it from here. If you simply comment out that parameter it will be disabled.

Since this is an exploration planner, the UI does not directly provide the functionality to set a goal point and get a path to it. There are two ways you can do it:

If you have triggered the exploration (by clicking on start planner in the UI) and the robot has explored some part of the map, then you can set a waypoint for it to plan to along the global graph as described here. The planner will plan a path to the closest vertex in the global graph.
The planner has a function implemented to find a path to a goal point. This is the function: (link). You can write a service that takes the goal position and calls this function. The volume in which the planner samples to calculate this path is set here.

Let me know if this helps.

Best, Mihir

ajay1606 commented 4 months ago

@MihirDharmadhikari Excellent, really very helpful to understand more better. But currently i have tried as it instructed in the document here.

But path trajectory not showing up in the rviz, as it shows while running simulation mode.

In case of simualtion: (Launch planner -> Initialization in RVIZ GUI -> Start Planner in RVIZ GUI

https://github.com/ntnu-arl/gbplanner_ros/assets/29722843/5d005c56-acfc-43a3-b2b3-b3bf5d4a6a3d

In case of rosbag: (Launch planner -> Initialization in RVIZ GUI -> Start Planner in RVIZ GUI

https://github.com/ntnu-arl/gbplanner_ros/assets/29722843/474aae88-6841-429f-a4f5-86e5e5e50cd4

Why I am unable to see explored trajectory in real data case ! would you please help. Greatly appreciate any response.

My launch file looks like this !

<launch>
  <!-- All settings -->
  <arg name="robot_name" default="smb"/>
  <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="true"/>

  <!-- 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/niosh_osrf.world"/> -->
  <!-- <arg name="world_file" default="$(find planner_gazebo_sim)/worlds/darpa_cave_01.world"/> -->
  <!-- <arg name="world_file" default="$(find planner_gazebo_sim)/worlds/pittsburgh_mine.world"/> -->

  <!-- Static TF -->
  <node pkg="tf" type="static_transform_publisher" name="robot_world_link" args="0 0 0 0 0 0 world navigation 100" />
  <!-- <node pkg="tf" type="static_transform_publisher" name="robot_world_link_" args="0 0 0 0 0 0 world odom 1" /> -->

  <include file="$(find smb_path_tracker)/launch/pure_pursuit_tracker.launch"/>

  <node pkg="topic_tools" type="relay" name="vlp_relay" args="/points_raw /input_pointcloud" />

  <node pkg="topic_tools" type="relay" name="front_lidar_relay" args="/points_raw /input_pointcloud" />

  <arg name="odometry_topic" default="/robot/odom"/>
  <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="odometry" to="/robot/odom" />
    <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/smb.rviz"/>

</launch>

MihirDharmadhikari commented 4 months ago

Hi @ajay1606 ,

From what I see in the video, I think the planner it trying to plan from the initial location where the robot was. Since you are replaying a rosbag and the robot won't follow the path that the planner has planned, the planner won't trigger the next planning iteration automatically. To test the exploration path I would suggest you do the following:

At the instance when you want to trigger the planner, click on stop planner.
Click start planner. The planner will plan the path from the current robot location. If the planner still does not plan and show a path, check in rviz where the green box is. That is the location from which the planner is attempting to plan the path. Additionally, you can change the verbosity of the planner here. Set it to Verbosity::INFO and recompile before you test. This will print a lot more information in the terminal about what is happening with the planner. If the planner does not plan, send me the terminal log as well.

Hope this helps.

Best, Mihir

ajay1606 commented 4 months ago

@MihirDharmadhikari Thank you so much for the very valuable inputs, I was eagerly looking for your response. I will try as you suggested.

Which topic publishes the actual planner trajectory output ? To Subscribe robots control algorithm ! Checked with robot/command/trajectory, in simulation mode. But the topic not publishing any data.

Am I looking at wrong topic ?

MihirDharmadhikari commented 4 months ago

Hi @ajay1606 ,

The planner operates in two modes kTopic and kAction. If you are using the kAction mode, it will act as an action server you can find more details about what are the action names etc. it in the wiki. But if you just want a publisher that publishes the trajectory (this is the easy way, the kAction is there in case the path follower is setup to work with ROS actions) use the kTopic mode. You can set this here, set the parameter to kTopic. Then the trajectory will be published on the robot/command/trajectory topic. The message type is trajectory_msgs/MultiDOFJointTrajectory. Additionally, it will publish a nav_msgs/Path message type on the topic pci_output_path.

Best, Mihir

ajay1606 commented 3 months ago

@MihirDharmadhikari

After following instruction as you suggested, I am able see some log messages as below;

And the green box in RVIZ shoes up here;

In this instance, planner stopped working;

And here, i tried to STOP_PLANNER to re initialize.

But after start agin, planning not working !

ajay1606 commented 3 months ago

@MihirDharmadhikari Thank so much for your feedback, as you mentioned its working normally now. I have made following changes in the file here.

run_mode: kSim to run_mode: kReal trigger_mode: kManual to trigger_mode: kAuto output_type: kAction to output_type: kTopic

After above modification robot able to explore path as expected. Thank you so much for your assistance.

By the way, able to save using/gbplanner_node/save_map, But how can i load saved map ? And how to avoid real time mapping ? Would you please give some hints !

Thanks in advance.

MihirDharmadhikari commented 3 months ago

Hi @ajay1606 ,

Glad to hear that it worked! To load the map, voxblox also has another service gbplanner_node/load_map. You can provide the file path to it and it will load the map. Once you load the map, voxblox will start integrating the subsequent point clouds against this map.

Best, Mihir

ajay1606 commented 3 months ago

@MihirDharmadhikari

I have saved map in the location using map.vxblx extension.

After running command, terminal shows no error. But there are no map published in topic, nor in rviz. How can i verify, map loading in gbplanned_node ? rosservice call /gbplanner_node/load_map "file_path: '/home/spotbot/gbplanner2_ws/src/exploration/gbplanner_ros/gbplanner/map/map.vxblx'"

ajay1606 commented 3 months ago

@MihirDharmadhikari Its working , thank you so much.

By the way, every time when i choose to Plan to way Point after setting goal pose Planning triggering mode changing to Auto to Manual.

And planner stops with following message. Where can i set it Auto ?

[ WARN] [1709949091.800309082, 1709243824.835715926]: Planner Trigger Mode set to kManual.

MihirDharmadhikari commented 3 months ago

Hi @ajay1606,

Sorry for the late reply. When you set the goal manually, you put the planner in manual mode. To set it back to the auto mode you need to press the start planner button in the UI.

Best, Mihir

ajay1606 commented 3 months ago

@MihirDharmadhikari Thank you so much for being so kind and sharing always very valuable inputs.

By the way, my last question in this, is it fine to increase the set_goal distance limitation ? Does it has any impact on performance ? Because currently, it is limited to 15 m with in that we have to choose the set_goal.

While deploy in real robot, do we need sim packages also ? Just trying to optimize specific code related to exploration and planning. So is it fine to remove sim code ? Is there any other packages depends on that ?

MihirDharmadhikari commented 3 months ago

Hi,

Can you point out to which parameter you are referring to (link to it in the code or config)?

Regarding the deployment, you can remove the sim packages.

Best, Mihir

ajay1606 commented 3 months ago

@MihirDharmadhikari Thank you so much for your quick response.

Here is the variable set for maximum allowed distance : https://github.com/ntnu-arl/gbplanner_ros/blob/77afce521a3aa7a4b94bf6a43a3c88ec08b6d544/gbplanner/src/rrg.cpp#L3320

Currently it is set to 15.0 here https://github.com/ntnu-arl/gbplanner_ros/blob/77afce521a3aa7a4b94bf6a43a3c88ec08b6d544/gbplanner/include/gbplanner/rrg.h#L53

Regards, Ajay

MihirDharmadhikari commented 3 months ago

Hi @ajay1606 ,

You can increase that distance, but keep in mind that this method does not calculate a path directly to the goal point you have set but to the closest vertex in the global graph that the exploration planner uses for global repositioning. If you see the distance very high, the robot will plan to a vertex that is quite far away from the desired goal. If this is fine for your application, then you can increase it.

Best, Mihir

ajay1606 commented 3 months ago

@MihirDharmadhikari Thank you so much, its been a nice discussion and able to learn lot with you. Thanks, and I will end this issue as my all questions has been well answered. Will open, new issue if we have any more questions.

Thanks for your kindness always !

MihirDharmadhikari commented 3 months ago

Always happy to help! :smile:

ntnu-arl / gbplanner_ros

Unable to set goal pose and get trajectory #48