test_Bridge.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /*---------------------------------------------------------------
11  APPLICATION: mrpt_ros bridge
12  FILE: test_Bridge.cpp
13  AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
14  ---------------------------------------------------------------*/
15 #include <mrpt/ros1bridge/gps.h>
16 #include <mrpt/ros1bridge/range.h>
17 #include <mrpt/ros1bridge/imu.h>
18 #include <mrpt/ros1bridge/image.h>
19 #include <mrpt/ros1bridge/stereo_image.h>
20 #include <mrpt/obs/CRawlog.h>
21 #include <mrpt/system/filesystem.h>
22 #include <ros/node_handle.h>
23 
24 #include <mrpt_bridge/image.h>
25 
27 #include <dirent.h>
28 
29 using namespace sensor_msgs;
30 using namespace mrpt::obs;
31 using namespace mrpt::system;
32 using namespace mrpt;
33 using namespace std;
34 using namespace cv;
35 
44 int main(int argc, char** argv)
45 {
46  ros::init(argc, argv, "testing_MRPT_bridge");
47  ros::NodeHandle n;
48 
49  ros::Publisher imu_pub =
50  n.advertise<sensor_msgs::Imu>("imu_publisher", 1000);
51  ros::Publisher navSatFix_pub =
52  n.advertise<sensor_msgs::NavSatFix>("navSatFix_publisher", 1000);
53  ros::Publisher image_pub =
54  n.advertise<sensor_msgs::Image>("image_publisher", 1000);
55  ros::Publisher image_pub_left =
56  n.advertise<sensor_msgs::Image>("left_image_publisher", 1000);
57  ros::Publisher image_pub_right =
58  n.advertise<sensor_msgs::Image>("right_image_publisher", 1000);
59 
60  int num_ranges = 10;
61  ros::Publisher range_pub[num_ranges];
62  for (int i = 0; i < num_ranges; i++)
63  {
64  stringstream ss;
65  ss << "range_publisher" << i;
66  string topic_name = ss.str();
67  range_pub[i] = n.advertise<sensor_msgs::Range>(topic_name, 1000);
68  }
69  sensor_msgs::Imu ros_Imu;
70  sensor_msgs::Range* ros_Range;
71  sensor_msgs::NavSatFix ros_GPS;
72  sensor_msgs::Image ros_Image;
73  sensor_msgs::Image ros_Image_left;
74  sensor_msgs::Image ros_Image_right;
75 
76  CObservationGPS mrpt_GPS;
77  CObservationIMU mrpt_IMU;
78  CObservationRange mrpt_Range;
79  CObservationImage mrpt_Image;
80  CObservationImage mrpt_Image_left;
81  CObservationImage mrpt_Image_right;
82 
83  int i = 0;
84  std_msgs::Header header;
85  header.frame_id = "/map";
86 
88  mrpt_Range.maxSensorDistance = 500;
89  mrpt_Range.minSensorDistance = 60;
90  mrpt_Range.sensorConeApperture = (float)M_PI / 3;
91  // initializing the measurement of each range value message in MRPT
92  for (int i = 0; i < num_ranges; i++)
93  {
94  CObservationRange::TMeasurement measurement;
95  measurement.sensedDistance = i * 1.0;
96  mrpt_Range.sensedData.push_back(measurement);
97  }
99  ros_Range = new sensor_msgs::Range[num_ranges];
100 
102  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_X) = 1;
103  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_Y) = 1;
104  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_Z) = 1;
105  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_W) = 1;
106 
107  mrpt_IMU.rawMeasurements.at(IMU_X_ACC_GLOBAL) = 1;
108  mrpt_IMU.rawMeasurements.at(IMU_Y_ACC_GLOBAL) = 1;
109  mrpt_IMU.rawMeasurements.at(IMU_Z_ACC_GLOBAL) = 1;
110 
111  mrpt_IMU.rawMeasurements.at(IMU_X_VEL) = 1;
112  mrpt_IMU.rawMeasurements.at(IMU_Y_VEL) = 1;
113  mrpt_IMU.rawMeasurements.at(IMU_Z_VEL) = 1;
114 
116  CObservationImage image;
117  DIR* dir;
118  dirent* pdir;
119  vector<string> files;
120  vector<string> files_fullpath;
121 
123  string file_path1 =
124  "/home/raghavender/catkin_ws/src/mrpt_navigation/mrpt_bridge/src/"
125  "images";
126 
127  dir = opendir(file_path1.c_str());
128  while ((pdir = readdir(dir))) files.push_back(pdir->d_name);
129  for (unsigned int i = 0, j = 0; i < files.size(); i++)
130  {
131  if (files.at(i).size() >
132  4) // this removes the . and .. in linux as all files will have
133  // size more than 4 .png .jpg etc.
134  {
135  files_fullpath.push_back(file_path1 + "/" + files.at(i));
136  // cout << files_fullpath.at(j) << endl;
137  j++;
138  }
139  } // end of for
140  sort(files_fullpath.begin(), files_fullpath.end());
141 
144  CObservationImage image1, image2;
145  DIR *dir1, *dir2;
146  dirent *pdir1, *pdir2;
147  vector<string> files1, files2;
148  vector<string> files_fullpath1, files_fullpath2;
149 
151  string file_path_left =
152  "/home/raghavender/catkin_ws/src/mrpt_navigation/mrpt_bridge/src/"
153  "stereo_images/left";
154  string file_path_right =
155  "/home/raghavender/catkin_ws/src/mrpt_navigation/mrpt_bridge/src/"
156  "stereo_images/right";
157 
158  dir1 = opendir(file_path_left.c_str());
159  while ((pdir1 = readdir(dir1))) files1.push_back(pdir1->d_name);
160  for (unsigned int i = 0, j = 0; i < files1.size(); i++)
161  {
162  if (files1.at(i).size() >
163  4) // this removes the . and .. in linux as all files will have
164  // size more than 4 .png .jpg etc.
165  {
166  files_fullpath1.push_back(file_path_left + "/" + files1.at(i));
167  j++;
168  }
169  } // end of for
170  sort(files_fullpath1.begin(), files_fullpath1.end());
171 
173  dir2 = opendir(file_path_right.c_str());
174  while ((pdir2 = readdir(dir2))) files2.push_back(pdir2->d_name);
175  for (unsigned int i = 0, j = 0; i < files2.size(); i++)
176  {
177  if (files2.at(i).size() >
178  4) // this removes the . and .. in linux as all files will have
179  // size more than 4 .png .jpg etc.
180  {
181  files_fullpath2.push_back(file_path_right + "/" + files2.at(i));
182  j++;
183  }
184  } // end of for
185  sort(files_fullpath2.begin(), files_fullpath2.end());
186 
187  ROS_INFO("Range Message Conversion STARTED..!!");
188 
189  ros::Rate loop_rate(10);
190  while (ros::ok())
191  {
193  header.seq = i;
194  header.stamp = ros::Time::now();
195  i++;
196 
198  mrpt_Image.image.loadFromFile(
199  files_fullpath.at(i % (files_fullpath.size() - 1)));
200 
201  mrpt_bridge::image::mrpt2ros(mrpt_Image, header, ros_Image);
202  image_pub.publish(ros_Image);
203 
205  mrpt_Image_left.image.loadFromFile(
206  files_fullpath1.at(i % (files_fullpath1.size() - 1)));
207  mrpt_bridge::image::mrpt2ros(mrpt_Image_left, header, ros_Image_left);
208  image_pub_left.publish(ros_Image_left);
209 
210  mrpt_Image_right.image.loadFromFile(
211  files_fullpath2.at(i % (files_fullpath2.size() - 1)));
212  mrpt_bridge::image::mrpt2ros(mrpt_Image_left, header, ros_Image_right);
213  image_pub_right.publish(ros_Image_right);
214 
216  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_X) = i * 0.1;
217  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_Y) = i * 0.1;
218  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_Z) = i * 0.1;
219  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_W) = i * -0.1;
220  mrpt_IMU.rawMeasurements.at(IMU_X_ACC_GLOBAL) = i * 0.1;
221  mrpt_IMU.rawMeasurements.at(IMU_Y_ACC_GLOBAL) = i * 0.1;
222  mrpt_IMU.rawMeasurements.at(IMU_Z_ACC_GLOBAL) = 9.8 + i * 0.003;
223  mrpt_IMU.rawMeasurements.at(IMU_X_VEL) = i * 0.1 * i * 0.2;
224  mrpt_IMU.rawMeasurements.at(IMU_Y_VEL) = i * 0.1 * i * 0.2;
225  mrpt_IMU.rawMeasurements.at(IMU_Z_VEL) = 0;
226  mrpt::ros1bridge::toROS(mrpt_IMU, header, ros_Imu);
227  imu_pub.publish(ros_Imu);
228 
230  mrpt::obs::gnss::Message_NMEA_GGA gga;
231  gga.fields.altitude_meters = 2;
232  gga.fields.latitude_degrees = i * 0.001;
233  gga.fields.longitude_degrees = i * 0.03 + 43;
234  gga.fields.fix_quality = 1;
235  mrpt_GPS.setMsg(gga);
236  mrpt::ros1bridge::toROS(mrpt_GPS, header, ros_GPS);
237  navSatFix_pub.publish(ros_GPS);
238 
240  mrpt::ros1bridge::toROS(mrpt_Range, header, ros_Range);
241  ROS_INFO(" published %d", i);
242  // publishing the ranges over num_ranges umber of topics
243  for (int i = 0; i < num_ranges; i++) range_pub[i].publish(ros_Range[i]);
244 
245  ros::spinOnce();
246  loop_rate.sleep();
247  }
248  return 0;
249 }
ros::NodeHandle
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
M_PI
#define M_PI
mrpt_bridge::image::mrpt2ros
bool mrpt2ros(const mrpt::obs::CObservationImage &obj, const std_msgs::Header &msg_header, sensor_msgs::Image &msg)
Definition: image.cpp:39
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
sensor_msgs
node_handle.h
ROS_INFO
#define ROS_INFO(...)
ros::ok
ROSCPP_DECL bool ok()
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Rate::sleep
bool sleep()
ros::Time::now
static Time now()
header
const std::string header
ros::spinOnce
ROSCPP_DECL void spinOnce()
main
int main(int argc, char **argv)
Definition: test_Bridge.cpp:44

mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Thu May 12 2022 02:26:47