00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include "rawlog_record_node.h"
00035 #include <boost/interprocess/sync/scoped_lock.hpp>
00036
00037 #include <mrpt_bridge/pose.h>
00038 #include <mrpt_bridge/laser_scan.h>
00039 #include <mrpt_bridge/marker_msgs.h>
00040 #include <mrpt_bridge/time.h>
00041
00042 #include <mrpt/version.h>
00043 using namespace mrpt::maps;
00044 using namespace mrpt::obs;
00045
00046 int main(int argc, char** argv)
00047 {
00048 ros::init(argc, argv, "rawlog_record");
00049 ros::NodeHandle n;
00050 RawlogRecordNode my_node(n);
00051 my_node.init();
00052 my_node.loop();
00053 return 0;
00054 }
00055
00056 RawlogRecordNode::~RawlogRecordNode() {}
00057 RawlogRecordNode::RawlogRecordNode(ros::NodeHandle& n) : n_(n) {}
00058
00059 void RawlogRecordNode::init()
00060 {
00061 updateRawLogName(mrpt::system::now());
00062 ROS_INFO("rawlog file: %s", base_param_.raw_log_name.c_str());
00063 if (base_param_.record_range_scan)
00064 {
00065 subLaser_ =
00066 n_.subscribe("scan", 1, &RawlogRecordNode::callbackLaser, this);
00067 }
00068 if (base_param_.record_bearing_range)
00069 {
00070 subMarker_ =
00071 n_.subscribe("marker", 1, &RawlogRecordNode::callbackMarker, this);
00072 }
00073 subOdometry_ =
00074 n_.subscribe("odom", 1, &RawlogRecordNode::callbackOdometry, this);
00075 }
00076
00077 void RawlogRecordNode::loop() { ros::spin(); }
00078
00079 bool RawlogRecordNode::waitForTransform(
00080 mrpt::poses::CPose3D& des, const std::string& target_frame,
00081 const std::string& source_frame, const ros::Time& time,
00082 const ros::Duration& timeout, const ros::Duration& polling_sleep_duration)
00083 {
00084 tf::StampedTransform transform;
00085 try
00086 {
00087 if (base_param_.debug)
00088 ROS_INFO(
00089 "debug: waitForTransform(): target_frame='%s' "
00090 "source_frame='%s'",
00091 target_frame.c_str(), source_frame.c_str());
00092
00093 listenerTF_.waitForTransform(
00094 target_frame, source_frame, time, polling_sleep_duration);
00095 listenerTF_.lookupTransform(
00096 target_frame, source_frame, time, transform);
00097 }
00098 catch (tf::TransformException)
00099 {
00100 ROS_INFO(
00101 "Failed to get transform target_frame (%s) to source_frame (%s)",
00102 target_frame.c_str(), source_frame.c_str());
00103 return false;
00104 }
00105 mrpt_bridge::convert(transform, des);
00106 return true;
00107 }
00108
00109 void RawlogRecordNode::convert(
00110 const nav_msgs::Odometry& src, mrpt::obs::CObservationOdometry& des)
00111 {
00112 mrpt_bridge::convert(src.header.stamp, des.timestamp);
00113 mrpt_bridge::convert(src.pose.pose, des.odometry);
00114 std::string odom_frame_id =
00115 tf::resolve(param_.tf_prefix, param_.odom_frame_id);
00116 des.sensorLabel = odom_frame_id;
00117 des.hasEncodersInfo = false;
00118 des.hasVelocities = false;
00119 }
00120
00121 void RawlogRecordNode::callbackOdometry(const nav_msgs::Odometry& _msg)
00122 {
00123
00124 if (!last_odometry_)
00125 {
00126 last_odometry_ = CObservationOdometry::Create();
00127 }
00128 convert(_msg, *last_odometry_);
00129 addObservation(_msg.header.stamp);
00130 }
00131
00132 void RawlogRecordNode::callbackLaser(const sensor_msgs::LaserScan& _msg)
00133 {
00134
00135 if (!last_range_scan_)
00136 {
00137 last_range_scan_ = CObservation2DRangeScan::Create();
00138 }
00139 mrpt::poses::CPose3D sensor_pose_on_robot;
00140
00141 if (getStaticTF(_msg.header.frame_id, sensor_pose_on_robot))
00142 {
00143 mrpt_bridge::convert(_msg, sensor_pose_on_robot, *last_range_scan_);
00144
00145 addObservation(_msg.header.stamp);
00146 }
00147 }
00148
00149 void RawlogRecordNode::callbackMarker(const marker_msgs::MarkerDetection& _msg)
00150 {
00151
00152 if (!last_bearing_range_)
00153 {
00154 last_bearing_range_ = CObservationBearingRange::Create();
00155 }
00156 if (!last_beacon_range_)
00157 {
00158 last_beacon_range_ = CObservationBeaconRanges::Create();
00159 }
00160 mrpt::poses::CPose3D sensor_pose_on_robot;
00161
00162 if (getStaticTF(_msg.header.frame_id, sensor_pose_on_robot))
00163 {
00164 mrpt_bridge::convert(_msg, sensor_pose_on_robot, *last_bearing_range_);
00165 last_bearing_range_->sensor_std_range =
00166 base_param_.bearing_range_std_range;
00167 last_bearing_range_->sensor_std_yaw = base_param_.bearing_range_std_yaw;
00168 last_bearing_range_->sensor_std_pitch =
00169 base_param_.bearing_range_std_pitch;
00170
00171 mrpt_bridge::convert(_msg, sensor_pose_on_robot, *last_beacon_range_);
00172 last_beacon_range_->stdError = base_param_.bearing_range_std_range;
00173 addObservation(_msg.header.stamp);
00174 }
00175 }
00176
00177 void RawlogRecordNode::addObservation(const ros::Time& time)
00178 {
00179 sync_attempts_sensor_frame_++;
00180 if (sync_attempts_sensor_frame_ > 10)
00181 ROS_INFO("Problem to syn data for sensor frame!");
00182
00183 if (!last_odometry_) return;
00184 auto odometry = CObservationOdometry::Create();
00185 *odometry = *last_odometry_;
00186 pRawLog.addObservationMemoryReference(odometry);
00187
00188 CObservation2DRangeScan::Ptr range_scan;
00189 CObservationBearingRange::Ptr bearing_range;
00190 CObservationBeaconRanges::Ptr beacon_range;
00191
00192 if (base_param_.record_range_scan)
00193 {
00194 if (!last_range_scan_) return;
00195 if (fabs(mrpt::system::timeDifference(
00196 last_odometry_->timestamp, last_range_scan_->timestamp)) >
00197 param_.sensor_frame_sync_threshold)
00198 {
00199 return;
00200 }
00201 range_scan = CObservation2DRangeScan::Create();
00202 *range_scan = *last_range_scan_;
00203 pRawLog.addObservationMemoryReference(range_scan);
00204 }
00205
00206 if (base_param_.record_bearing_range)
00207 {
00208 if (!last_bearing_range_) return;
00209 if (fabs(mrpt::system::timeDifference(
00210 last_odometry_->timestamp, last_bearing_range_->timestamp)) >
00211 param_.sensor_frame_sync_threshold)
00212 {
00213 return;
00214 }
00215 bearing_range = CObservationBearingRange::Create();
00216 *bearing_range = *last_bearing_range_;
00217 pRawLog.addObservationMemoryReference(bearing_range);
00218 }
00219 if (base_param_.record_beacon_range)
00220 {
00221 if (!last_beacon_range_) return;
00222 if (fabs(mrpt::system::timeDifference(
00223 last_odometry_->timestamp, last_beacon_range_->timestamp)) >
00224 param_.sensor_frame_sync_threshold)
00225 {
00226 return;
00227 }
00228 beacon_range = CObservationBeaconRanges::Create();
00229 *beacon_range = *last_beacon_range_;
00230 pRawLog.addObservationMemoryReference(beacon_range);
00231 }
00232
00233 static std::shared_ptr<mrpt::poses::CPose2D> lastOdomPose;
00234 if (!lastOdomPose)
00235 {
00236 lastOdomPose = std::make_shared<mrpt::poses::CPose2D>();
00237 *lastOdomPose = odometry->odometry;
00238 }
00239
00240 mrpt::poses::CPose2D incOdoPose = odometry->odometry - *lastOdomPose;
00241
00242 CActionRobotMovement2D odom_move;
00243 odom_move.timestamp = odometry->timestamp;
00244 odom_move.computeFromOdometry(incOdoPose, base_param_.motionModelOptions);
00245 auto action = CActionCollection::Create();
00246 action->insert(odom_move);
00247 pRawLogASF.addActionsMemoryReference(action);
00248
00249 auto sf = CSensoryFrame::Create();
00250 if (base_param_.record_range_scan)
00251 {
00252 CObservation::Ptr obs_range_scan = CObservation::Ptr(range_scan);
00253 sf->insert(obs_range_scan);
00254 }
00255
00256 if (base_param_.record_bearing_range)
00257 {
00258 CObservation::Ptr obs_bearing_range = CObservation::Ptr(bearing_range);
00259 sf->insert(obs_bearing_range);
00260 }
00261 if (base_param_.record_beacon_range)
00262 {
00263 CObservation::Ptr obs_bearing_range = CObservation::Ptr(beacon_range);
00264 sf->insert(obs_bearing_range);
00265 }
00266 pRawLogASF.addObservationsMemoryReference(sf);
00267
00268 *lastOdomPose = odometry->odometry;
00269
00270 sync_attempts_sensor_frame_ = 0;
00271 }
00272
00273 bool RawlogRecordNode::getStaticTF(
00274 std::string source_frame, mrpt::poses::CPose3D& des)
00275 {
00276 std::string target_frame_id =
00277 tf::resolve(param_.tf_prefix, param_.base_frame_id);
00278 std::string source_frame_id = source_frame;
00279 std::string key = target_frame_id + "->" + source_frame_id;
00280 mrpt::poses::CPose3D pose;
00281 tf::StampedTransform transform;
00282
00283 if (static_tf_.find(key) == static_tf_.end())
00284 {
00285 try
00286 {
00287 if (base_param_.debug)
00288 ROS_INFO(
00289 "debug: updateLaserPose(): target_frame_id='%s' "
00290 "source_frame_id='%s'",
00291 target_frame_id.c_str(), source_frame_id.c_str());
00292
00293 listenerTF_.lookupTransform(
00294 target_frame_id, source_frame_id, ros::Time(0), transform);
00295 tf::Vector3 translation = transform.getOrigin();
00296 tf::Quaternion quat = transform.getRotation();
00297 pose.x() = translation.x();
00298 pose.y() = translation.y();
00299 pose.z() = translation.z();
00300 tf::Matrix3x3 Rsrc(quat);
00301 mrpt::math::CMatrixDouble33 Rdes;
00302 for (int c = 0; c < 3; c++)
00303 for (int r = 0; r < 3; r++) Rdes(r, c) = Rsrc.getRow(r)[c];
00304 pose.setRotationMatrix(Rdes);
00305 static_tf_[key] = pose;
00306 ROS_INFO(
00307 "Static tf '%s' with '%s'", key.c_str(),
00308 pose.asString().c_str());
00309 }
00310 catch (tf::TransformException ex)
00311 {
00312 ROS_INFO("getStaticTF");
00313 ROS_ERROR("%s", ex.what());
00314 ros::Duration(1.0).sleep();
00315 return false;
00316 }
00317 }
00318 des = static_tf_[key];
00319 return true;
00320 }