costmap_2d_ros always filled with lethal obstacles [closed]

asked 2012-03-20 10:54:43 -0500

ThomasB gravatar image

updated 2014-12-07 00:59:46 -0500

tfoote gravatar image

I have made a node that keeps a local costmap_2d_ros object but currently it seems that the costmap is filled with all lethal obstacles (value 254) and I am unable to clear it. I based my code off of the code in the move_base package. I have pasted the relevant code below. In summary, I use the methods getCostmapCopy and getCharMap and print out the values in the char array. All the values are 254 and I then try using resetMapOutsideWindow with the window size being 0,0. The values are still all 254. Any advice would be greatly appreciated and if you need any more information I'd be happy to supply it.

namespace matrix_encoder {
   class MatrixEncoder {
      public:
         MatrixEncoder(std::string name, tf::TransformListener& tf);
         //~MatrixEncoder();
      private:
         tf::TransformListener& tf_;
         costmap_2d::Costmap2DROS* encoder_costmap_ros;
         costmap_2d::Costmap2D costmap; // The underlying costmap to update
         const unsigned char* charArray;  // pointer to the underlying unsigned char array used as the costmap
   };
};

namespace matrix_encoder {

   MatrixEncoder::MatrixEncoder(std::string name, tf::TransformListener& tf) :
   tf_(tf), encoder_costmap_ros(NULL), charArray(NULL) {


   ros::NodeHandle nh;

   encoder_costmap_ros = new costmap_2d::Costmap2DROS("encoder_costmap", tf_);
   encoder_costmap_ros->getCostmapCopy(costmap);
   charArray = costmap.getCharMap();
   int index = 0;
   for (int j = 0; j < numXcells; j++) {
      for(int i = 0; i < numYcells; i++) {
         ROS_INFO("%d", charArray[index++]);
      }
   }

   index = 0;
   encoder_costmap_ros->resetMapOutsideWindow(0,0);
   encoder_costmap_ros->getCostmapCopy(costmap);
   charArray = costmap.getCharMap();
   ROS_INFO("About to print cleared map");
   for (int j = 0; j < numXcells; j++) {
      for(int i = 0; i < numYcells; i++) {
         ROS_INFO("%d", charArray[index++]);
      }
   }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "encoder");

  tf::TransformListener tf(ros::Duration(10));
  matrix_encoder::MatrixEncoder matrix_encoder("matrix_encoder", tf);

  ros::NodeHandle n;

  ros::spin();

  return 0;
}
edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2015-03-03 01:37:58.147737

Comments

I've had the same problem. A quick hack is to zero out the costmap (using memset) if you don't plan to deal with obstacles.

PKG gravatar image PKG  ( 2012-04-03 12:17:56 -0500 )edit

I too have the same problem. Anybody know how to solve this if you DO want to deal with obstacles?

BlitherPants gravatar image BlitherPants  ( 2014-03-04 11:54:01 -0500 )edit