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-2017, 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_bridge/GPS.h"
16 #include "mrpt_bridge/range.h"
17 #include "mrpt_bridge/imu.h"
18 #include "mrpt_bridge/image.h"
19 #include "mrpt_bridge/stereo_image.h"
20 
21 #include "../GPS.cpp"
22 #include "../range.cpp"
23 #include "../imu.cpp"
24 #include "../image.cpp"
25 #include "../stereo_image.cpp"
26 
28 #include <mrpt/obs/CRawlog.h>
29 #include <mrpt/system/filesystem.h>
30 
32 #include <dirent.h>
33 
34 
35 using namespace sensor_msgs;
36 using namespace mrpt::obs;
37 using namespace mrpt::system;
38 using namespace mrpt;
39 using namespace std;
40 using namespace cv;
41 
49 int main(int argc, char **argv)
50 {
51  ros::init(argc, argv, "testing_MRPT_bridge");
52  ros::NodeHandle n;
53 
54  ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu_publisher", 1000);
55  ros::Publisher navSatFix_pub = n.advertise<sensor_msgs::NavSatFix>("navSatFix_publisher", 1000);
56  ros::Publisher image_pub = n.advertise<sensor_msgs::Image>("image_publisher", 1000);
57  ros::Publisher image_pub_left = n.advertise<sensor_msgs::Image>("left_image_publisher", 1000);
58  ros::Publisher image_pub_right = 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 
101 
103  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_X) = 1;
104  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_Y) = 1;
105  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_Z) = 1;
106  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_W) = 1;
107 
108  mrpt_IMU.rawMeasurements.at(IMU_X_ACC_GLOBAL) = 1;
109  mrpt_IMU.rawMeasurements.at(IMU_Y_ACC_GLOBAL) = 1;
110  mrpt_IMU.rawMeasurements.at(IMU_Z_ACC_GLOBAL) = 1;
111 
112  mrpt_IMU.rawMeasurements.at(IMU_X_VEL) = 1;
113  mrpt_IMU.rawMeasurements.at(IMU_Y_VEL) = 1;
114  mrpt_IMU.rawMeasurements.at(IMU_Z_VEL) = 1;
115 
117  CObservationImage image;
118  DIR *dir;
119  dirent *pdir;
120  vector<string> files;
121  vector<string> files_fullpath;
122 
124  string file_path1 = "/home/raghavender/catkin_ws/src/mrpt_navigation/mrpt_bridge/src/images";
125 
126  dir = opendir(file_path1.c_str());
127  while((pdir = readdir(dir)))
128  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() > 4) // this removes the . and .. in linux as all files will have size more than 4 .png .jpg etc.
132  {
133  files_fullpath.push_back(file_path1 + "/" + files.at(i));
134  //cout << files_fullpath.at(j) << endl;
135  j++;
136  }
137  } // end of for
138  sort(files_fullpath.begin(),files_fullpath.end());
139 
140 
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 = "/home/raghavender/catkin_ws/src/mrpt_navigation/mrpt_bridge/src/stereo_images/left";
152  string file_path_right = "/home/raghavender/catkin_ws/src/mrpt_navigation/mrpt_bridge/src/stereo_images/right";
153 
154  dir1 = opendir(file_path_left.c_str());
155  while((pdir1 = readdir(dir1)))
156  files1.push_back(pdir1->d_name);
157  for(unsigned int i=0,j=0 ; i<files1.size() ; i++)
158  {
159  if(files1.at(i).size() > 4) // this removes the . and .. in linux as all files will have size more than 4 .png .jpg etc.
160  {
161  files_fullpath1.push_back(file_path_left + "/" + files1.at(i));
162  j++;
163  }
164  } // end of for
165  sort(files_fullpath1.begin(),files_fullpath1.end());
166 
168  dir2 = opendir(file_path_right.c_str());
169  while((pdir2 = readdir(dir2)))
170  files2.push_back(pdir2->d_name);
171  for(unsigned int i=0,j=0 ; i<files2.size() ; i++)
172  {
173  if(files2.at(i).size() > 4) // this removes the . and .. in linux as all files will have size more than 4 .png .jpg etc.
174  {
175  files_fullpath2.push_back(file_path_right + "/" + files2.at(i));
176  j++;
177  }
178  } // end of for
179  sort(files_fullpath2.begin(),files_fullpath2.end());
180 
181  ROS_INFO("Range Message Conversion STARTED..!!");
182 
183 
184  ros::Rate loop_rate(10);
185  while(ros::ok())
186  {
188  header.seq = i;
189  header.stamp = ros::Time::now();
190  i++;
191 
193  mrpt_Image.image.loadFromFile(files_fullpath.at(i%(files_fullpath.size()-1)));
194  mrpt_bridge::image::mrpt2ros(mrpt_Image, header, ros_Image);
195  image_pub.publish(ros_Image);
196 
198  mrpt_Image_left.image.loadFromFile(files_fullpath1.at(i%(files_fullpath1.size()-1)));
199  mrpt_bridge::image::mrpt2ros(mrpt_Image_left, header, ros_Image_left);
200  image_pub_left.publish(ros_Image_left);
201 
202  mrpt_Image_right.image.loadFromFile(files_fullpath2.at(i%(files_fullpath2.size()-1)));
203  mrpt_bridge::image::mrpt2ros(mrpt_Image_left, header, ros_Image_right);
204  image_pub_right.publish(ros_Image_right);
205 
206 
208  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_X) = i*0.1;
209  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_Y) = i*0.1;
210  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_Z) = i*0.1;
211  mrpt_IMU.rawMeasurements.at(IMU_ORI_QUAT_W) = i*-0.1;
212  mrpt_IMU.rawMeasurements.at(IMU_X_ACC_GLOBAL) = i * 0.1;
213  mrpt_IMU.rawMeasurements.at(IMU_Y_ACC_GLOBAL) = i*0.1;
214  mrpt_IMU.rawMeasurements.at(IMU_Z_ACC_GLOBAL) = 9.8+ i*0.003;
215  mrpt_IMU.rawMeasurements.at(IMU_X_VEL) = i*0.1 * i*0.2;
216  mrpt_IMU.rawMeasurements.at(IMU_Y_VEL) = i*0.1 * i*0.2;
217  mrpt_IMU.rawMeasurements.at(IMU_Z_VEL) = 0;
218  mrpt_bridge::imu::mrpt2ros(mrpt_IMU, header, ros_Imu);
219  imu_pub.publish(ros_Imu);
220 
221 
223  mrpt::obs::gnss::Message_NMEA_GGA gga;
224  gga.fields.altitude_meters = 2;
225  gga.fields.latitude_degrees = i*0.001;
226  gga.fields.longitude_degrees = i*0.03 + 43;
227  gga.fields.fix_quality = 1;
228  mrpt_GPS.setMsg(gga);
229  mrpt_bridge::GPS::mrpt2ros(mrpt_GPS, header, ros_GPS);
230  navSatFix_pub.publish(ros_GPS);
231 
232 
234  mrpt_bridge::range::mrpt2ros(mrpt_Range, header, ros_Range);
235  ROS_INFO(" published %d",i);
236  // publishing the ranges over num_ranges umber of topics
237  for(int i=0 ; i<num_ranges ; i++)
238  range_pub[i].publish(ros_Range[i]);
239 
240  ros::spinOnce();
241  loop_rate.sleep();
242  }
243  return 0;
244 }
245 
246 
stereo_image.h
mrpt_bridge::GPS::mrpt2ros
bool mrpt2ros(const CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Definition: GPS.cpp:54
ros::Publisher
mrpt_bridge::range::mrpt2ros
bool mrpt2ros(const CObservationRange &obj, const std_msgs::Header &msg_header, sensor_msgs::Range *msg)
Definition: range.cpp:41
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros::spinOnce
ROSCPP_DECL void spinOnce()
mrpt
Definition: include/mrpt_bridge/beacon.h:35
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::ok
ROSCPP_DECL bool ok()
mrpt::obs
Definition: include/mrpt_bridge/beacon.h:46
imu.h
mrpt_bridge::image::mrpt2ros
bool mrpt2ros(const CObservationImage &obj, const std_msgs::Header &msg_header, sensor_msgs::Image &msg)
Definition: image.cpp:54
image.h
range.h
GPS.h
std_msgs::Header_
Definition: map.h:24
ros::Rate::sleep
bool sleep()
main
int main(int argc, char **argv)
Definition: test_Bridge.cpp:49
std
ros::Rate
header
const std::string header
ROS_INFO
#define ROS_INFO(...)
sensor_msgs
ros::NodeHandle
mrpt_bridge::imu::mrpt2ros
bool mrpt2ros(const CObservationIMU &obj, const std_msgs::Header &msg_header, sensor_msgs::Imu &msg)
Definition: imu.cpp:47
ros::Time::now
static Time now()

mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Sun Mar 6 2022 03:48:10