catch_the_bird.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 <sensor_msgs/Image.h>
19 #include <std_msgs/String.h>
20 
21 #include <webots_ros/get_float.h>
22 #include <webots_ros/set_float.h>
23 #include <webots_ros/set_int.h>
24 
25 #include <webots_ros/range_finder_get_info.h>
26 #include <webots_ros/robot_get_device_list.h>
27 #include <webots_ros/save_image.h>
28 
29 #define TIME_STEP 32;
30 
31 static int controllerCount;
32 static std::vector<std::string> controllerList;
33 static std::vector<float> imageRangeFinder;
34 
36 webots_ros::set_int timeStepSrv;
37 
38 // catch names of the controllers availables on ROS network
39 void controllerNameCallback(const std_msgs::String::ConstPtr &name) {
41  controllerList.push_back(name->data);
42  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
43 }
44 
45 // get range image from the range-finder
46 void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) {
47  int size = image->width * image->height;
48  imageRangeFinder.resize(size);
49 
50  const float *depth_data = reinterpret_cast<const float *>(&image->data[0]);
51  for (int i = 0; i < size; ++i)
52  imageRangeFinder[i] = depth_data[i];
53 }
54 
55 void quit(int sig) {
56  timeStepSrv.request.value = 0;
57  timeStepClient.call(timeStepSrv);
58  ROS_INFO("User stopped the 'catch_the_bird' node.");
59  ros::shutdown();
60  exit(0);
61 }
62 
63 int main(int argc, char **argv) {
64  std::string controllerName, motorName, rangeFinderName;
65  std::vector<std::string> deviceList;
66  int width, height;
67  float i, step;
68  bool birdCatched = false;
69 
70  // create a node named 'catch_the_bird' on ROS network
71  ros::init(argc, argv, "catch_the_bird", ros::init_options::AnonymousName);
73 
74  signal(SIGINT, quit);
75 
76  // subscribe to the topic model_name to get the list of availables controllers
77  ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCallback);
78  while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
79  ros::spinOnce();
80  ros::spinOnce();
81  ros::spinOnce();
82  }
83  ros::spinOnce();
84 
85  // if there is more than one controller available, let the user choose
86  if (controllerCount == 1)
87  controllerName = controllerList[0];
88  else {
89  int wantedController = 0;
90  std::cout << "Choose the # of the controller you want to use:\n";
91  std::cin >> wantedController;
92  if (1 <= wantedController && wantedController <= controllerCount)
93  controllerName = controllerList[wantedController - 1];
94  else {
95  ROS_ERROR("Invalid number for controller choice.");
96  return 1;
97  }
98  }
99  // leave topic once it's not necessary anymore
100  nameSub.shutdown();
101 
102  // call device_list service to get the list of the devices available on the controller and print it the device_list_srv object
103  // contains 2 members request and response. Their fields are described in the corresponding .srv file
104  ros::ServiceClient deviceListClient =
105  n.serviceClient<webots_ros::robot_get_device_list>(controllerName + "/robot/get_device_list");
106  webots_ros::robot_get_device_list deviceListSrv;
107 
108  if (deviceListClient.call(deviceListSrv))
109  deviceList = deviceListSrv.response.list;
110  else
111  ROS_ERROR("Failed to call service device_list.");
112  motorName = deviceList[0];
113  rangeFinderName = deviceList[1];
114 
115  ros::ServiceClient rangeFinderGetInfoClient =
116  n.serviceClient<webots_ros::range_finder_get_info>(controllerName + '/' + rangeFinderName + "/get_info");
117  webots_ros::range_finder_get_info rangeFinderGetInfoSrv;
118  if (rangeFinderGetInfoClient.call(rangeFinderGetInfoSrv)) {
119  width = rangeFinderGetInfoSrv.response.width;
120  height = rangeFinderGetInfoSrv.response.height;
121  ROS_INFO("Range-finder size is %d x %d.", width, height);
122  } else
123  ROS_ERROR("Failed to call service range_finder_get_info.");
124 
125  // enable the range-finder
126  ros::ServiceClient enableRangeFinderClient =
127  n.serviceClient<webots_ros::set_int>(controllerName + '/' + rangeFinderName + "/enable");
128  webots_ros::set_int enableRangeFinderSrv;
129  ros::Subscriber subRangeFinderRangeFinder;
130 
131  enableRangeFinderSrv.request.value = 2 * TIME_STEP;
132  if (enableRangeFinderClient.call(enableRangeFinderSrv) && enableRangeFinderSrv.response.success) {
133  ROS_INFO("Range-finder enabled with sampling period %d.", enableRangeFinderSrv.request.value);
134  subRangeFinderRangeFinder = n.subscribe(controllerName + '/' + rangeFinderName + "/range_image", 1, rangeFinderCallback);
135 
136  // wait for the topics to be initialized
137  while (subRangeFinderRangeFinder.getNumPublishers() == 0) {
138  }
139  } else
140  ROS_ERROR("Failed to call service enable for %s.", rangeFinderName.c_str());
141 
142  ros::ServiceClient rangeFinderSaveImageClient =
143  n.serviceClient<webots_ros::save_image>(controllerName + '/' + rangeFinderName + "/save_image");
144  webots_ros::save_image rangeFinderSaveImageSrv;
145  rangeFinderSaveImageSrv.request.filename = std::string(getenv("HOME")) + std::string("/bird_catched.png");
146  rangeFinderSaveImageSrv.request.quality = 100;
147 
148  // enable motor
149  ros::ServiceClient motorSetPositionClient =
150  n.serviceClient<webots_ros::set_float>(controllerName + '/' + motorName + "/set_position");
151  webots_ros::set_float motorSetPositionSrv;
152  motorSetPositionSrv.request.value = 0;
153  i = 0.2;
154  step = 0.025;
155 
156  ros::ServiceClient motorGetTargetPositionClient =
157  n.serviceClient<webots_ros::get_float>(controllerName + '/' + motorName + "/get_target_position");
158  webots_ros::get_float motorGetTargetPositionSrv;
159 
160  // enable time_step
161  timeStepClient = n.serviceClient<webots_ros::set_int>(controllerName + "/robot/time_step");
162  timeStepSrv.request.value = TIME_STEP;
163 
164  // main loop
165  while (!birdCatched && ros::ok()) {
166  motorSetPositionSrv.request.value = i;
167  motorSetPositionClient.call(motorSetPositionSrv);
168  if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) {
169  ROS_ERROR("Failed to call next step with time_step service.");
170  exit(1);
171  }
172  motorGetTargetPositionClient.call(motorGetTargetPositionSrv);
173  if (i >= 3.14)
174  step = -0.025;
175  if (i <= -3.14)
176  step = 0.025;
177  i += step;
178  ros::spinOnce();
179  while (imageRangeFinder.size() < (width * height))
180  ros::spinOnce();
181  // check if it sees the bird and take a picture of the bird
182  if (imageRangeFinder[12 + (width * height / 4)] < 0.5) {
183  birdCatched = true;
184  if (rangeFinderSaveImageClient.call(rangeFinderSaveImageSrv) && rangeFinderSaveImageSrv.response.success == 1)
185  ROS_INFO("What a beautifull bird we found here!");
186  else
187  ROS_INFO("Failed to call service save_image to take a picture of the bird.");
188  }
189  }
190  timeStepSrv.request.value = 0;
191  timeStepClient.call(timeStepSrv);
192  n.shutdown();
193 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
static int controllerCount
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::vector< float > imageRangeFinder
static std::string controllerName
int main(int argc, char **argv)
ros::NodeHandle * n
Definition: pioneer3at.cpp:40
ros::ServiceClient timeStepClient
#define ROS_INFO(...)
uint32_t getNumPublishers() const
ROSCPP_DECL bool ok()
#define TIME_STEP
webots_ros::set_int timeStepSrv
unsigned int step
static std::vector< std::string > controllerList
ROSCPP_DECL void shutdown()
std::vector< unsigned char > image
Definition: e_puck_line.cpp:87
void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image)
void controllerNameCallback(const std_msgs::String::ConstPtr &name)
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)


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