#include <std_msgs/String.h>
#include "ros/ros.h"
#include <signal.h>
#include <webots_ros/robot_get_device_list.h>
#include "rosinljubljana/rosinljubljana.h"
#include <iostream>
#include <vector>

Include dependency graph for robot_webots.cpp:

Functions
void	controllerNameCallback (const std_msgs::String::ConstPtr &name)

int	main (int argc, char **argv)

void	quit (int sig)

Variables
static int	controllerCount

static std::vector< std::string >	controllerList

Detailed Description

Example of rosinljubljana package for webots simulation environment:

/*! \file robot_webots.cpp
 *  Example of rosinljubljana package for webots simulation environment:
 *  \verbinclude robot_webots.cpp
 */

#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();
  }
}

Definition in file robot_webots.cpp.

Function Documentation

◆ controllerNameCallback()

void controllerNameCallback ( const std_msgs::String::ConstPtr & name )

Definition at line 23 of file robot_webots.cpp.

◆ main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 35 of file robot_webots.cpp.

◆ quit()

void quit ( int sig )

Definition at line 29 of file robot_webots.cpp.

Variable Documentation

◆ controllerCount

int controllerCount

static

Definition at line 19 of file robot_webots.cpp.

◆ controllerList

std::vector<std::string> controllerList