convert.cpp
Go to the documentation of this file.
1 // Copyright 2018 Open Source Robotics Foundation, Inc.
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 <algorithm>
16 #include <exception>
17 #include <ros/console.h>
18 
19 #include "ros_ign_bridge/convert.hpp"
20 
21 namespace ros_ign_bridge
22 {
23 
24 // This can be used to replace `::` with `/` to make frame_id compatible with TF
25 std::string replace_delimiter(const std::string &input,
26  const std::string &old_delim,
27  const std::string new_delim)
28 {
29  std::string output;
30  output.reserve(input.size());
31 
32  std::size_t last_pos = 0;
33 
34  while (last_pos < input.size())
35  {
36  std::size_t pos = input.find(old_delim, last_pos);
37  output += input.substr(last_pos, pos - last_pos);
38  if (pos != std::string::npos)
39  {
40  output += new_delim;
41  pos += old_delim.size();
42  }
43 
44  last_pos = pos;
45  }
46 
47  return output;
48 }
49 
50 // Frame id from ROS to ign is not supported right now
51 // std::string frame_id_ros_to_ign(const std::string &frame_id)
52 // {
53 // return replace_delimiter(frame_id, "/", "::");
54 // }
55 
56 std::string frame_id_ign_to_ros(const std::string &frame_id)
57 {
58  return replace_delimiter(frame_id, "::", "/");
59 }
60 
61 template<>
62 void
63 convert_ros_to_ign(
64  const std_msgs::Bool & ros_msg,
65  ignition::msgs::Boolean & ign_msg)
66 {
67  ign_msg.set_data(ros_msg.data);
68 }
69 
70 template<>
71 void
72 convert_ign_to_ros(
73  const ignition::msgs::Boolean & ign_msg,
74  std_msgs::Bool & ros_msg)
75 {
76  ros_msg.data = ign_msg.data();
77 }
78 
79 template<>
80 void
81 convert_ros_to_ign(
82  const std_msgs::ColorRGBA & ros_msg,
83  ignition::msgs::Color & ign_msg)
84 {
85  ign_msg.set_r(ros_msg.r);
86  ign_msg.set_g(ros_msg.g);
87  ign_msg.set_b(ros_msg.b);
88  ign_msg.set_a(ros_msg.a);
89 }
90 
91 template<>
92 void
93 convert_ign_to_ros(
94  const ignition::msgs::Color & ign_msg,
95  std_msgs::ColorRGBA & ros_msg)
96 {
97  ros_msg.r = ign_msg.r();
98  ros_msg.g = ign_msg.g();
99  ros_msg.b = ign_msg.b();
100  ros_msg.a = ign_msg.a();
101 }
102 
103 template<>
104 void
105 convert_ros_to_ign(
106  const std_msgs::Empty &,
107  ignition::msgs::Empty &)
108 {
109 }
110 
111 template<>
112 void
113 convert_ign_to_ros(
114  const ignition::msgs::Empty &,
115  std_msgs::Empty &)
116 {
117 }
118 
119 template<>
120 void
121 convert_ros_to_ign(
122  const std_msgs::Int32 & ros_msg,
123  ignition::msgs::Int32 & ign_msg)
124 {
125  ign_msg.set_data(ros_msg.data);
126 }
127 
128 template<>
129 void
130 convert_ign_to_ros(
131  const ignition::msgs::Int32 & ign_msg,
132  std_msgs::Int32 & ros_msg)
133 {
134  ros_msg.data = ign_msg.data();
135 }
136 
137 template<>
138 void
139 convert_ros_to_ign(
140  const std_msgs::Float32 & ros_msg,
141  ignition::msgs::Float & ign_msg)
142 {
143  ign_msg.set_data(ros_msg.data);
144 }
145 
146 template<>
147 void
148 convert_ign_to_ros(
149  const ignition::msgs::Float & ign_msg,
150  std_msgs::Float32 & ros_msg)
151 {
152  ros_msg.data = ign_msg.data();
153 }
154 
155 template<>
156 void
157 convert_ros_to_ign(
158  const std_msgs::Float64 & ros_msg,
159  ignition::msgs::Double & ign_msg)
160 {
161  ign_msg.set_data(ros_msg.data);
162 }
163 
164 template<>
165 void
166 convert_ign_to_ros(
167  const ignition::msgs::Double & ign_msg,
168  std_msgs::Float64 & ros_msg)
169 {
170  ros_msg.data = ign_msg.data();
171 }
172 
173 template<>
174 void
175 convert_ros_to_ign(
176  const std_msgs::Header & ros_msg,
177  ignition::msgs::Header & ign_msg)
178 {
179  ign_msg.mutable_stamp()->set_sec(ros_msg.stamp.sec);
180  ign_msg.mutable_stamp()->set_nsec(ros_msg.stamp.nsec);
181  auto newPair = ign_msg.add_data();
182  newPair->set_key("seq");
183  newPair->add_value(std::to_string(ros_msg.seq));
184  newPair = ign_msg.add_data();
185  newPair->set_key("frame_id");
186  newPair->add_value(ros_msg.frame_id);
187 }
188 
189 template<>
190 void
191 convert_ign_to_ros(
192  const ignition::msgs::Header & ign_msg,
193  std_msgs::Header & ros_msg)
194 {
195  ros_msg.stamp = ros::Time(ign_msg.stamp().sec(), ign_msg.stamp().nsec());
196  for (auto i = 0; i < ign_msg.data_size(); ++i)
197  {
198  auto aPair = ign_msg.data(i);
199  if (aPair.key() == "seq" && aPair.value_size() > 0)
200  {
201  std::string value = aPair.value(0);
202  try
203  {
204  unsigned long ul = std::stoul(value, nullptr);
205  ros_msg.seq = ul;
206  }
207  catch (std::exception & e)
208  {
209  ROS_ERROR_STREAM("Exception converting [" << value << "] to an "
210  << "unsigned int" << std::endl);
211  }
212  }
213  else if (aPair.key() == "frame_id" && aPair.value_size() > 0)
214  {
215  ros_msg.frame_id = frame_id_ign_to_ros(aPair.value(0));
216  }
217  }
218 }
219 
220 template<>
221 void
222 convert_ros_to_ign(
223  const std_msgs::String & ros_msg,
224  ignition::msgs::StringMsg & ign_msg)
225 {
226  ign_msg.set_data(ros_msg.data);
227 }
228 
229 template<>
230 void
231 convert_ign_to_ros(
232  const ignition::msgs::StringMsg & ign_msg,
233  std_msgs::String & ros_msg)
234 {
235  ros_msg.data = ign_msg.data();
236 }
237 
238 template<>
239 void
240 convert_ign_to_ros(
241  const ignition::msgs::Clock & ign_msg,
242  rosgraph_msgs::Clock & ros_msg)
243 {
244  ros_msg.clock = ros::Time(ign_msg.sim().sec(), ign_msg.sim().nsec());
245 }
246 
247 template<>
248 void
249 convert_ros_to_ign(
250  const rosgraph_msgs::Clock & ros_msg,
251  ignition::msgs::Clock & ign_msg)
252 {
253  ign_msg.mutable_sim()->set_sec(ros_msg.clock.sec);
254  ign_msg.mutable_sim()->set_nsec(ros_msg.clock.nsec);
255 }
256 
257 template<>
258 void
259 convert_ros_to_ign(
260  const geometry_msgs::Quaternion & ros_msg,
261  ignition::msgs::Quaternion & ign_msg)
262 {
263  ign_msg.set_x(ros_msg.x);
264  ign_msg.set_y(ros_msg.y);
265  ign_msg.set_z(ros_msg.z);
266  ign_msg.set_w(ros_msg.w);
267 }
268 
269 template<>
270 void
271 convert_ign_to_ros(
272  const ignition::msgs::Quaternion & ign_msg,
273  geometry_msgs::Quaternion & ros_msg)
274 {
275  ros_msg.x = ign_msg.x();
276  ros_msg.y = ign_msg.y();
277  ros_msg.z = ign_msg.z();
278  ros_msg.w = ign_msg.w();
279 }
280 
281 template<>
282 void
283 convert_ros_to_ign(
284  const geometry_msgs::Vector3 & ros_msg,
285  ignition::msgs::Vector3d & ign_msg)
286 {
287  ign_msg.set_x(ros_msg.x);
288  ign_msg.set_y(ros_msg.y);
289  ign_msg.set_z(ros_msg.z);
290 }
291 
292 template<>
293 void
294 convert_ign_to_ros(
295  const ignition::msgs::Vector3d & ign_msg,
296  geometry_msgs::Vector3 & ros_msg)
297 {
298  ros_msg.x = ign_msg.x();
299  ros_msg.y = ign_msg.y();
300  ros_msg.z = ign_msg.z();
301 }
302 
303 template<>
304 void
305 convert_ros_to_ign(
306  const geometry_msgs::Point & ros_msg,
307  ignition::msgs::Vector3d & ign_msg)
308 {
309  ign_msg.set_x(ros_msg.x);
310  ign_msg.set_y(ros_msg.y);
311  ign_msg.set_z(ros_msg.z);
312 }
313 
314 template<>
315 void
316 convert_ign_to_ros(
317  const ignition::msgs::Vector3d & ign_msg,
318  geometry_msgs::Point & ros_msg)
319 {
320  ros_msg.x = ign_msg.x();
321  ros_msg.y = ign_msg.y();
322  ros_msg.z = ign_msg.z();
323 }
324 
325 template<>
326 void
327 convert_ros_to_ign(
328  const geometry_msgs::Pose & ros_msg,
329  ignition::msgs::Pose & ign_msg)
330 {
331  convert_ros_to_ign(ros_msg.position, *ign_msg.mutable_position());
332  convert_ros_to_ign(ros_msg.orientation, *ign_msg.mutable_orientation());
333 }
334 
335 template<>
336 void
337 convert_ign_to_ros(
338  const ignition::msgs::Pose & ign_msg,
339  geometry_msgs::Pose & ros_msg)
340 {
341  convert_ign_to_ros(ign_msg.position(), ros_msg.position);
342  convert_ign_to_ros(ign_msg.orientation(), ros_msg.orientation);
343 }
344 
345 template<>
346 void
347 convert_ros_to_ign(
348  const geometry_msgs::PoseArray & ros_msg,
349  ignition::msgs::Pose_V & ign_msg)
350 {
351  ign_msg.clear_pose();
352  for (auto const &t : ros_msg.poses)
353  {
354  auto p = ign_msg.add_pose();
355  convert_ros_to_ign(t, *p);
356  }
357 
358  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
359 }
360 
361 template<>
362 void
363 convert_ign_to_ros(
364  const ignition::msgs::Pose_V & ign_msg,
365  geometry_msgs::PoseArray & ros_msg)
366 {
367  ros_msg.poses.clear();
368  for (auto const &p : ign_msg.pose())
369  {
370  geometry_msgs::Pose pose;
371  convert_ign_to_ros(p, pose);
372  ros_msg.poses.push_back(pose);
373  }
374  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
375 }
376 
377 template<>
378 void
379 convert_ros_to_ign(
380  const geometry_msgs::PoseStamped & ros_msg,
381  ignition::msgs::Pose & ign_msg)
382 {
383  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
384  convert_ros_to_ign(ros_msg.pose, ign_msg);
385 }
386 
387 template<>
388 void
389 convert_ign_to_ros(
390  const ignition::msgs::Pose & ign_msg,
391  geometry_msgs::PoseStamped & ros_msg)
392 {
393  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
394  convert_ign_to_ros(ign_msg, ros_msg.pose);
395 }
396 
397 template<>
398 void
399 convert_ros_to_ign(
400  const geometry_msgs::Transform & ros_msg,
401  ignition::msgs::Pose & ign_msg)
402 {
403  convert_ros_to_ign(ros_msg.translation , *ign_msg.mutable_position());
404  convert_ros_to_ign(ros_msg.rotation, *ign_msg.mutable_orientation());
405 }
406 
407 template<>
408 void
409 convert_ign_to_ros(
410  const ignition::msgs::Pose & ign_msg,
411  geometry_msgs::Transform & ros_msg)
412 {
413  convert_ign_to_ros(ign_msg.position(), ros_msg.translation);
414  convert_ign_to_ros(ign_msg.orientation(), ros_msg.rotation);
415 }
416 
417 template<>
418 void
419 convert_ros_to_ign(
420  const geometry_msgs::TransformStamped & ros_msg,
421  ignition::msgs::Pose & ign_msg)
422 {
423  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
424  convert_ros_to_ign(ros_msg.transform, ign_msg);
425 
426  auto newPair = ign_msg.mutable_header()->add_data();
427  newPair->set_key("child_frame_id");
428  newPair->add_value(ros_msg.child_frame_id);
429 }
430 
431 template<>
432 void
433 convert_ign_to_ros(
434  const ignition::msgs::Pose & ign_msg,
435  geometry_msgs::TransformStamped & ros_msg)
436 {
437  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
438  convert_ign_to_ros(ign_msg, ros_msg.transform);
439  for (auto i = 0; i < ign_msg.header().data_size(); ++i)
440  {
441  auto aPair = ign_msg.header().data(i);
442  if (aPair.key() == "child_frame_id" && aPair.value_size() > 0)
443  {
444  ros_msg.child_frame_id = frame_id_ign_to_ros(aPair.value(0));
445  break;
446  }
447  }
448 }
449 
450 template<>
451 void
452 convert_ros_to_ign(
453  const tf2_msgs::TFMessage & ros_msg,
454  ignition::msgs::Pose_V & ign_msg)
455 {
456  ign_msg.clear_pose();
457  for (auto const &t : ros_msg.transforms)
458  {
459  auto p = ign_msg.add_pose();
460  convert_ros_to_ign(t, *p);
461  }
462 
463  if (!ros_msg.transforms.empty())
464  {
465  convert_ros_to_ign(ros_msg.transforms[0].header,
466  (*ign_msg.mutable_header()));
467  }
468 }
469 
470 template<>
471 void
472 convert_ign_to_ros(
473  const ignition::msgs::Pose_V & ign_msg,
474  tf2_msgs::TFMessage & ros_msg)
475 {
476  ros_msg.transforms.clear();
477  for (auto const &p : ign_msg.pose())
478  {
479  geometry_msgs::TransformStamped tf;
480  convert_ign_to_ros(p, tf);
481  ros_msg.transforms.push_back(tf);
482  }
483 }
484 
485 template<>
486 void
487 convert_ros_to_ign(
488  const geometry_msgs::Twist & ros_msg,
489  ignition::msgs::Twist & ign_msg)
490 {
491  convert_ros_to_ign(ros_msg.linear, (*ign_msg.mutable_linear()));
492  convert_ros_to_ign(ros_msg.angular, (*ign_msg.mutable_angular()));
493 }
494 
495 template<>
496 void
497 convert_ign_to_ros(
498  const ignition::msgs::Twist & ign_msg,
499  geometry_msgs::Twist & ros_msg)
500 {
501  convert_ign_to_ros(ign_msg.linear(), ros_msg.linear);
502  convert_ign_to_ros(ign_msg.angular(), ros_msg.angular);
503 }
504 
505 // template<>
506 // void
507 // convert_ros_to_ign(
508 // const mav_msgs::Actuators & ros_msg,
509 // ignition::msgs::Actuators & ign_msg)
510 // {
511 // convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
512 //
513 // for (auto i = 0u; i < ros_msg.angles.size(); ++i)
514 // ign_msg.add_position(ros_msg.angles[i]);
515 // for (auto i = 0u; i < ros_msg.angular_velocities.size(); ++i)
516 // ign_msg.add_velocity(ros_msg.angular_velocities[i]);
517 // for (auto i = 0u; i < ros_msg.normalized.size(); ++i)
518 // ign_msg.add_normalized(ros_msg.normalized[i]);
519 // }
520 //
521 // template<>
522 // void
523 // convert_ign_to_ros(
524 // const ignition::msgs::Actuators & ign_msg,
525 // mav_msgs::Actuators & ros_msg)
526 // {
527 // convert_ign_to_ros(ign_msg.header(), ros_msg.header);
528 //
529 // for (auto i = 0; i < ign_msg.position_size(); ++i)
530 // ros_msg.angles.push_back(ign_msg.position(i));
531 // for (auto i = 0; i < ign_msg.velocity_size(); ++i)
532 // ros_msg.angular_velocities.push_back(ign_msg.velocity(i));
533 // for (auto i = 0; i < ign_msg.normalized_size(); ++i)
534 // ros_msg.normalized.push_back(ign_msg.normalized(i));
535 // }
536 
537 template<>
538 void
539 convert_ros_to_ign(
540  const nav_msgs::OccupancyGrid & ros_msg,
541  ignition::msgs::OccupancyGrid & ign_msg)
542 {
543  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
544 
545  ign_msg.mutable_info()->mutable_map_load_time()->set_sec(
546  ros_msg.info.map_load_time.sec);
547  ign_msg.mutable_info()->mutable_map_load_time()->set_nsec(
548  ros_msg.info.map_load_time.nsec);
549 
550  ign_msg.mutable_info()->set_resolution(
551  ros_msg.info.resolution);
552  ign_msg.mutable_info()->set_width(
553  ros_msg.info.width);
554  ign_msg.mutable_info()->set_height(
555  ros_msg.info.height);
556 
557  convert_ros_to_ign(ros_msg.info.origin,
558  (*ign_msg.mutable_info()->mutable_origin()));
559 
560  ign_msg.set_data(&ros_msg.data[0], ros_msg.data.size());
561 }
562 
563 template<>
564 void
565 convert_ign_to_ros(
566  const ignition::msgs::OccupancyGrid & ign_msg,
567  nav_msgs::OccupancyGrid & ros_msg)
568 {
569  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
570 
571  ros_msg.info.map_load_time.sec = ign_msg.info().map_load_time().sec();
572  ros_msg.info.map_load_time.nsec = ign_msg.info().map_load_time().nsec();
573  ros_msg.info.resolution = ign_msg.info().resolution();
574  ros_msg.info.width = ign_msg.info().width();
575  ros_msg.info.height = ign_msg.info().height();
576 
577  convert_ign_to_ros(ign_msg.info().origin(), ros_msg.info.origin);
578 
579  ros_msg.data.resize(ign_msg.data().size());
580  memcpy(&ros_msg.data[0], ign_msg.data().c_str(), ign_msg.data().size());
581 }
582 
583 template<>
584 void
585 convert_ros_to_ign(
586  const nav_msgs::Odometry & ros_msg,
587  ignition::msgs::Odometry & ign_msg)
588 {
589  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
590  convert_ros_to_ign(ros_msg.pose.pose, (*ign_msg.mutable_pose()));
591  convert_ros_to_ign(ros_msg.twist.twist, (*ign_msg.mutable_twist()));
592 
593  auto childFrame = ign_msg.mutable_header()->add_data();
594  childFrame->set_key("child_frame_id");
595  childFrame->add_value(ros_msg.child_frame_id);
596 }
597 
598 template<>
599 void
600 convert_ign_to_ros(
601  const ignition::msgs::Odometry & ign_msg,
602  nav_msgs::Odometry & ros_msg)
603 {
604  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
605  convert_ign_to_ros(ign_msg.pose(), ros_msg.pose.pose);
606  convert_ign_to_ros(ign_msg.twist(), ros_msg.twist.twist);
607 
608  for (auto i = 0; i < ign_msg.header().data_size(); ++i)
609  {
610  auto aPair = ign_msg.header().data(i);
611  if (aPair.key() == "child_frame_id" && aPair.value_size() > 0)
612  {
613  ros_msg.child_frame_id = frame_id_ign_to_ros(aPair.value(0));
614  break;
615  }
616  }
617 }
618 
619 template<>
620 void
621 convert_ros_to_ign(
622  const sensor_msgs::FluidPressure & ros_msg,
623  ignition::msgs::FluidPressure & ign_msg)
624 {
625  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
626  ign_msg.set_pressure(ros_msg.fluid_pressure);
627  ign_msg.set_variance(ros_msg.variance);
628 }
629 
630 template<>
631 void
632 convert_ign_to_ros(
633  const ignition::msgs::FluidPressure & ign_msg,
634  sensor_msgs::FluidPressure & ros_msg)
635 {
636  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
637  ros_msg.fluid_pressure = ign_msg.pressure();
638  ros_msg.variance = ign_msg.variance();
639 }
640 
641 template<>
642 void
643 convert_ros_to_ign(
644  const sensor_msgs::Image & ros_msg,
645  ignition::msgs::Image & ign_msg)
646 {
647  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
648 
649  ign_msg.set_width(ros_msg.width);
650  ign_msg.set_height(ros_msg.height);
651 
652  unsigned int num_channels;
653  unsigned int octets_per_channel;
654 
655  if (ros_msg.encoding == "mono8")
656  {
657  ign_msg.set_pixel_format_type(
658  ignition::msgs::PixelFormatType::L_INT8);
659  num_channels = 1;
660  octets_per_channel = 1u;
661  }
662  else if (ros_msg.encoding == "mono16")
663  {
664  ign_msg.set_pixel_format_type(
665  ignition::msgs::PixelFormatType::L_INT16);
666  num_channels = 1;
667  octets_per_channel = 2u;
668  }
669  else if (ros_msg.encoding == "rgb8")
670  {
671  ign_msg.set_pixel_format_type(
672  ignition::msgs::PixelFormatType::RGB_INT8);
673  num_channels = 3;
674  octets_per_channel = 1u;
675  }
676  else if (ros_msg.encoding == "rgba8")
677  {
678  ign_msg.set_pixel_format_type(
679  ignition::msgs::PixelFormatType::RGBA_INT8);
680  num_channels = 4;
681  octets_per_channel = 1u;
682  }
683  else if (ros_msg.encoding == "bgra8")
684  {
685  ign_msg.set_pixel_format_type(
686  ignition::msgs::PixelFormatType::BGRA_INT8);
687  num_channels = 4;
688  octets_per_channel = 1u;
689  }
690  else if (ros_msg.encoding == "rgb16")
691  {
692  ign_msg.set_pixel_format_type(
693  ignition::msgs::PixelFormatType::RGB_INT16);
694  num_channels = 3;
695  octets_per_channel = 2u;
696  }
697  else if (ros_msg.encoding == "bgr8")
698  {
699  ign_msg.set_pixel_format_type(
700  ignition::msgs::PixelFormatType::BGR_INT8);
701  num_channels = 3;
702  octets_per_channel = 1u;
703  }
704  else if (ros_msg.encoding == "bgr16")
705  {
706  ign_msg.set_pixel_format_type(
707  ignition::msgs::PixelFormatType::BGR_INT16);
708  num_channels = 3;
709  octets_per_channel = 2u;
710  }
711  else if (ros_msg.encoding == "32FC1")
712  {
713  ign_msg.set_pixel_format_type(
714  ignition::msgs::PixelFormatType::R_FLOAT32);
715  num_channels = 1;
716  octets_per_channel = 4u;
717  }
718  else
719  {
720  ign_msg.set_pixel_format_type(
721  ignition::msgs::PixelFormatType::UNKNOWN_PIXEL_FORMAT);
722  ROS_ERROR_STREAM("Unsupported pixel format [" << ros_msg.encoding << "]"
723  << std::endl);
724  return;
725  }
726 
727  ign_msg.set_step(ign_msg.width() * num_channels * octets_per_channel);
728 
729  ign_msg.set_data(&(ros_msg.data[0]), ign_msg.step() * ign_msg.height());
730 }
731 
732 template<>
733 void
734 convert_ign_to_ros(
735  const ignition::msgs::Image & ign_msg,
736  sensor_msgs::Image & ros_msg)
737 {
738  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
739 
740  ros_msg.height = ign_msg.height();
741  ros_msg.width = ign_msg.width();
742 
743  unsigned int num_channels;
744  unsigned int octets_per_channel;
745 
746  if (ign_msg.pixel_format_type() ==
747  ignition::msgs::PixelFormatType::L_INT8)
748  {
749  ros_msg.encoding = "mono8";
750  num_channels = 1;
751  octets_per_channel = 1u;
752  }
753  else if (ign_msg.pixel_format_type() ==
754  ignition::msgs::PixelFormatType::L_INT16)
755  {
756  ros_msg.encoding = "mono16";
757  num_channels = 1;
758  octets_per_channel = 2u;
759  }
760  else if (ign_msg.pixel_format_type() ==
761  ignition::msgs::PixelFormatType::RGB_INT8)
762  {
763  ros_msg.encoding = "rgb8";
764  num_channels = 3;
765  octets_per_channel = 1u;
766  }
767  else if (ign_msg.pixel_format_type() ==
768  ignition::msgs::PixelFormatType::RGBA_INT8)
769  {
770  ros_msg.encoding = "rgba8";
771  num_channels = 4;
772  octets_per_channel = 1u;
773  }
774  else if (ign_msg.pixel_format_type() ==
775  ignition::msgs::PixelFormatType::BGRA_INT8)
776  {
777  ros_msg.encoding = "bgra8";
778  num_channels = 4;
779  octets_per_channel = 1u;
780  }
781  else if (ign_msg.pixel_format_type() ==
782  ignition::msgs::PixelFormatType::RGB_INT16)
783  {
784  ros_msg.encoding = "rgb16";
785  num_channels = 3;
786  octets_per_channel = 2u;
787  }
788  else if (ign_msg.pixel_format_type() ==
789  ignition::msgs::PixelFormatType::BGR_INT8)
790  {
791  ros_msg.encoding = "bgr8";
792  num_channels = 3;
793  octets_per_channel = 1u;
794  }
795  else if (ign_msg.pixel_format_type() ==
796  ignition::msgs::PixelFormatType::BGR_INT16)
797  {
798  ros_msg.encoding = "bgr16";
799  num_channels = 3;
800  octets_per_channel = 2u;
801  }
802  else if (ign_msg.pixel_format_type() ==
803  ignition::msgs::PixelFormatType::R_FLOAT32)
804  {
805  ros_msg.encoding = "32FC1";
806  num_channels = 1;
807  octets_per_channel = 4u;
808  }
809  else
810  {
811  ROS_ERROR_STREAM("Unsupported pixel format ["
812  << ign_msg.pixel_format_type() << "]" << std::endl);
813  return;
814  }
815 
816  ros_msg.is_bigendian = false;
817  ros_msg.step = ros_msg.width * num_channels * octets_per_channel;
818 
819  auto count = ros_msg.step * ros_msg.height;
820  ros_msg.data.resize(ros_msg.step * ros_msg.height);
821  std::copy(
822  ign_msg.data().begin(),
823  ign_msg.data().begin() + count,
824  ros_msg.data.begin());
825 }
826 
827 template<>
828 void
829 convert_ros_to_ign(
830  const sensor_msgs::CameraInfo & ros_msg,
831  ignition::msgs::CameraInfo & ign_msg)
832 {
833  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
834 
835  ign_msg.set_width(ros_msg.width);
836  ign_msg.set_height(ros_msg.height);
837 
838  auto distortion = ign_msg.mutable_distortion();
839  if (ros_msg.distortion_model == "plumb_bob")
840  {
841  distortion->set_model(ignition::msgs::CameraInfo::Distortion::PLUMB_BOB);
842  }
843  else if (ros_msg.distortion_model == "rational_polynomial")
844  {
845  distortion->set_model(ignition::msgs::CameraInfo::Distortion::RATIONAL_POLYNOMIAL);
846  }
847  else if (ros_msg.distortion_model == "equidistant")
848  {
849  distortion->set_model(ignition::msgs::CameraInfo::Distortion::EQUIDISTANT);
850  }
851  else
852  {
853  ROS_ERROR_STREAM("Unsupported distortion model ["
854  << ros_msg.distortion_model << "]" << std::endl);
855  }
856  for (auto i = 0u; i < ros_msg.D.size(); ++i)
857  {
858  distortion->add_k(ros_msg.D[i]);
859  }
860 
861  auto intrinsics = ign_msg.mutable_intrinsics();
862  for (auto i = 0u; i < ros_msg.K.size(); ++i)
863  {
864  intrinsics->add_k(ros_msg.K[i]);
865  }
866 
867  auto projection = ign_msg.mutable_projection();
868  for (auto i = 0u; i < ros_msg.P.size(); ++i)
869  {
870  projection->add_p(ros_msg.P[i]);
871  }
872 
873  for (auto i = 0u; i < ros_msg.R.size(); ++i)
874  {
875  ign_msg.add_rectification_matrix(ros_msg.R[i]);
876  }
877 }
878 
879 template<>
880 void
881 convert_ign_to_ros(
882  const ignition::msgs::CameraInfo & ign_msg,
883  sensor_msgs::CameraInfo & ros_msg)
884 {
885  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
886 
887  ros_msg.height = ign_msg.height();
888  ros_msg.width = ign_msg.width();
889 
890  auto distortion = ign_msg.distortion();
891  if (ign_msg.has_distortion())
892  {
893  auto distortion = ign_msg.distortion();
894  if (distortion.model() ==
895  ignition::msgs::CameraInfo::Distortion::PLUMB_BOB)
896  {
897  ros_msg.distortion_model = "plumb_bob";
898  }
899  else if (distortion.model() ==
900  ignition::msgs::CameraInfo::Distortion::RATIONAL_POLYNOMIAL)
901  {
902  ros_msg.distortion_model = "rational_polynomial";
903  }
904  else if (distortion.model() ==
905  ignition::msgs::CameraInfo::Distortion::EQUIDISTANT)
906  {
907  ros_msg.distortion_model = "equidistant";
908  }
909  else
910  {
911  ROS_ERROR_STREAM("Unsupported distortion model ["
912  << distortion.model() << "]" << std::endl);
913  }
914 
915  ros_msg.D.resize(distortion.k_size());
916  for (auto i = 0; i < distortion.k_size(); ++i)
917  {
918  ros_msg.D[i] = distortion.k(i);
919  }
920  }
921 
922  auto intrinsics = ign_msg.intrinsics();
923  if (ign_msg.has_intrinsics())
924  {
925  auto intrinsics = ign_msg.intrinsics();
926 
927  for (auto i = 0; i < intrinsics.k_size(); ++i)
928  {
929  ros_msg.K[i] = intrinsics.k(i);
930  }
931  }
932 
933  auto projection = ign_msg.projection();
934  if (ign_msg.has_projection())
935  {
936  auto projection = ign_msg.projection();
937 
938  for (auto i = 0; i < projection.p_size(); ++i)
939  {
940  ros_msg.P[i] = projection.p(i);
941  }
942  }
943 
944  for (auto i = 0; i < ign_msg.rectification_matrix_size(); ++i)
945  {
946  ros_msg.R[i] = ign_msg.rectification_matrix(i);
947  }
948 }
949 
950 template<>
951 void
952 convert_ros_to_ign(
953  const sensor_msgs::Imu & ros_msg,
954  ignition::msgs::IMU & ign_msg)
955 {
956  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
957 
958  // ToDo: Verify that this is the expected value (probably not).
959  ign_msg.set_entity_name(ros_msg.header.frame_id);
960 
961  if (!ignition::math::equal(ros_msg.orientation_covariance[0], -1.0))
962  {
963  // -1 in orientation covariance matrix means there are no orientation
964  // values, see
965  // http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Imu.html
966  convert_ros_to_ign(ros_msg.orientation, (*ign_msg.mutable_orientation()));
967  }
968 
969  convert_ros_to_ign(ros_msg.angular_velocity,
970  (*ign_msg.mutable_angular_velocity()));
971  convert_ros_to_ign(ros_msg.linear_acceleration,
972  (*ign_msg.mutable_linear_acceleration()));
973 }
974 
975 template<>
976 void
977 convert_ign_to_ros(
978  const ignition::msgs::IMU & ign_msg,
979  sensor_msgs::Imu & ros_msg)
980 {
981  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
982 
983  if (ign_msg.has_orientation())
984  {
985  convert_ign_to_ros(ign_msg.orientation(), ros_msg.orientation);
986  }
987  else
988  {
989  // ign may not publish orientation values.
990  // So set 1st element of orientation covariance matrix to -1 to indicate
991  // there are no orientation estimates, see ROS imu msg documentation:
992  // http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Imu.html
993  ros_msg.orientation_covariance[0] = -1.0f;
994  }
995 
996  convert_ign_to_ros(ign_msg.angular_velocity(), ros_msg.angular_velocity);
997  convert_ign_to_ros(ign_msg.linear_acceleration(), ros_msg.linear_acceleration);
998 
999  // Covariances not supported in Ignition::msgs::IMU
1000 }
1001 
1002 template<>
1003 void
1004 convert_ros_to_ign(
1005  const sensor_msgs::JointState & ros_msg,
1006  ignition::msgs::Model & ign_msg)
1007 {
1008  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
1009 
1010  const auto nan = std::numeric_limits<double>::quiet_NaN();
1011  for (auto i = 0u; i < ros_msg.name.size(); ++i)
1012  {
1013  auto newJoint = ign_msg.add_joint();
1014  newJoint->set_name(ros_msg.name[i]);
1015 
1016  if (ros_msg.position.size() > i)
1017  newJoint->mutable_axis1()->set_position(ros_msg.position[i]);
1018  else
1019  newJoint->mutable_axis1()->set_position(nan);
1020 
1021  if (ros_msg.velocity.size() > i)
1022  newJoint->mutable_axis1()->set_velocity(ros_msg.velocity[i]);
1023  else
1024  newJoint->mutable_axis1()->set_velocity(nan);
1025 
1026  if (ros_msg.effort.size() > i)
1027  newJoint->mutable_axis1()->set_force(ros_msg.effort[i]);
1028  else
1029  newJoint->mutable_axis1()->set_force(nan);
1030  }
1031 }
1032 
1033 template<>
1034 void
1035 convert_ign_to_ros(
1036  const ignition::msgs::Model & ign_msg,
1037  sensor_msgs::JointState & ros_msg)
1038 {
1039  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
1040 
1041  for (auto i = 0; i < ign_msg.joint_size(); ++i)
1042  {
1043  ros_msg.name.push_back(ign_msg.joint(i).name());
1044  ros_msg.position.push_back(ign_msg.joint(i).axis1().position());
1045  ros_msg.velocity.push_back(ign_msg.joint(i).axis1().velocity());
1046  ros_msg.effort.push_back(ign_msg.joint(i).axis1().force());
1047  }
1048 }
1049 
1050 template<>
1051 void
1052 convert_ros_to_ign(
1053  const sensor_msgs::LaserScan & ros_msg,
1054  ignition::msgs::LaserScan & ign_msg)
1055 {
1056  const unsigned int num_readings =
1057  (ros_msg.angle_max - ros_msg.angle_min) / ros_msg.angle_increment;
1058 
1059  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
1060  ign_msg.set_frame(ros_msg.header.frame_id);
1061  ign_msg.set_angle_min(ros_msg.angle_min);
1062  ign_msg.set_angle_max(ros_msg.angle_max);
1063  ign_msg.set_angle_step(ros_msg.angle_increment);
1064  ign_msg.set_range_min(ros_msg.range_min);
1065  ign_msg.set_range_max(ros_msg.range_max);
1066  ign_msg.set_count(num_readings);
1067 
1068  // Not supported in sensor_msgs::LaserScan.
1069  ign_msg.set_vertical_angle_min(0.0);
1070  ign_msg.set_vertical_angle_max(0.0);
1071  ign_msg.set_vertical_angle_step(0.0);
1072  ign_msg.set_vertical_count(0u);
1073 
1074  for (auto i = 0u; i < ign_msg.count(); ++i)
1075  {
1076  ign_msg.add_ranges(ros_msg.ranges[i]);
1077  ign_msg.add_intensities(ros_msg.intensities[i]);
1078  }
1079 }
1080 
1081 template<>
1082 void
1083 convert_ign_to_ros(
1084  const ignition::msgs::LaserScan & ign_msg,
1085  sensor_msgs::LaserScan & ros_msg)
1086 {
1087  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
1088  ros_msg.header.frame_id = frame_id_ign_to_ros(ign_msg.frame());
1089 
1090  ros_msg.angle_min = ign_msg.angle_min();
1091  ros_msg.angle_max = ign_msg.angle_max();
1092  ros_msg.angle_increment = ign_msg.angle_step();
1093 
1094  // Not supported in ignition::msgs::LaserScan.
1095  ros_msg.time_increment = 0.0;
1096  ros_msg.scan_time = 0.0;
1097 
1098  ros_msg.range_min = ign_msg.range_min();
1099  ros_msg.range_max = ign_msg.range_max();
1100 
1101  auto count = ign_msg.count();
1102  auto vertical_count = ign_msg.vertical_count();
1103 
1104  // If there are multiple vertical beams, use the one in the middle.
1105  size_t start = (vertical_count / 2) * count;
1106 
1107  // Copy ranges into ROS message.
1108  ros_msg.ranges.resize(count);
1109  std::copy(
1110  ign_msg.ranges().begin() + start,
1111  ign_msg.ranges().begin() + start + count,
1112  ros_msg.ranges.begin());
1113 
1114  // Copy intensities into ROS message.
1115  ros_msg.intensities.resize(count);
1116  std::copy(
1117  ign_msg.intensities().begin() + start,
1118  ign_msg.intensities().begin() + start + count,
1119  ros_msg.intensities.begin());
1120 }
1121 
1122 template<>
1123 void
1124 convert_ros_to_ign(
1125  const sensor_msgs::MagneticField & ros_msg,
1126  ignition::msgs::Magnetometer & ign_msg)
1127 {
1128  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
1129  convert_ros_to_ign(ros_msg.magnetic_field, (*ign_msg.mutable_field_tesla()));
1130 }
1131 
1132 template<>
1133 void
1134 convert_ign_to_ros(
1135  const ignition::msgs::Magnetometer & ign_msg,
1136  sensor_msgs::MagneticField & ros_msg)
1137 {
1138  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
1139  convert_ign_to_ros(ign_msg.field_tesla(), ros_msg.magnetic_field);
1140 
1141  // magnetic_field_covariance is not supported in Ignition::Msgs::Magnetometer.
1142 }
1143 
1144 template<>
1145 void
1146 convert_ros_to_ign(
1147  const sensor_msgs::NavSatFix & ros_msg,
1148  ignition::msgs::NavSat & ign_msg)
1149 {
1150  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
1151  ign_msg.set_latitude_deg(ros_msg.latitude);
1152  ign_msg.set_longitude_deg(ros_msg.longitude);
1153  ign_msg.set_altitude(ros_msg.altitude);
1154  ign_msg.set_frame_id(ros_msg.header.frame_id);
1155 
1156  // Not supported in sensor_msgs::NavSatFix.
1157  ign_msg.set_velocity_east(0.0);
1158  ign_msg.set_velocity_north(0.0);
1159  ign_msg.set_velocity_up(0.0);
1160 }
1161 
1162 template<>
1163 void
1164 convert_ign_to_ros(
1165  const ignition::msgs::NavSat & ign_msg,
1166  sensor_msgs::NavSatFix & ros_msg)
1167 {
1168  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
1169  ros_msg.header.frame_id = frame_id_ign_to_ros(ign_msg.frame_id());
1170  ros_msg.latitude = ign_msg.latitude_deg();
1171  ros_msg.longitude = ign_msg.longitude_deg();
1172  ros_msg.altitude = ign_msg.altitude();
1173 
1174  // position_covariance is not supported in Ignition::Msgs::NavSat.
1175  ros_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
1176  ros_msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
1177 }
1178 
1179 template<>
1180 void
1181 convert_ros_to_ign(
1182  const sensor_msgs::PointCloud2 & ros_msg,
1183  ignition::msgs::PointCloudPacked &ign_msg)
1184 {
1185  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
1186 
1187  ign_msg.set_height(ros_msg.height);
1188  ign_msg.set_width(ros_msg.width);
1189  ign_msg.set_is_bigendian(ros_msg.is_bigendian);
1190  ign_msg.set_point_step(ros_msg.point_step);
1191  ign_msg.set_row_step(ros_msg.row_step);
1192  ign_msg.set_is_dense(ros_msg.is_dense);
1193  ign_msg.mutable_data()->resize(ros_msg.data.size());
1194  memcpy(ign_msg.mutable_data()->data(), ros_msg.data.data(),
1195  ros_msg.data.size());
1196 
1197  for (unsigned int i = 0; i < ros_msg.fields.size(); ++i)
1198  {
1199  ignition::msgs::PointCloudPacked::Field *pf = ign_msg.add_field();
1200  pf->set_name(ros_msg.fields[i].name);
1201  pf->set_count(ros_msg.fields[i].count);
1202  pf->set_offset(ros_msg.fields[i].offset);
1203  switch (ros_msg.fields[i].datatype)
1204  {
1205  default:
1206  case sensor_msgs::PointField::INT8:
1207  pf->set_datatype(ignition::msgs::PointCloudPacked::Field::INT8);
1208  break;
1209  case sensor_msgs::PointField::UINT8:
1210  pf->set_datatype(ignition::msgs::PointCloudPacked::Field::UINT8);
1211  break;
1212  case sensor_msgs::PointField::INT16:
1213  pf->set_datatype(ignition::msgs::PointCloudPacked::Field::INT16);
1214  break;
1215  case sensor_msgs::PointField::UINT16:
1216  pf->set_datatype(ignition::msgs::PointCloudPacked::Field::UINT16);
1217  break;
1218  case sensor_msgs::PointField::INT32:
1219  pf->set_datatype(ignition::msgs::PointCloudPacked::Field::INT32);
1220  break;
1221  case sensor_msgs::PointField::UINT32:
1222  pf->set_datatype(ignition::msgs::PointCloudPacked::Field::UINT32);
1223  break;
1224  case sensor_msgs::PointField::FLOAT32:
1225  pf->set_datatype(ignition::msgs::PointCloudPacked::Field::FLOAT32);
1226  break;
1227  case sensor_msgs::PointField::FLOAT64:
1228  pf->set_datatype(ignition::msgs::PointCloudPacked::Field::FLOAT64);
1229  break;
1230  };
1231  }
1232 }
1233 
1234 template<>
1235 void
1236 convert_ign_to_ros(
1237  const ignition::msgs::PointCloudPacked & ign_msg,
1238  sensor_msgs::PointCloud2 & ros_msg)
1239 {
1240  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
1241 
1242  ros_msg.height = ign_msg.height();
1243  ros_msg.width = ign_msg.width();
1244  ros_msg.is_bigendian = ign_msg.is_bigendian();
1245  ros_msg.point_step = ign_msg.point_step();
1246  ros_msg.row_step = ign_msg.row_step();
1247  ros_msg.is_dense = ign_msg.is_dense();
1248  ros_msg.data.resize(ign_msg.data().size());
1249  memcpy(ros_msg.data.data(), ign_msg.data().c_str(), ign_msg.data().size());
1250 
1251  for (int i = 0; i < ign_msg.field_size(); ++i)
1252  {
1253  sensor_msgs::PointField pf;
1254  pf.name = ign_msg.field(i).name();
1255  pf.count = ign_msg.field(i).count();
1256  pf.offset = ign_msg.field(i).offset();
1257  switch (ign_msg.field(i).datatype())
1258  {
1259  default:
1260  case ignition::msgs::PointCloudPacked::Field::INT8:
1261  pf.datatype = sensor_msgs::PointField::INT8;
1262  break;
1263  case ignition::msgs::PointCloudPacked::Field::UINT8:
1264  pf.datatype = sensor_msgs::PointField::UINT8;
1265  break;
1266  case ignition::msgs::PointCloudPacked::Field::INT16:
1267  pf.datatype = sensor_msgs::PointField::INT16;
1268  break;
1269  case ignition::msgs::PointCloudPacked::Field::UINT16:
1270  pf.datatype = sensor_msgs::PointField::UINT16;
1271  break;
1272  case ignition::msgs::PointCloudPacked::Field::INT32:
1273  pf.datatype = sensor_msgs::PointField::INT32;
1274  break;
1275  case ignition::msgs::PointCloudPacked::Field::UINT32:
1276  pf.datatype = sensor_msgs::PointField::UINT32;
1277  break;
1278  case ignition::msgs::PointCloudPacked::Field::FLOAT32:
1279  pf.datatype = sensor_msgs::PointField::FLOAT32;
1280  break;
1281  case ignition::msgs::PointCloudPacked::Field::FLOAT64:
1282  pf.datatype = sensor_msgs::PointField::FLOAT64;
1283  break;
1284  };
1285  ros_msg.fields.push_back(pf);
1286  }
1287 }
1288 
1289 template<>
1290 void
1291 convert_ros_to_ign(
1292  const sensor_msgs::BatteryState & ros_msg,
1293  ignition::msgs::BatteryState & ign_msg)
1294 {
1295  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
1296 
1297  ign_msg.set_voltage(ros_msg.voltage);
1298  ign_msg.set_current(ros_msg.current);
1299  ign_msg.set_charge(ros_msg.charge);
1300  ign_msg.set_capacity(ros_msg.capacity);
1301  ign_msg.set_percentage(ros_msg.percentage);
1302 
1303  if (ros_msg.power_supply_status == sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN)
1304  {
1305  ign_msg.set_power_supply_status(ignition::msgs::BatteryState::UNKNOWN);
1306  }
1307  else if (ros_msg.power_supply_status == sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_CHARGING)
1308  {
1309  ign_msg.set_power_supply_status(ignition::msgs::BatteryState::CHARGING);
1310  }
1311  else if (ros_msg.power_supply_status ==
1312  sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING)
1313  {
1314  ign_msg.set_power_supply_status(ignition::msgs::BatteryState::DISCHARGING);
1315  }
1316  else if (ros_msg.power_supply_status ==
1317  sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING)
1318  {
1319  ign_msg.set_power_supply_status(ignition::msgs::BatteryState::NOT_CHARGING);
1320  }
1321  else if (ros_msg.power_supply_status == sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_FULL)
1322  {
1323  ign_msg.set_power_supply_status(ignition::msgs::BatteryState::FULL);
1324  }
1325  else
1326  {
1327  ROS_ERROR_STREAM("Unsupported power supply status ["
1328  << ros_msg.power_supply_status << "]" << std::endl);
1329  }
1330 }
1331 
1332 template<>
1333 void
1334 convert_ign_to_ros(
1335  const ignition::msgs::BatteryState & ign_msg,
1336  sensor_msgs::BatteryState & ros_msg)
1337 {
1338  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
1339 
1340  ros_msg.voltage = ign_msg.voltage();
1341  ros_msg.current = ign_msg.current();
1342  ros_msg.charge = ign_msg.charge();
1343  ros_msg.capacity = ign_msg.capacity();
1344  ros_msg.design_capacity = std::numeric_limits<double>::quiet_NaN();
1345  ros_msg.percentage = ign_msg.percentage();
1346 
1347  if (ign_msg.power_supply_status() ==
1348  ignition::msgs::BatteryState::UNKNOWN)
1349  {
1350  ros_msg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
1351  }
1352  else if (ign_msg.power_supply_status() ==
1353  ignition::msgs::BatteryState::CHARGING)
1354  {
1355  ros_msg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_CHARGING;
1356  }
1357  else if (ign_msg.power_supply_status() ==
1358  ignition::msgs::BatteryState::DISCHARGING)
1359  {
1360  ros_msg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
1361  }
1362  else if (ign_msg.power_supply_status() ==
1363  ignition::msgs::BatteryState::NOT_CHARGING)
1364  {
1365  ros_msg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING;
1366  }
1367  else if (ign_msg.power_supply_status() ==
1368  ignition::msgs::BatteryState::FULL)
1369  {
1370  ros_msg.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_FULL;
1371  }
1372  else
1373  {
1374  ROS_ERROR_STREAM("Unsupported power supply status ["
1375  << ign_msg.power_supply_status() << "]" << std::endl);
1376  }
1377 
1378  ros_msg.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
1379  ros_msg.power_supply_technology = sensor_msgs::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN;
1380  ros_msg.present = true;
1381 }
1382 
1383 template<>
1384 void
1385 convert_ros_to_ign(
1386  const visualization_msgs::Marker & ros_msg,
1387  ignition::msgs::Marker & ign_msg)
1388 {
1389  convert_ros_to_ign(ros_msg.header, (*ign_msg.mutable_header()));
1390 
1391  // Note, in ROS's Marker message ADD and MODIFY both map to a value of "0",
1392  // so that case is not needed here.
1393  switch(ros_msg.action)
1394  {
1395  case visualization_msgs::Marker::ADD:
1396  ign_msg.set_action(ignition::msgs::Marker::ADD_MODIFY);
1397  break;
1398  case visualization_msgs::Marker::DELETE:
1399  ign_msg.set_action(ignition::msgs::Marker::DELETE_MARKER);
1400  break;
1401  case visualization_msgs::Marker::DELETEALL:
1402  ign_msg.set_action(ignition::msgs::Marker::DELETE_ALL);
1403  break;
1404  default:
1405  ROS_ERROR_STREAM("Unknown visualization_msgs::Marker action [" <<
1406  ros_msg.action << "]\n");
1407  break;
1408  }
1409 
1410  ign_msg.set_ns(ros_msg.ns);
1411  ign_msg.set_id(ros_msg.id);
1412  // ign_msg.set_layer(); // No "layer" concept in ROS
1413 
1414  // Type
1415  switch(ros_msg.type)
1416  {
1417 #ifndef IGNITION_CITADEL
1418  case visualization_msgs::Marker::ARROW:
1419  ign_msg.set_type(ignition::msgs::Marker::ARROW);
1420  break;
1421 #endif
1422  case visualization_msgs::Marker::CUBE:
1423  ign_msg.set_type(ignition::msgs::Marker::BOX);
1424  break;
1425  case visualization_msgs::Marker::SPHERE:
1426  ign_msg.set_type(ignition::msgs::Marker::SPHERE);
1427  break;
1428  case visualization_msgs::Marker::CYLINDER:
1429  ign_msg.set_type(ignition::msgs::Marker::CYLINDER);
1430  break;
1431  case visualization_msgs::Marker::LINE_STRIP:
1432  ign_msg.set_type(ignition::msgs::Marker::LINE_STRIP);
1433  break;
1434  case visualization_msgs::Marker::LINE_LIST:
1435  ign_msg.set_type(ignition::msgs::Marker::LINE_LIST);
1436  break;
1437  case visualization_msgs::Marker::CUBE_LIST:
1438  ROS_ERROR_STREAM("Unsupported visualization_msgs::Marker type" <<
1439  "[CUBE_LIST]\n");
1440  break;
1441  case visualization_msgs::Marker::SPHERE_LIST:
1442  ROS_ERROR_STREAM("Unsupported visualization_msgs::Marker type" <<
1443  "[SPHERE_LIST]\n");
1444  break;
1445  case visualization_msgs::Marker::POINTS:
1446  ign_msg.set_type(ignition::msgs::Marker::POINTS);
1447  break;
1448  case visualization_msgs::Marker::TEXT_VIEW_FACING:
1449  ign_msg.set_type(ignition::msgs::Marker::TEXT);
1450  break;
1451  case visualization_msgs::Marker::MESH_RESOURCE:
1452  ROS_ERROR_STREAM("Unsupported visualization_msgs::Marker type" <<
1453  "[MESH_RESOURCE]\n");
1454  break;
1455  case visualization_msgs::Marker::TRIANGLE_LIST:
1456  ign_msg.set_type(ignition::msgs::Marker::TRIANGLE_LIST);
1457  break;
1458  default:
1459  ROS_ERROR_STREAM("Unknown visualization_msgs::Marker type [" <<
1460  ros_msg.type << "]\n");
1461  break;
1462  }
1463 
1464  // Lifetime
1465  ign_msg.mutable_lifetime()->set_sec(ros_msg.lifetime.sec);
1466  ign_msg.mutable_lifetime()->set_nsec(ros_msg.lifetime.nsec);
1467 
1468  // Pose
1469  convert_ros_to_ign(ros_msg.pose, *ign_msg.mutable_pose());
1470 
1471  // Scale
1472  convert_ros_to_ign(ros_msg.scale, *ign_msg.mutable_scale());
1473 
1474  // Material
1475  convert_ros_to_ign(ros_msg.color, *ign_msg.mutable_material()->mutable_ambient());
1476  convert_ros_to_ign(ros_msg.color, *ign_msg.mutable_material()->mutable_diffuse());
1477  convert_ros_to_ign(ros_msg.color, *ign_msg.mutable_material()->mutable_specular());
1478 
1479  // Point
1480  ign_msg.clear_point();
1481  for (auto const &pt : ros_msg.points)
1482  {
1483  auto p = ign_msg.add_point();
1484  convert_ros_to_ign(pt, *p);
1485  }
1486 
1487  ign_msg.set_text(ros_msg.text);
1488 
1489  // ign_msg.set_parent(); // No "parent" concept in ROS
1490  // ign_msg.set_visibility(); // No "visibility" concept in ROS
1491 }
1492 
1493 template<>
1494 void
1495 convert_ign_to_ros(
1496  const ignition::msgs::Marker & ign_msg,
1497  visualization_msgs::Marker & ros_msg)
1498 {
1499  convert_ign_to_ros(ign_msg.header(), ros_msg.header);
1500 
1501  switch(ign_msg.action())
1502  {
1503  case ignition::msgs::Marker::ADD_MODIFY:
1504  ros_msg.action = visualization_msgs::Marker::ADD;
1505  break;
1506  case ignition::msgs::Marker::DELETE_MARKER:
1507  ros_msg.action = visualization_msgs::Marker::DELETE;
1508  break;
1509  case ignition::msgs::Marker::DELETE_ALL:
1510  ros_msg.action = visualization_msgs::Marker::DELETEALL;
1511  break;
1512  default:
1513  ROS_ERROR_STREAM("Unknown ignition.msgs.marker action [" <<
1514  ign_msg.action() << "]\n");
1515  break;
1516  }
1517 
1518  ros_msg.ns = ign_msg.ns();
1519  ros_msg.id = ign_msg.id();
1520 
1521  switch(ign_msg.type())
1522  {
1523 #ifndef IGNITION_CITADEL
1524  case ignition::msgs::Marker::ARROW:
1525  ros_msg.type = visualization_msgs::Marker::TRIANGLE_LIST;
1526  break;
1527  case ignition::msgs::Marker::AXIS:
1528  ROS_ERROR_STREAM("Unsupported ignition.msgs.marker type " <<
1529  "[AXIS]\n");
1530  break;
1531  case ignition::msgs::Marker::CONE:
1532  ROS_ERROR_STREAM("Unsupported ignition.msgs.marker type " <<
1533  "[CONE]\n");
1534  break;
1535 #endif
1536  case ignition::msgs::Marker::NONE:
1537  ROS_ERROR_STREAM("Unsupported ignition.msgs.marker type " <<
1538  "[NONE]\n");
1539  break;
1540  case ignition::msgs::Marker::BOX:
1541  ros_msg.type = visualization_msgs::Marker::CUBE;
1542  break;
1543  case ignition::msgs::Marker::CYLINDER:
1544  ros_msg.type = visualization_msgs::Marker::CYLINDER;
1545  break;
1546  case ignition::msgs::Marker::LINE_LIST:
1547  ros_msg.type = visualization_msgs::Marker::LINE_LIST;
1548  break;
1549  case ignition::msgs::Marker::LINE_STRIP:
1550  ros_msg.type = visualization_msgs::Marker::LINE_STRIP;
1551  break;
1552  case ignition::msgs::Marker::POINTS:
1553  ros_msg.type = visualization_msgs::Marker::POINTS;
1554  break;
1555  case ignition::msgs::Marker::SPHERE:
1556  ros_msg.type = visualization_msgs::Marker::SPHERE;
1557  break;
1558  case ignition::msgs::Marker::TEXT:
1559  ros_msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
1560  break;
1561  case ignition::msgs::Marker::TRIANGLE_FAN:
1562  ROS_ERROR_STREAM("Unsupported ignition.msgs.marker type " <<
1563  "[TRIANGLE_FAN]\n");
1564  break;
1565  case ignition::msgs::Marker::TRIANGLE_LIST:
1566  ros_msg.type = visualization_msgs::Marker::TRIANGLE_LIST;
1567  break;
1568  case ignition::msgs::Marker::TRIANGLE_STRIP:
1569  ROS_ERROR_STREAM("Unsupported ignition.msgs.marker type " <<
1570  "[TRIANGLE_STRIP]\n");
1571  break;
1572  default:
1573  ROS_ERROR_STREAM("Unknown ignition.msgs.marker type " <<
1574  "[" << ign_msg.type() << "]\n");
1575  break;
1576  }
1577 
1578  // Lifetime
1579  ros_msg.lifetime.sec = ign_msg.lifetime().sec();
1580  ros_msg.lifetime.nsec = ign_msg.lifetime().nsec();
1581 
1582  // Pose
1583  convert_ign_to_ros(ign_msg.pose(), ros_msg.pose);
1584 
1585  // Scale
1586  convert_ign_to_ros(ign_msg.scale(), ros_msg.scale);
1587 
1588  // Material
1589  convert_ign_to_ros(ign_msg.material().ambient(), ros_msg.color);
1590  ros_msg.colors.clear();
1591 
1592  // Points
1593  ros_msg.points.clear();
1594  ros_msg.points.reserve(ign_msg.point_size());
1595  for (auto const & pt: ign_msg.point())
1596  {
1597  geometry_msgs::Point p;
1598  convert_ign_to_ros(pt, p);
1599  ros_msg.points.push_back(p);
1600  }
1601 
1602  ros_msg.text = ign_msg.text();
1603 }
1604 
1605 template<>
1606 void
1607 convert_ros_to_ign(
1608  const visualization_msgs::MarkerArray & ros_msg,
1609  ignition::msgs::Marker_V & ign_msg)
1610 {
1611  ign_msg.clear_header();
1612  ign_msg.clear_marker();
1613  for (const auto &marker : ros_msg.markers)
1614  {
1615  auto m = ign_msg.add_marker();
1616  convert_ros_to_ign(marker, *m);
1617  }
1618 }
1619 
1620 template<>
1621 void
1622 convert_ign_to_ros(
1623  const ignition::msgs::Marker_V & ign_msg,
1624  visualization_msgs::MarkerArray & ros_msg)
1625 {
1626  ros_msg.markers.clear();
1627  ros_msg.markers.reserve(ign_msg.marker_size());
1628 
1629  for (auto const &marker : ign_msg.marker())
1630  {
1631  visualization_msgs::Marker m;
1632  convert_ign_to_ros(marker, m);
1633  ros_msg.markers.push_back(m);
1634  }
1635 }
1636 
1637 } // namespace ros_ign_bridge
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
ros_ign_bridge::frame_id_ign_to_ros
std::string frame_id_ign_to_ros(const std::string &frame_id)
Definition: convert.cpp:56
console.h
convert.hpp
ros_ign_bridge::convert_ros_to_ign
void convert_ros_to_ign(const std_msgs::Bool &ros_msg, ignition::msgs::Boolean &ign_msg)
Definition: convert.cpp:63
ros_ign_bridge::replace_delimiter
std::string replace_delimiter(const std::string &input, const std::string &old_delim, const std::string new_delim)
Definition: convert.cpp:25
start
ROSCPP_DECL void start()
ros_ign_bridge::convert_ign_to_ros
void convert_ign_to_ros(const ignition::msgs::Boolean &ign_msg, std_msgs::Bool &ros_msg)
Definition: convert.cpp:72
ros::Time
ros_ign_bridge
Definition: convert.hpp:59

ros_ign_bridge
Author(s):
autogenerated on Sat Mar 11 2023 04:02:16