ros_ibeo_lux.cpp
Go to the documentation of this file.
1 /*
2  * Unpublished Copyright (c) 2009-2019 AutonomouStuff, LLC, All Rights Reserved.
3  *
4  * This file is part of the ibeo_lux ROS 1.0 driver which is released under the MIT license.
5  * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details.
6  */
7 
11 
12 // Tx
13 #include <network_interface/TCPFrame.h>
14 
15 #include <unordered_map>
16 #include <vector>
17 #include <queue>
18 #include <deque>
19 #include <string>
20 
21 using std::string;
22 using std::vector;
23 using namespace AS::Network;
24 using namespace AS::Drivers::IbeoLux;
25 
28 std::unordered_map<int64_t, ros::Publisher> pub_list;
29 
30 // Main routine
31 int main(int argc, char **argv)
32 {
33  // int c;
34  string ip_address = "192.168.0.1";
35  int port = 12002;
36  string frame_id = "ibeo_lux";
37  bool is_fusion = false;
39  std::deque<uint8_t> grand_buffer;
40  std::queue<std::vector<uint8_t>> messages;
41 
42  // ROS initialization
43  ros::init(argc, argv, "ibeo_lux");
45  ros::NodeHandle priv("~");
46  bool exit = false;
47 
48  if (priv.getParam("ip_address", ip_address))
49  {
50  ROS_INFO("Ibeo LUX - Got ip_address: %s", ip_address.c_str());
51 
52  if (ip_address == "")
53  {
54  ROS_ERROR("Ibeo LUX - IP Address Invalid");
55  exit = true;
56  }
57  }
58 
59  if (priv.getParam("port", port))
60  {
61  ROS_INFO("Ibeo LUX - Got port: %d", port);
62 
63  if (port < 0)
64  {
65  ROS_ERROR("Ibeo LUX - Port Invalid");
66  exit = true;
67  }
68  }
69 
70  if (priv.getParam("is_fusion", is_fusion))
71  {
72  ROS_INFO("Ibeo LUX - Is Fusion ECU: %s", (is_fusion) ? "true" : "false");
73  }
74 
75  if (priv.getParam("sensor_frame_id", frame_id))
76  {
77  ROS_INFO("Ibeo LUX - Got sensor frame ID: %s", frame_id.c_str());
78  }
79 
80  if (exit)
81  return 0;
82 
83  // Advertise messages to send
84  ros::Publisher raw_tcp_pub = n.advertise<network_interface::TCPFrame>("tcp_tx", 1);
85  ros::Publisher pointcloud_pub = n.advertise<pcl::PointCloud <pcl::PointXYZ> >("as_tx/point_cloud", 1);
86  ros::Publisher object_markers_pub = n.advertise<visualization_msgs::MarkerArray>("as_tx/objects", 1);
87  ros::Publisher object_contour_points_pub = n.advertise<visualization_msgs::Marker>("as_tx/object_contour_points", 1);
88 
89  ros::Publisher scan_data_pub, object_data_pub, vehicle_state_pub, error_warn_pub;
90  ros::Publisher fusion_scan_2204_pub,
91  fusion_scan_2205_pub,
92  fusion_object_2225_pub,
93  fusion_object_2280_pub,
94  fusion_img_2403_pub,
95  fusion_vehicle_2806_pub,
96  fusion_vehicle_2807_pub;
97 
98  // LUX Sensor Only
99  if (!is_fusion)
100  {
101  scan_data_pub = n.advertise<ibeo_msgs::ScanData2202>("parsed_tx/scan_data_2202", 1);
102  object_data_pub = n.advertise<ibeo_msgs::ObjectData2221>("parsed_tx/object_data_2221", 1);
103  vehicle_state_pub = n.advertise<ibeo_msgs::HostVehicleState2805>("parsed_tx/host_vehicle_state_2805", 1);
104  error_warn_pub = n.advertise<ibeo_msgs::ErrorWarning>("parsed_tx/error_warning", 1);
105 
106  pub_list.insert(std::make_pair(ErrorWarning::DATA_TYPE, error_warn_pub));
107  pub_list.insert(std::make_pair(ScanData2202::DATA_TYPE, scan_data_pub));
108  pub_list.insert(std::make_pair(ObjectData2221::DATA_TYPE, object_data_pub));
109  pub_list.insert(std::make_pair(HostVehicleState2805::DATA_TYPE, vehicle_state_pub));
110  }
111  else // Fusion ECU Only
112  {
113  fusion_scan_2204_pub = n.advertise<ibeo_msgs::ScanData2204>("parsed_tx/scan_data_2204", 1);
114  fusion_scan_2205_pub = n.advertise<ibeo_msgs::ScanData2205>("parsed_tx/scan_data_2205", 1);
115  fusion_object_2225_pub = n.advertise<ibeo_msgs::ObjectData2225>("parsed_tx/object_data_2225", 1);
116  fusion_object_2280_pub = n.advertise<ibeo_msgs::ObjectData2280>("parsed_tx/object_data_2280", 1);
117  fusion_img_2403_pub = n.advertise<ibeo_msgs::CameraImage>("parsed_tx/camera_image", 1);
118  fusion_vehicle_2806_pub = n.advertise<ibeo_msgs::HostVehicleState2806>("parsed_tx/host_vehicle_state_2806", 1);
119  fusion_vehicle_2807_pub = n.advertise<ibeo_msgs::HostVehicleState2807>("parsed_tx/host_vehicle_state_2807", 1);
120 
121  pub_list.insert(std::make_pair(ScanData2204::DATA_TYPE, fusion_scan_2204_pub));
122  pub_list.insert(std::make_pair(ScanData2205::DATA_TYPE, fusion_scan_2205_pub));
123  pub_list.insert(std::make_pair(ObjectData2225::DATA_TYPE, fusion_object_2225_pub));
124  pub_list.insert(std::make_pair(ObjectData2280::DATA_TYPE, fusion_object_2280_pub));
125  pub_list.insert(std::make_pair(CameraImage::DATA_TYPE, fusion_img_2403_pub));
126  pub_list.insert(std::make_pair(HostVehicleState2806::DATA_TYPE, fusion_vehicle_2806_pub));
127  pub_list.insert(std::make_pair(HostVehicleState2807::DATA_TYPE, fusion_vehicle_2807_pub));
128  }
129 
130  // Wait for time to be valid
132 
133  network_interface::TCPFrame tcp_raw_msg;
134 
135  ReturnStatuses status;
136 
137  ros::Rate loop_rate = (is_fusion) ? ros::Rate(1100) : ros::Rate(40);
138  // Loop as long as module should run
139 
140  bool fusion_filter_sent = false;
141 
142  while (ros::ok())
143  {
144  if (!tcp_interface.is_open())
145  {
146  if (is_fusion)
147  fusion_filter_sent = false;
148 
149  status = tcp_interface.open(ip_address.c_str(), port);
150 
151  if (status != ReturnStatuses::OK)
152  ROS_WARN("Ibeo LUX - Unable to connect to sensor at %s: %d - %s",
153  ip_address.c_str(),
154  static_cast<int>(status), return_status_desc(status).c_str());
155 
156  ros::Duration(1.0).sleep();
157  }
158  else
159  {
160  if (is_fusion && !fusion_filter_sent)
161  {
162  std::vector<uint8_t> set_filter_cmd =
163  {0xaf, 0xfe, 0xc0, 0xc2,
164  0x00, 0x00, 0x00, 0x00,
165  0x00, 0x00, 0x00, 0x08,
166  0x00, 0x00, 0x20, 0x10,
167  0x00, 0x00, 0x00, 0x00,
168  0x00, 0x00, 0x00, 0x00,
169  0x00, 0x05, 0x00, 0x02,
170  0x00, 0x00, 0xff, 0xff};
171 
172  ROS_INFO_THROTTLE(3, "Ibeo LUX - Sending Fusion filter command to begin transmission.");
173 
174  status = tcp_interface.write(set_filter_cmd);
175 
176  if (status != ReturnStatuses::OK)
177  ROS_ERROR_THROTTLE(3, "Ibeo LUX - Failed to send Fusion filter command.");
178  else
179  fusion_filter_sent = true;
180 
181  ros::Duration(0.5).sleep();
182  }
183  else
184  {
185  buf_size = IBEO_PAYLOAD_SIZE;
186  std::vector<uint8_t> msg_buf;
187  msg_buf.reserve(buf_size);
188 
189  status = tcp_interface.read(&msg_buf); // Read a (big) chunk.
190 
191  if (status != ReturnStatuses::OK && status != ReturnStatuses::NO_MESSAGES_RECEIVED)
192  {
193  ROS_WARN("Ibeo ScaLa - Failed to read from socket: %d - %s",
194  static_cast<int>(status),
195  return_status_desc(status).c_str());
196  }
197  else if (status == ReturnStatuses::OK)
198  {
199  buf_size = msg_buf.size();
200  grand_buffer.insert(grand_buffer.end(), msg_buf.begin(), msg_buf.end());
201 
202  int first_mw = 0;
203 
204  while (true)
205  {
206  first_mw = find_magic_word(grand_buffer, MAGIC_WORD);
207 
208  if (first_mw == -1) // no magic word found. move along.
209  {
210  break;
211  }
212  else // magic word found. pull out message from grand buffer and add it to the message list.
213  {
214  if (first_mw > 0)
215  grand_buffer.erase(
216  grand_buffer.begin(),
217  grand_buffer.begin() + first_mw); // Unusable data in beginning of buffer.
218 
219  // From here on, the detected Magic Word should be at the beginning of the grand_buffer.
220 
222  header.parse(std::vector<uint8_t>(grand_buffer.begin(), grand_buffer.begin() + IBEO_HEADER_SIZE));
223 
224  auto total_msg_size = IBEO_HEADER_SIZE + header.message_size;
225 
226  if (grand_buffer.size() < total_msg_size)
227  break; // Incomplete message left in grand buffer. Wait for next read.
228 
229  std::vector<uint8_t> msg(
230  grand_buffer.begin(),
231  grand_buffer.begin() + total_msg_size);
232  messages.push(msg);
233  grand_buffer.erase(
234  grand_buffer.begin(),
235  grand_buffer.begin() + total_msg_size);
236  }
237 
238  if (!ros::ok())
239  break;
240  }
241  }
242 
243  while (!messages.empty())
244  {
245  auto message = messages.front();
246 
247  network_interface::TCPFrame raw_frame;
248  raw_frame.address = ip_address;
249  raw_frame.port = port;
250  raw_frame.size = message.size();
251  raw_frame.data.insert(raw_frame.data.end(), message.begin(), message.end());
252  raw_frame.header.frame_id = frame_id;
253  raw_frame.header.stamp = ros::Time::now();
254 
255  raw_tcp_pub.publish(raw_frame);
256 
257  IbeoDataHeader ibeo_header;
258  ibeo_header.parse(message);
259 
260  // Instantiate a parser class of the correct type.
261  auto class_parser = IbeoTxMessage::make_message(ibeo_header.data_type_id);
262  // Look up the message publisher for this type.
263  auto pub = pub_list.find(ibeo_header.data_type_id);
264 
265  // Only parse message types we know how to handle.
266  if (class_parser != NULL && pub != pub_list.end())
267  {
268  // Parse the raw data into the class.
269  class_parser->parse(message);
270  // Create a new message of the correct type and publish it.
271  handler.fillAndPublish(ibeo_header.data_type_id, frame_id, pub->second, class_parser.get());
272 
273  if (class_parser->has_scan_points)
274  {
275  pcl::PointCloud<pcl::PointXYZL> pcl_cloud;
276  pcl_cloud.header.frame_id = frame_id;
277  // pcl_cloud.header.stamp = ibeo_header.time;
278  pcl_conversions::toPCL(ros::Time::now(), pcl_cloud.header.stamp);
279  std::vector<Point3DL> scan_points = class_parser->get_scan_points();
280  handler.fillPointcloud(scan_points, &pcl_cloud);
281  pointcloud_pub.publish(pcl_cloud);
282  }
283 
284  if (class_parser->has_contour_points)
285  {
286  visualization_msgs::Marker marker;
287  marker.header.frame_id = frame_id;
288  marker.header.stamp = ros::Time::now();
289  std::vector<Point3D> contour_points = class_parser->get_contour_points();
290 
291  if (contour_points.size() > 0)
292  {
293  handler.fillContourPoints(contour_points, &marker, frame_id);
294  object_contour_points_pub.publish(marker);
295  }
296  }
297 
298  if (class_parser->has_objects)
299  {
300  std::vector<IbeoObject> objects = class_parser->get_objects();
301  visualization_msgs::MarkerArray marker_array;
302  handler.fillMarkerArray(objects, &marker_array, frame_id);
303 
304  for (visualization_msgs::Marker m : marker_array.markers)
305  {
306  m.header.frame_id = frame_id;
307  }
308 
309  object_markers_pub.publish(marker_array);
310  }
311  }
312 
313  messages.pop();
314  } // Message parse loop
315  } // If fusion filter sent or != fusion
316  } // If sensor is connected
317 
318  loop_rate.sleep();
319  // ros::spinOnce(); // No callbacks yet
320  }
321 
322  status = tcp_interface.close();
323 
324  if (status != ReturnStatuses::OK)
325  ROS_ERROR(
326  "Ibeo LUX - Closing the connection to the LUX failed: %i - %s",
327  static_cast<int>(status),
328  return_status_desc(status).c_str());
329 
330  return 0;
331 }
void fillPointcloud(const std::vector< Point3DL > &points, pcl::PointCloud< pcl::PointXYZL > *new_msg)
const int32_t IBEO_PAYLOAD_SIZE
const uint8_t IBEO_HEADER_SIZE
void publish(const boost::shared_ptr< M > &message) const
int main(int argc, char **argv)
void fillAndPublish(const uint8_t &type_id, const std::string &frame_id, const ros::Publisher &pub, IbeoTxMessage *parser_class)
void fillContourPoints(const std::vector< Point3D > &points, visualization_msgs::Marker *new_msg, const std::string &frame_id)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void fillMarkerArray(const std::vector< IbeoObject > &objects, visualization_msgs::MarkerArray *new_msg, const std::string &frame_id)
#define ROS_WARN(...)
return_statuses open(const char *ip_address, const int &port)
std::string return_status_desc(const return_statuses &ret)
#define ROS_INFO_THROTTLE(period,...)
std::unordered_map< int64_t, ros::Publisher > pub_list
const size_t MAGIC_WORD
#define ROS_INFO(...)
#define ROS_ERROR_THROTTLE(period,...)
ROSCPP_DECL bool ok()
TCPInterface tcp_interface
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
IbeoLuxRosMsgHandler handler
bool sleep()
return_statuses close()
int32_t find_magic_word(uint8_t *in, uint32_t buf_size, size_t magic_word)
bool getParam(const std::string &key, std::string &s) const
static Time now()
const std::string header
static bool waitForValid()
#define ROS_ERROR(...)
void parse(const std::vector< uint8_t > &in)
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
return_statuses write(unsigned char *msg, const size_t &msg_size)
return_statuses read(unsigned char *msg, const size_t &buf_size, size_t &bytes_read, int timeout_ms=0)


ibeo_lux
Author(s): Joe Kale , Joshua Whitley
autogenerated on Sat Jun 8 2019 04:47:04