Unable to set goal pose and get trajectory about gbplanner_ros HOT 17 CLOSED

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:

1. 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.
2. 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

from gbplanner_ros.

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

from gbplanner_ros.

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

from gbplanner_ros.

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

from gbplanner_ros.

@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'"

from gbplanner_ros.

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

from gbplanner_ros.

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

from gbplanner_ros.

Always happy to help! 😄

from gbplanner_ros.

@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

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

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>

from gbplanner_ros.

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

from gbplanner_ros.

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

from gbplanner_ros.

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

from gbplanner_ros.

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

from gbplanner_ros.

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

from gbplanner_ros.

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

from gbplanner_ros.

@MihirDharmadhikari Thank you so much for your quick response.

Here is the variable set for maximum allowed distance :

Currently it is set to 15.0 here

Regards,
Ajay

from gbplanner_ros.

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

from gbplanner_ros.