robot.cpp

Go to the documentation of this file.

 
#include <std_msgs/String.h>
#include "ros/ros.h"
#include <signal.h>
 
 
#include <webots_ros/robot_get_device_list.h>
#include <webots_ros/robot_get_device_list.h>
 
#include "rosinljubljana/rosinljubljana.h"
 
#include <iostream>
#include <vector>
 
void quit(int sig) {
  ROS_INFO("User stopped the 'robot_information_parser' node.");
  ros::shutdown();
  exit(0);
}
 
int main(int argc, char **argv) {
 
  // create a node named 'robot_information_parser' on ROS network
  ros::init(argc, argv, "robot");
  ros::NodeHandle n;
  ros::Rate loop_rate(100);
 
  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();
 
  }
 
  Field field(n);
 
  field.mapResolution = 0.008;
  field.mapWidth = 500;
  field.mapHeight = 500;
 
    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);
 
  field.obstacles.push_back(obstacle);
 
  field.obstacles.push_back(obstacle2);
 
  field.generateOccupancyGrid();
 
        while(ros::ok()){
 
    field.publishOccupancyGrid(n);
 
    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();
  }
}