Integrating a Costmap into the cost function template

[bosky2001]

Hello,
I am interested in integrating a ROS Occupancy Grid to be used as a cost-map within the cost function template. However, I couldn't find any documentation or examples on how to do this in the repository.

Specifically, I'd like to understand:

1. How to integrate an ROS Occupancy Grid into the cost function template
2. Do I have to modify the template or how the Occupancy grid data is stored to be passed onto the cost function?

If anyone can point me in the right direction, I would greatly appreciate the help!

Thanks in advance!

[bogidude]

This is going to be a bit of a long answer so please bear with me.

There is currently no existing cost function that is exactly what you are looking for. We do provide a utility class called texture helpers to load costmaps into a CUDA texture which is a fast way to query into a 2D array on the GPU and provides some useful helper methods. I will describe the steps to using it in the more detailed steps below. You will need to create your own cost function class that inherits from the base cost function class and define computeStateCost() on the CPU and GPU. This new cost function (I'll call it RosCost for the rest of this for simplicity) will need to make use of a TwoDTextureHelper<float>* member variable (I will call it map_helper_) to have access to the map on both the CPU and GPU. In order to pass a ros occupancy grid to map_helper_, you will need to convert it into either a std::vector<float> or an Eigen::MatrixXf and then you can pass it to the map_helper_ using the updateTexture() method (this is described in more detail below).

We do have an example cost function called Comparison Cost in the MPPI-Generic paper's example code repo (header and source files are located here) that can show you how to use the tex_helper_ once it is given a map. The texture helper API is located here but I will try to highlight the pieces you need to interact with. The texture helper is set up to store multiple maps if needed. You will tell it how many maps to store by passing the number through its constructor (in your case, it sounds like 1 map). Other things that need to be setup for the texture helper is the width, height, resolution, origin, and rotation of the map. You should be able to get this information from the ROS Occupancy Grid's MapInfo. The map I used for the paper's comparisons and its properties were created starting from here, in case you are curious.

This is how I would recommend setting it up.

1. Create a new cost function RosCost like the ComparisonCost example. Make sure to make a member variable for the map_helper_ (it is referred to as tex_helper_ in the ComparisonCost code) and to initalize it correctly in the cost function's constructor. You don't need to copy the computeGoalCost(), distanceToObstacle(), computeObstacleCost(), computeGoalAngleCost() methods from ComparisonCost, but the rest should be neeeded.
2. Add a class method that takes nav_msgs::OccupancyGrid::ConstPtr& ros_grid_msg_ptr as input, and puts that map into the map_helper_ using the following Texture Helper methods so that it can be used on the GPU.

   void fillMap(const nav_msgs::OccupancyGrid::ConstPtr& ros_grid_msg_ptr);

   a. To update the resolution of the map_helper_, you would call

   map_helper_->updateResolution(0, ros_grid_msg_ptr->info.resolution);

   where 0 is indicating that you are wanting to update the resolution of the zeroth map.
   b. To update the origin, you would need code like the following

   float3 origin = make_float3(ros_grid_msg_ptr->info.origin.position.x,
   ros_grid_msg_ptr->info.origin.position.y, 0);
   map_helper_->updateOrigin(0, origin);

   c. ROS occupancy grids look to also provide the rotation of the map in the map origin which we can also store in the texture helper

   Eigen::Quaternionf quat;
   quat.x() = ros_grid_msg_ptr->info.origin.orientation.x;
   quat.y() = ros_grid_msg_ptr->info.origin.orientation.y;
   quat.z() = ros_grid_msg_ptr->info.origin.orientation.z;
   quat.w() = ros_grid_msg_ptr->info.origin.orientation.w;
   
   Eigen::Matrix3f R = quat.toRotationMatrix();
   std::array<float3, 3> map_helper_R;
   // You might need to transpose R before filling in map_helper_R
   map_helper_R[0] = make_float3(R(0,0), R(0,1), R(0,2));
   map_helper_R[1] = make_float3(R(1,0), R(1,1), R(1,2));
   map_helper_R[2] = make_float3(R(2,0), R(2,1), R(2,2));
   map_helper_->updateRotation(0, map_helper_R);

   d. Setting the width and height is done as follows.

   cudaExtent map_dim = make_cudaExtent(ros_grid_msg_ptr->info.width, ros_grid_msg_ptr->info.height, 0);
   map_helper_->setExtent(0, map_dim);

   e. Finally, filling in the actual occupancy grid. It requires that the map's height and width are already set before calling.

   std::vector<float> map_data;
   // Put ros_grid_msg_ptr->data into map_data.
   
   // Set this bool to true if map_data is stored in column-major order
   bool column_major = false;
   map_helper_->updateTexture(0, map_data, column_major);
   
   // Enabling the texture is used to assert to the cost function code that the map has been filled with data.
   map_helper_->enableTexture(0);

3. Add the map query to both the CPU and GPU versions of computeStateCost(). To query into the map in either the GPU computeStateCost(y, timestep, theta_c, crash_status) or the CPU computeStateCost(y, timestep, crash_status), you would use the following:

   const float pos_x = y[O_IND_CLASS(DYN_P, POS_X)]; // get the x position from the state vector y
   const float pos_y = y[O_IND_CLASS(DYN_P, POS_Y)]; // get the y position from the state vector y
   const float3 pos = make_float3(pos_x, pos_y, 0);
   
   float map_query = 0;
   // Check that the map has been filled in (set true by enableTexture())
   if (map_helper_->checkTextureUse(0)) {
   	map_query = map_helper_->queryTextureAtWorldPose(0, pos);
   }
   
   // calculate cost based on the map_query

   The queryTextureAtWorldPose() method works on both the CPU and GPU and will conduct bilinear interpolation based on the 4 nearest cells to the position queried. On the GPU, this is done with GPU hardware instructions and on a special texture memory that should be faster to access which is why we recommend using the texture helper API in the first place. The ComparisonCost's CPU version of computeStateCost() is currently set to return 0; which is a bug. Your CPU version should do the same calculation as the GPU version because it can be used in the MPPI algorithm.
4. Make the necessary adjustments to create and copy things to the GPU. Adjust the cost functions paramsToDevice() and GPUSetup() methods as follows to also update the GPU side of map_helper_ as well.

   void paramsToDevice() {
   	if(this->GPUMemStatus_) {
   		map_helper_->copyToDevice();
   	}
   	PARENT_CLASS::paramsToDevice();
   }
   
   void GPUSetup() {
   	PARENT_CLASS* derived = static_cast<PARENT_CLASS*>(this);
   	derived->GPUSetup();
   
   	// Setup texture helper on GPU
   	map_helper_->GPUSetup();
   	// Make sure that the GPU pointer to map_helper_ actually points to the map_helper_ on the GPU, not the CPU.
   	HANDLE_ERROR(cudaMemcpyAsync(&(this->cost_d_->map_helper_), 
   						   &(map_helper_->ptr_d_), 
   						   sizeof(TwoDTextureHelper<float>*),
   						   cudaMemcpyHostToDevice, 
   						   this->stream_));
   
   }

I think that should be everything needed though if you run into problems or have further questions, feel free to message in this thread again.

[bosky2001]

Hi,
I know its been quite some time since , but thank you so much for this extremely detailed walkthrough on how to integrate the costmap. It's been very helpful for understanding and also using this library!