Ask Your Question
0

How to use dwa_local_planner ROS wrapper C++ API

asked 2020-03-25 06:40:39 -0500

itaouil gravatar image

updated 2020-03-25 15:52:05 -0500

Hi,

UPDATE: This question was answered. Look at the answer below for a quick how-to :).

I am trying to use the ROS WRAPPER for the dwa_local_planner, however I am having some issues when I call the function dwaComputeVelocityCommands as it throws a segmentation fault on the function dwa_local_planner::DWAPlanner::findBestPath. This is the error throws:

Thread 1 "planner" received signal SIGSEGV, Segmentation fault.
0x00007ffff7b9e282 in dwa_local_planner::DWAPlanner::findBestPath(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> >&) ()
   from /opt/ros/melodic/lib/libdwa_local_planner.so

and here is the full backtrace from GDB:

(gdb) backtrace 
#0  0x00007ffff7b9e282 in dwa_local_planner::DWAPlanner::findBestPath(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> >&) ()
   from /opt/ros/melodic/lib/libdwa_local_planner.so
#1  0x00007ffff7ba8135 in dwa_local_planner::DWAPlannerROS::dwaComputeVelocityCommands(geometry_msgs::PoseStamped_<std::allocator<void> >&, geometry_msgs::Twist_<std::allocator<void> >&) () from /opt/ros/melodic/lib/libdwa_local_planner.so
#2  0x0000555555580516 in Planner::dwaTrajectoryControl(hsr_planner::ClutterPlannerService const&) ()
#3  0x000055555557f92d in Planner::requestClutterPlan() ()
#4  0x000055555557ed85 in Planner::Planner(tf2_ros::Buffer&, tf2_ros::TransformListener&) ()
#5  0x0000555555580b31 in main ()

I am not sure whether this is a bug in my code or on the API itself, and I have bee trying to solve this issue for a bunch of hours already so I would like to know whether the steps I took are correct or not, or if you have any suggestion, please :).

How I Initialize

I start by creating the tf2 Buffer as well as the TransformListener in the main and then passing these to the constructor of the Class in order to populate the costmap (pointer) and the dynamic planner.

Here is the main:

int main(int argc, char **argv)
{
    // Create ROS node
    ros::init(argc, argv, "planner");
    ROS_INFO("Created Planner node...");

    // TF2 objects
    tf2_ros::Buffer l_buffer(ros::Duration(10));
    tf2_ros::TransformListener l_tf(l_buffer);

    // Create Planner instance
    Planner planner(l_buffer, l_tf);

    // Spin ROS
    ros::spin();

    return 0;
}

and here is the initialization of the members costmap and dynamic planner in the class (or how I do it):

// Create shared pointers instances
m_costMap = new costmap_2d::Costmap2DROS("hsr_costmap", m_buffer);

// Initialize dwa local planner
m_dp.initialize("hrs_dwa_planner", &m_buffer, m_costMap);

Afterwards, I set the global plan using setPlan which is always successful and works well, and proceed to compute the velocities using dwaComputeVelocityCommands passing to it the global pose obtained from the costmap pointer and the Twist object as follows:

    // Set global plan for local planner
    if (m_dp.setPlan(p_service.response.path))
    {
        ROS_INFO("DWA set plan: SUCCESS");
    }
    else
    {
        ROS_ERROR("DWA set plan: FAILED");
    }

    // Create twist messag to be
    // populate by the local planner
    geometry_msgs::Twist l_cmd_vel;
    l_cmd_vel.linear.x = 0;
    l_cmd_vel.linear.y = 0;
    l_cmd_vel.linear.z = 0;
    l_cmd_vel.angular.x = 0;
    l_cmd_vel.angular.y = 0;
    l_cmd_vel.angular.z = 0;

    // Get robot pose in the map
    geometry_msgs::PoseStamped l_global_pose;
    m_costMap->getRobotPose(l_global_pose);

    // Keep sending commands
    // until goal is reached
    while (!m_dp.isGoalReached())
    {
        ROS_INFO(m_dp.isGoalReached());

        // Compute velocity commands
        if (m_dp.dwaComputeVelocityCommands(l_global_pose, l_cmd_vel))
        {
            ROS_INFO("DWA compute cmd_vel: SUCCESS");
        }
        else
        {
            ROS_ERROR("DWA compute cmd_vel: FAILED");
        }

        // Send commands
        m_velPub.publish(l_cmd_vel ...
(more)
edit retag flag offensive close merge delete

Comments

Just a note:

I am trying to use the ROS API [..]

with "ROS API", people typically refer to the set of parameters, topics, services and actions a (set of) node(s) exposes and consumes.

From your question text/description it would appear you are using the C++ API of several ROS libraries or packages instead.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-25 09:05:13 -0500 )edit

Hi,

Yeah I meant ROS wrapper.

Thanks for the note :).

