robot_webots.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>
 
static int controllerCount;
static std::vector<std::string> controllerList;
 
// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
  controllerCount++;
  controllerList.push_back(name->data);
  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}
 
void quit(int sig) {
  ROS_INFO("User stopped the 'robot_information_parser' node.");
  ros::shutdown();
  exit(0);
}
 
int main(int argc, char **argv) {
 
 
  std::vector<std::string> controllerName;
  std::vector<std::string> deviceList;
 
  // create a node named 'robot_information_parser' on ROS network
  ros::init(argc, argv, "robot");
  ros::NodeHandle n;
  ros::Rate loop_rate(30);
 
  signal(SIGINT, quit);
 
  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'");
    }
 
  // subscribe to the topic model_name to get the list of availables controllers
  ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCallback);
  while (controllerCount < robot_list.size() || controllerCount < nameSub.getNumPublishers()) {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();
 
  std::stringstream robot_name_;
 
  for (int i = 0; i<controllerCount; i++){
    ROS_INFO_STREAM(controllerList[i]);
    for (int j = 0; j<robot_list.size(); j++){
      robot_name_.str(std::string());
      robot_name_ << "robot_" << robot_list.at(j);
 
    if (controllerList[i].find(robot_name_.str()) != std::string::npos) {
      controllerName.push_back(controllerList[i]);
    }
  }
  }
 
  WebotsRobot robots[controllerName.size()];
 
  for (int i=0; i<controllerName.size(); i++)
  {
    robots[i].setName(std::string(1, (controllerName.at(i)).at(6)), n);
    robots[i].init(n, controllerName.at(i));
    robots[i].setFrames();
 
    robots[i].enableGyro(n);
    robots[i].subscribeGyro(n);
 
    robots[i].subscribeMove(n);
 
  }
 
 
 
  Field field(n);
 
  field.mapResolution = 0.05;
  field.mapWidth = 500;
  field.mapHeight = 500;
 
  std::vector<Point_2> points;
  Point_2 p1(-1, -2);
  Point_2 p2(1, -1);
  Point_2 p3(1, 2);
  Point_2 p4(-1, 2);
  Point_2 p5(-1, -2);
  Point_2 p11(3.5, 5);
 
  points.push_back(p1);
  points.push_back(p2);
  points.push_back(p3);
  points.push_back(p11);
  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();
  }
}