diff --git a/src/depth_to_2d.cpp b/src/depth_to_2d.cpp index 30a743e6ef809c353705b12882494a2d5502d308..0c73f8af8f77d4f6ff8e77e491f1749b2b0dd193 100644 --- a/src/depth_to_2d.cpp +++ b/src/depth_to_2d.cpp @@ -70,12 +70,18 @@ void conversion_callback(const PointCloud::ConstPtr& pcl, const sensor_msgs::Imu obstacle_map.cells[cell_ind].y = i * device_height; obstacle_map.cells[cell_ind].z += pt.z; // Accumulating the z values of all the points that fall in this cell } + + //Publishing the obstacle map for consumption from the tactile device + map_pub.publish(obstacle_map); } int main(int argc, char** argv) { ros::init(argc, argv, "depth_to_2d"); ros::NodeHandle nh; + // Initializing the publisher of the obstacle map + map_pub = nh.advertise<nav_msgs::GridCells>("/obstacle_map", 1); + // Initialising the spatial resolution of the camera and the tactile device // These value are assumptions but can be updated to reflect real values cam_width = 1024;