Robotics StackExchange | Archived questions

Problem with costmap2dros and tf transform listener in ros melodic

I have a code that works well in ros kinetic however while using the same in ros melodic is causing issues. I tried debugging it to no avail. I want the following code to create costmap in ros melodic.

#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include "std_msgs/Int32.h"
#include <std_srvs/Empty.h>
#include <string.h>

costmap_2d::Costmap2DROS* lcr ;

void clearCallback(const std_msgs::Int32::ConstPtr& msg)
{
 if(msg->data==1){
    lcr->resetLayers();
    lcr->stop();
    lcr->start();
    costmap_2d::Costmap2D* getcmp = lcr->getCostmap();
  }
}

    int main(int argc, char** argv)
    {
  ros::init(argc, argv, "costmap_node");
  ros::NodeHandle n;
  tf::TransformListener tm(ros::Duration(10));
  lcr = new costmap_2d::Costmap2DROS ("my_costmap", tm);

  ros::Subscriber sub = n.subscribe("clear", 1000, clearCallback);


  std::cout<<ros::Time::now()<<"\n";

  ros::spin();

  return (0);
}

the errors that i get are

error: ‘tf’ has not been declared
   tf::TransformListener tm(ros::Duration(10)); 

it goes away with

 #include <tf/transform_listener.h>

and

error: expected primary-expression before ‘)’ token
   lcr = new costmap_2d::Costmap2DROS ("my_costmap", tm);

Asked by DimpleB on 2019-09-04 04:26:08 UTC

Comments

Answers

The interface to costmaps changed in melodic to use tf2. https://github.com/ros-planning/navigation/blob/melodic-devel/costmap_2d/include/costmap_2d/costmap_2d_ros.h#L82

Asked by David Lu on 2019-09-04 10:01:02 UTC

Comments

Thank you for your reply. I am still not able to understand how to rewrite it correctly. But didn't yield positive results. Is there any documentation that can help

#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include "std_msgs/Int32.h"
#include <std_srvs/Empty.h>
#include <string.h>
#include <tf2_ros/transform_listener.h>

costmap_2d::Costmap2DROS* lcr ;

void clearCallback(const std_msgs::Int32::ConstPtr& msg)
{
  if(msg->data==1){
    lcr->resetLayers();
    lcr->stop();
    lcr->start();
    costmap_2d::Costmap2D* getcmp = lcr->getCostmap();
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "costmap_node");
  ros::NodeHandle n;
  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tf2_listener(tfBuffer);;
  lcr = new costmap_2d::Costmap2DROS ("my_costmap", tf2_listener);
ros::Subscriber sub = n.subscribe("clear", 1000, clearCallback);

Asked by DimpleB on 2019-09-06 02:54:26 UTC

You need to pass the tfBuffer into the Costmap, not the listener.

Asked by David Lu on 2019-09-06 09:29:18 UTC