Rosinljubljana package

Field and Obstacles

Field class represents the world robots are located in. Obstacles in said world are defined by Obstacle class.

The code

Field field(n);
 
field.mapResolution = 0.008;
field.mapWidth = 500;
field.mapHeight = 500;

We create an object field with nodehandle n. Parameters mapResolution, mapWidth and mapHeight must be set for map generation to work.

std::vector<Point_2> points;
Point_2 p1(-0.81,  0.8);
Point_2 p2(1.43, 0.88);
Point_2 p3(1.43, -0.38);
Point_2 p4(-0.81, -0.515);
Point_2 p5(-0.81, 0.8);
 
points.push_back(p1);
points.push_back(p2);
points.push_back(p3);
points.push_back(p4);
points.push_back(p5);
 
Obstacle obstacle("Prvi");
 
std::vector<Point_2> points_2;
Point_2 p6(0.5, 0.8);
Point_2 p7(0.3, 0);
 
points_2.push_back(p6);
points_2.push_back(p7);
 
Obstacle obstacle2("Drugi");
 
obstacle2.setSegments(points_2);
obstacle.setSegments(points);

We create vectors of points which represent obstacles in the world. Elements in vectors are of CGAL class Point_2.

field.obstacles.push_back(obstacle);
field.obstacles.push_back(obstacle2);

Created obstacles are added to field object. First obstacle must be closed polygon for LIDAR simulation to work!!!

field.generateOccupancyGrid();

Based on obstacles in field we create OccupancyGrid. DDA algorithem is used to correctly populate occupancy grid. Occupancy grid generated by presented code can be seen image below.

while(ros::ok()){
 
  field.publishOccupancyGrid(n);
 
  ros.spinOnce();
}

We publish nav_msgs/OccupancyGrid message.

Robot

Robot class represents a mobile robot. It is used manage coordinate system transformations, generate sensor_msgs/LaserScan massage.

The code

std::vector<int> robot_list;
if (n.getParam("robot_list", robot_list))
{
  ROS_INFO("Got param: robot_list");
}
else
{
  ROS_ERROR("Failed to get param 'robot_list'");
}
 
Robot robots[robot_list.size()];
 
for (int i=0; i<robot_list.size(); i++)
{
  robots[i].setName(std::to_string(robot_list.at(i)), n);
  robots[i].setFrames();
  robots[i].transmitFrames();
}

robot array is created based on rosparam robot_list. Name of robot must correspond with ar tag on the robot. Robot frames are set and broadcasted. Each robot is subscribed to robot_#/ekf_odom topic from robot_localization package and publishes transformations.

while(ros::ok()){
  for (int i=0; i<robot_list.size(); i++)
  {
    robots[i].transmitPoseFromAlvarPoseMsg();
    robots[i].transmitFrames();
  }
  for (int i=0; i<robot_list.size(); i++)
  {
    robots[i].transmitLaserScan(robots, robot_list.size(), &field);
  }
  loop_rate.sleep();
  ros::spinOnce();
}

Each iteration geometry_msgs/Pose message is published based on ar_track_alvar transforms, transformations of robot frames are published and sensor_msgs/LaserScan is published.