Now, we are going to make a node get the laser data, do something with it, and publish the new data. Perhaps, this will be useful at a later date, and with this example, you will learn how to do it.

Copy the following code snippet to the c8_laserscan.cpp file in your /chapter8_tutorials/src directory:

#include <ros/ros.h> 
#include "std_msgs/String.h" 
#include <sensor_msgs/LaserScan.h> 
 
#include<stdio.h> 
using namespace std; 
class Scan2{ 
  public: 
  Scan2(); 
  private: 
  ros::NodeHandle n; 
  ros::Publisher scan_pub; 
  ros::Subscriber scan_sub; 
  void scanCallBack(const sensor_msgs::LaserScan::ConstPtr& 
   scan2); 
}; 
Scan2::Scan2() 
{ 
  scan_pub = n.advertise<sensor_msgs::LaserScan>("/scan2",1); 
  scan_sub = n.subscribe<sensor_msgs::LaserScan>("/scan",1, 
   &Scan2::scanCallBack, this); 
} 
 
void Scan2::scanCallBack(const sensor_msgs::LaserScan::ConstPtr& 
 scan2) 
{ 
  int ranges = scan2->ranges.size(); 
  //populate the LaserScan message 
  sensor_msgs::LaserScan scan; 
  scan.header.stamp = scan2->header.stamp; 
  scan.header.frame_id = scan2->header.frame_id; 
  scan.angle_min = scan2->angle_min; 
  scan.angle_max = scan2->angle_max; 
  scan.angle_increment = scan2->angle_increment; 
  scan.time_increment = scan2->time_increment; 
  scan.range_min = 0.0; 
  scan.range_max = 100.0; 
  scan.ranges.resize(ranges); 
  for(int i = 0; i < ranges; ++i) 
  { 
    scan.ranges[i] = scan2->ranges[i] + 1; 
  } 
  scan_pub.publish(scan); 
} 
int main(int argc, char** argv) 
{ 
  ros::init(argc, argv, "laser_scan_publisher"); 
  Scan2 scan2; 
  ros::spin(); 
} 

We are going to break the code and see what it is doing.

In the main function, we initialize the node with the name example2_laser_scan_publisher and create an instance of the class that we have created in the file.

In the constructor, we will create two topics: one of them will subscribe to the other topic, which is the original data from the laser. The second topic will publish the newly modified data from the laser.

This example is very simple; we are only going to add one unit to the data received from the laser topic and publish it again. We do that in the scanCallBack() function. Take the input message and copy all the fields to another variable. Then, take the field where the data is stored and add the one unit. Once the new value is stored, publish the new topic:

void Scan2::scanCallBack(const sensor_msgs::LaserScan::ConstPtr& 
 scan2) 
{ 
  ... 
  sensor_msgs::LaserScan scan; 
  scan.header.stamp = scan2->header.stamp; 
  ... 
  ... 
  scan.range_max = 100.0; 
  scan.ranges.resize(ranges); 
   
  for(int i = 0; i < ranges; ++i){ 
    scan.ranges[i] = scan2->ranges[i] + 1; 
  } 
   
  scan_pub.publish(scan); 
}