From d8dc993f9ef1f0a00d0959dde28c78052e8b7199 Mon Sep 17 00:00:00 2001 From: jgmonroy Date: Mon, 27 Feb 2017 11:47:53 +0100 Subject: [PATCH] - New param to set the topic name where to publish the odometry - New param to set if tf transformations are published or not (usefull when comparing different odometries) - new option to set the initial pose of the robot (from another topic). This is useful for simulation when the robot spans at pose (X,Y). - New launch file with the added params --- .../rf2o_laser_odometry/CLaserOdometry2D.h | 25 ++-- launch/rf2o_laser_odometry.launch | 14 ++- src/CLaserOdometry2D.cpp | 114 +++++++++++++----- 3 files changed, 110 insertions(+), 43 deletions(-) diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h index 273584e..1d87b7c 100644 --- a/include/rf2o_laser_odometry/CLaserOdometry2D.h +++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h @@ -61,24 +61,29 @@ public: void Init(); void odometryCalculation(); - std::string laser_scan_topic; - std::string base_frame_id; - std::string odom_frame_id; - double freq; + std::string laser_scan_topic; + std::string odom_topic; + bool publish_tf; + std::string base_frame_id; + std::string odom_frame_id; + std::string init_pose_from_topic; + double freq; protected: - ros::NodeHandle n; - sensor_msgs::LaserScan last_scan; - bool module_initialized,first_laser_scan,new_scan_available; - tf::TransformListener tf_listener; //Do not put inside the callback - tf::TransformBroadcaster odom_broadcaster; + ros::NodeHandle n; + sensor_msgs::LaserScan last_scan; + bool module_initialized,first_laser_scan,new_scan_available, GT_pose_initialized; + tf::TransformListener tf_listener; //Do not put inside the callback + tf::TransformBroadcaster odom_broadcaster; + nav_msgs::Odometry initial_robot_pose; //Subscriptions & Publishers - ros::Subscriber laser_sub; + ros::Subscriber laser_sub, initPose_sub; ros::Publisher odom_pub; //CallBacks void LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan); + void initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose); // Internal Data std::vector range; diff --git a/launch/rf2o_laser_odometry.launch b/launch/rf2o_laser_odometry.launch index 06779c6..805f56e 100644 --- a/launch/rf2o_laser_odometry.launch +++ b/launch/rf2o_laser_odometry.launch @@ -10,10 +10,14 @@ - - - - - + + # topic where the lidar scans are being published + # topic where tu publish the odometry estimations + # wheter or not to publish the tf::transform (base->odom) + # frame_id (tf) of the mobile robot base. A tf transform from the laser_frame to the base_frame is mandatory + # frame_id (tf) to publish the odometry estimations + # (Odom topic) Leave empty to start at point (0,0) + # Execution frequency. See "Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16" + diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index c06446f..994f85e 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -34,15 +34,36 @@ CLaserOdometry2D::CLaserOdometry2D() //---------------- ros::NodeHandle pn("~"); pn.param("laser_scan_topic",laser_scan_topic,"/laser_scan"); + pn.param("odom_topic", odom_topic, "/odom_rf2o"); pn.param("base_frame_id", base_frame_id, "/base_link"); pn.param("odom_frame_id", odom_frame_id, "/odom"); + pn.param("publish_tf", publish_tf, true); + pn.param("init_pose_from_topic", init_pose_from_topic, "/base_pose_ground_truth"); pn.param("freq",freq,10.0); //Publishers and Subscribers //-------------------------- - odom_pub = pn.advertise(odom_frame_id, 5); + odom_pub = pn.advertise(odom_topic, 5); laser_sub = n.subscribe(laser_scan_topic,1,&CLaserOdometry2D::LaserCallBack,this); + //init pose?? + if (init_pose_from_topic != "") + { + initPose_sub = n.subscribe(init_pose_from_topic,1,&CLaserOdometry2D::initPoseCallBack,this); + GT_pose_initialized = false; + } + else + { + GT_pose_initialized = true; + initial_robot_pose.pose.pose.position.x = 0; + initial_robot_pose.pose.pose.position.y = 0; + initial_robot_pose.pose.pose.position.z = 0; + initial_robot_pose.pose.pose.orientation.w = 0; + initial_robot_pose.pose.pose.orientation.x = 0; + initial_robot_pose.pose.pose.orientation.y = 0; + initial_robot_pose.pose.pose.orientation.z = 0; + } + //Init variables module_initialized = false; first_laser_scan = true; @@ -101,12 +122,32 @@ void CLaserOdometry2D::Init() LaserPoseOnTheRobot.setRotationMatrix(R); - //Set the initial pose - laser_pose = LaserPoseOnTheRobot; - laser_oldpose = LaserPoseOnTheRobot; + //Robot initial pose (see MQTT:bridge) + mrpt::poses::CPose3D robotInitialPose; + geometry_msgs::Pose _src = initial_robot_pose.pose.pose; - // Init module - //------------- + robotInitialPose.x(_src.position.x); + robotInitialPose.y(_src.position.y); + + mrpt::math::CQuaternionDouble quat; + quat.x(_src.orientation.x); + quat.y(_src.orientation.y); + quat.z(_src.orientation.z); + quat.r(_src.orientation.w); + double roll, pitch, yaw; + quat.rpy(roll, pitch, yaw); + robotInitialPose.setYawPitchRoll(yaw,pitch,roll); + //robotInitialPose.phi(yaw); + + + + //Set the initial pose + laser_pose = CPose2D(robotInitialPose + LaserPoseOnTheRobot); + laser_oldpose = CPose2D(robotInitialPose + LaserPoseOnTheRobot); + + + // Init module (internal) + //------------------------ range_wf.setSize(1, width); //Resize vectors according to levels @@ -976,16 +1017,19 @@ void CLaserOdometry2D::PoseUpdate() //first, we'll publish the odometry over tf //--------------------------------------- - geometry_msgs::TransformStamped odom_trans; - odom_trans.header.stamp = ros::Time::now(); - odom_trans.header.frame_id = base_frame_id; - odom_trans.child_frame_id = odom_frame_id; - odom_trans.transform.translation.x = robot_pose.x(); - odom_trans.transform.translation.y = robot_pose.y(); - odom_trans.transform.translation.z = 0.0; - odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw()); - //send the transform - odom_broadcaster.sendTransform(odom_trans); + if (publish_tf) + { + geometry_msgs::TransformStamped odom_trans; + odom_trans.header.stamp = ros::Time::now(); + odom_trans.header.frame_id = base_frame_id; + odom_trans.child_frame_id = odom_frame_id; + odom_trans.transform.translation.x = robot_pose.x(); + odom_trans.transform.translation.y = robot_pose.y(); + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(robot_pose.yaw()); + //send the transform + odom_broadcaster.sendTransform(odom_trans); + } //next, we'll publish the odometry message over ROS //------------------------------------------------- @@ -1014,21 +1058,35 @@ void CLaserOdometry2D::PoseUpdate() void CLaserOdometry2D::LaserCallBack(const sensor_msgs::LaserScan::ConstPtr& new_scan) { - //Keep in memory the last received laser_scan - last_scan = *new_scan; + if (GT_pose_initialized) + { + //Keep in memory the last received laser_scan + last_scan = *new_scan; - //Initialize module on first scan - if (first_laser_scan) - { - Init(); - first_laser_scan = false; + //Initialize module on first scan + if (first_laser_scan) + { + Init(); + first_laser_scan = false; + } + else + { + //copy laser scan to internal variable + for (unsigned int i = 0; iranges[i]; + new_scan_available = true; + } } - else +} + + +void CLaserOdometry2D::initPoseCallBack(const nav_msgs::Odometry::ConstPtr& new_initPose) +{ + //Initialize module on first GT pose. Else do Nothing! + if (!GT_pose_initialized) { - //copy laser scan to internal variable - for (unsigned int i = 0; iranges[i]; - new_scan_available = true; + initial_robot_pose = *new_initPose; + GT_pose_initialized = true; } }