How to use dwa_local_planner ROS wrapper C++ API
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 ...
Just a note:
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.
Hi,
Yeah I meant ROS wrapper.
Thanks for the note :).
What does your tf tree look like?
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.
you do now.
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.