itaouil gravatar image itaouil  ( 2020-03-25 09:10:56 -0500 )edit

What does your tf tree look like?

David Lu gravatar image David Lu  ( 2020-03-25 09:25:29 -0500 )edit

I don't have enough points to upload the tf tree image, but it seems good to me. If you have any suggestions on how I can post it let me know.

itaouil gravatar image itaouil  ( 2020-03-25 09:43:31 -0500 )edit

I don't have enough points to upload the tf tree image

you do now.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-25 09:51:50 -0500 )edit

Your first approach is missing a call to updatePlanAndLocalCosts, which I believe is causing the segfault.

I would debug to make sure you know why you're getting the TF error when initializing the costmap.

David Lu gravatar image David Lu  ( 2020-03-25 10:02:49 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-03-25 15:51:06 -0500

itaouil gravatar image

I managed to get the dwa_local_planner work. For anyone wanting to set it up, here is a quick tutorial on how to do it. And in case you are wondering, the issues was that the costmap was not initialized correctly as I did not load the parameters in the yaml file, so there was nothing going on there, which made the SEGMENTATION FAULT being thrown.

Costmaps and Launch file

First of all, we have to set up costmaps settings for our global and local costmap. For this, we will make use of .yaml files. We will need three yaml files, one for common costmap settings, one for the global costmap settings, and one for local costmap settings. The files (as of 2020 - Melodic branch) look like this:

Common Parameters

obstacle_range: 2.5
raytrace_range: 3.0
robot_radius: 0.430
inflation_radius: 0.55
origin_x: 0
origin_y: 0

observation_sources: laser_scan_sensor point_cloud_sensor
laser_scan_sensor: {sensor_frame: base_range_sensor_link, data_type: LaserScan, topic: /hsrb/base_scan, marking: true, clearing: true}
point_cloud_sensor: {sensor_frame: head_rgbd_sensor_rgb_frame, data_type: PointCloud2, topic: /hsrb/head_rgbd_sensor/depth_registered/rectified_points, marking: true, clearing: true}

Please change the sensor frame, topic, and dataType fields accordingly

Global Parameters

global_costmap:
  global_frame: "map"
  rolling_window: true
  transform_tolerance: 2.0
  robot_base_frame: "base_link"

plugins: 
    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

Local Parameters

local_costmap:
  width: 6.0
  height: 6.0
  resolution: 0.05
  global_frame: "odom"
  rolling_window: true
  update_frequency: 2.0
  publish_frequency: 2.0
  transform_tolerance: 2.0
  robot_base_frame: "base_link"

plugins: 
  - {name: obstacle_layer,  type: "costmap_2d::VoxelLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

Finally, we create a launch file which will launch the node file we created (or will create later), while loading the costmaps parameter defined above in the parameter space. My launch file looked like this:

<launch>

<!-- Run the map server --> 
<!--<node name="map_server" pkg="map_server" type="map_server" args="$(find my_map_package)/my_map.pgm my_map_resolution"/>-->

<node pkg="hsr_planner" type="planner" respawn="false" name="hsr_planner" output="screen">
    <rosparam file="$(find hsr_planner)/params/costmap_common_params.yaml" command="load" ns="global_costmap" /> 
    <rosparam file="$(find hsr_planner)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find hsr_planner)/params/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find hsr_planner)/params/global_costmap_params.yaml" command="load" /> 
</node>

</launch>

Please note to change paths accordingly to your environment and adapt your fields to your robot/topics etc.

Code

Now the fun part :).

In order to initialize the dwa_local_planner instance, we first need to create our costmap/s. In this way:

// TF2 objects
tf2_ros::Buffer l_buffer(ros::Duration(10));
tf2_ros::TransformListener l_tf(l_buffer);

// Initialize costmaps (global and local)
l_local = new costmap_2d::Costmap2DROS("local_costmap", l_buffer);
l_global = new costmap_2d::Costmap2DROS("global_costmap", l_buffer);

// Start map
l_local->start();
l_global->start();

Then we proceed to initialize the dwa_local_planner instance as follows, while trying to compute the velocities to be sent to the robot in order to reach the goal:

// Check that planner is initialized
if (!m_dp.isInitialized())
{
    // Initialize dwa local planner
    m_dp.initialize("hsr_dwa_planner", &m_buffer, m_local);

    ROS_INFO("DWA has been initialized successfully...");
}

// Set global plan for local planner
if ...
(more)
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-03-25 06:40:39 -0500

Seen: 29 times

Last updated: Mar 25