robot_information_parser.cpp
Go to the documentation of this file.
1 // Copyright 1996-2019 Cyberbotics Ltd.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #include <signal.h>
16 #include "ros/ros.h"
17 
18 // include files to use services like 'robot_get_time'.
19 // srv files needed to use webots service can be found in the /srv folder where you found this example.
20 // for more info on how to create and use services with ROS refer to their website: http://wiki.ros.org/
21 // here 'webots_ros' is the name of the package used for this node. Replace it by your own package.
22 #include <webots_ros/robot_get_device_list.h>
23 
24 #include <webots_ros/get_int.h>
25 #include <webots_ros/get_string.h>
26 
27 // include files to use standard message types in topic
28 // Webots only use basic messages type defined in ROS library
29 #include <std_msgs/String.h>
30 
31 #define TIME_STEP 32
32 
33 static int controllerCount;
34 static std::vector<std::string> controllerList;
35 
36 // catch names of the controllers availables on ROS network
37 void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
39  controllerList.push_back(name->data);
40  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
41 }
42 
43 void quit(int sig) {
44  ROS_INFO("User stopped the 'robot_information_parser' node.");
45  ros::shutdown();
46  exit(0);
47 }
48 
49 int main(int argc, char **argv) {
50  std::string controllerName;
51  std::vector<std::string> deviceList;
52  // create a node named 'robot_information_parser' on ROS network
53  ros::init(argc, argv, "robot_information_parser", ros::init_options::AnonymousName);
55 
56  signal(SIGINT, quit);
57 
58  // subscribe to the topic model_name to get the list of availables controllers
59  ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCallback);
60  while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
61  ros::spinOnce();
62  ros::spinOnce();
63  ros::spinOnce();
64  }
65  ros::spinOnce();
66 
67  // if there is more than one controller available, it let the user choose
68  if (controllerCount == 1)
69  controllerName = controllerList[0];
70  else {
71  int wantedController = 0;
72  std::cout << "Choose the # of the controller you want to use:\n";
73  std::cin >> wantedController;
74  if (1 <= wantedController && wantedController <= controllerCount)
75  controllerName = controllerList[wantedController - 1];
76  else {
77  ROS_ERROR("Invalid number for controller choice.");
78  return 1;
79  }
80  }
81  // leave topic once it is not necessary anymore
82  nameSub.shutdown();
83 
84  // call get_type and get_model services to get more general information about the robot
85  ros::ServiceClient getTypeClient = n.serviceClient<webots_ros::get_int>(controllerName + "/robot/get_type");
86  webots_ros::get_int getTypeSrv;
87  ros::ServiceClient getModelClient = n.serviceClient<webots_ros::get_string>(controllerName + "/robot/get_model");
88  webots_ros::get_string getModelSrv;
89 
90  getTypeClient.call(getTypeSrv);
91  if (getTypeSrv.response.value == 40)
92  ROS_INFO("This controller is on a basic robot.");
93  else if (getTypeSrv.response.value == 41)
94  ROS_INFO("This controller is on a supervisor robot.");
95  else
96  ROS_INFO("This controller is on a differential wheels robot.");
97 
98  if (getModelClient.call(getModelSrv)) {
99  if (!getModelSrv.response.value.empty())
100  ROS_INFO("The model of this robot is %s.", getModelSrv.response.value.c_str());
101  else
102  ROS_ERROR("The robot doesn't seems to have a model.");
103  } else
104  ROS_ERROR("Could not get the model of this robot.");
105 
106  // call deviceList service to get the list of the name of the devices available on the controller and print it
107  // the deviceListSrv object contains 2 members: request and response. Their fields are described in the corresponding .srv
108  // file
109  ros::ServiceClient deviceListClient =
110  n.serviceClient<webots_ros::robot_get_device_list>(controllerName + "/robot/get_device_list");
111  webots_ros::robot_get_device_list deviceListSrv;
112 
113  if (deviceListClient.call(deviceListSrv)) {
114  deviceList = deviceListSrv.response.list;
115  ROS_INFO("The controller has %lu devices availables:", deviceList.size());
116  for (unsigned int i = 0; i < deviceList.size(); i++)
117  ROS_INFO("Device [%d]: %s.", i, deviceList[i].c_str());
118  } else
119  ROS_ERROR("Failed to call service deviceList.");
120 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
void quit(int sig)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
static std::string controllerName
ros::NodeHandle * n
Definition: pioneer3at.cpp:40
#define ROS_INFO(...)
uint32_t getNumPublishers() const
static int controllerCount
void controllerNameCallback(const std_msgs::String::ConstPtr &name)
ROSCPP_DECL void shutdown()
static std::vector< std::string > controllerList
int main(int argc, char **argv)
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)


webots_ros
Author(s):
autogenerated on Mon Jul 8 2019 03:19:27