mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-06 01:37:46 +08:00
ros-pkg: Navigation: fixed local costmap point cloud observation source, independent of Z value. Added data_player node to replay stuff saved in RTAB-Map databases (like a rosbag but for rtabmap.db).
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1945 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
@@ -0,0 +1,361 @@
|
||||
/*
|
||||
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <tf/tf.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <std_srvs/Empty.h>
|
||||
#include <rtabmap/MsgConversion.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
|
||||
#include <rtabmap/core/DBReader.h>
|
||||
|
||||
bool paused = false;
|
||||
bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
if(paused)
|
||||
{
|
||||
ROS_WARN("Already paused!");
|
||||
}
|
||||
else
|
||||
{
|
||||
paused = true;
|
||||
ROS_INFO("paused!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
if(!paused)
|
||||
{
|
||||
ROS_WARN("Already running!");
|
||||
}
|
||||
else
|
||||
{
|
||||
paused = false;
|
||||
ROS_INFO("resumed!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "data_player");
|
||||
|
||||
//ULogger::setType(ULogger::kTypeConsole);
|
||||
//ULogger::setLevel(ULogger::kDebug);
|
||||
//ULogger::setEventLevel(ULogger::kWarning);
|
||||
|
||||
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
std::string frameId = "base_link";
|
||||
std::string odomFrameId = "odom";
|
||||
std::string cameraFrameId = "camera_optical_link";
|
||||
double rate = 1.0f;
|
||||
std::string databasePath = "";
|
||||
bool publishTf = true;
|
||||
int startId = 0;
|
||||
|
||||
pnh.param("frame_id", frameId, frameId);
|
||||
pnh.param("odom_frame_id", odomFrameId, odomFrameId);
|
||||
pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
|
||||
pnh.param("rate", rate, rate);
|
||||
pnh.param("database", databasePath, databasePath);
|
||||
pnh.param("publish_tf", publishTf, publishTf);
|
||||
pnh.param("start_id", startId, startId);
|
||||
|
||||
ROS_INFO("frame_id = %s", frameId.c_str());
|
||||
ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
|
||||
ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
|
||||
ROS_INFO("database = %s", databasePath.c_str());
|
||||
ROS_INFO("rate = %f", rate);
|
||||
ROS_INFO("publish_tf = %s", publishTf?"true":"false");
|
||||
|
||||
rtabmap::DBReader reader(databasePath, rate);
|
||||
|
||||
if(databasePath.empty())
|
||||
{
|
||||
ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if(!reader.init(startId))
|
||||
{
|
||||
ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
|
||||
return -1;
|
||||
}
|
||||
|
||||
ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
|
||||
ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
|
||||
|
||||
image_transport::ImageTransport it(nh);
|
||||
image_transport::Publisher imagePub;
|
||||
image_transport::Publisher rgbPub;
|
||||
image_transport::Publisher depthPub;
|
||||
image_transport::Publisher leftPub;
|
||||
image_transport::Publisher rightPub;
|
||||
ros::Publisher rgbCamInfoPub;
|
||||
ros::Publisher depthCamInfoPub;
|
||||
ros::Publisher leftCamInfoPub;
|
||||
ros::Publisher rightCamInfoPub;
|
||||
ros::Publisher odometryPub;
|
||||
tf::TransformBroadcaster tfBroadcaster;
|
||||
ros::Publisher cloudPub;
|
||||
|
||||
cv::Mat image, depth, depth2d;
|
||||
float fx,fy,cx,cy;
|
||||
rtabmap::Transform localTransform, pose;
|
||||
int seq = 0;
|
||||
|
||||
reader.getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
|
||||
while(ros::ok() && !image.empty())
|
||||
{
|
||||
ROS_INFO("Reading image %d...", seq);
|
||||
|
||||
ros::Time time = ros::Time::now();
|
||||
|
||||
sensor_msgs::CameraInfo camInfoA; //rgb or left
|
||||
sensor_msgs::CameraInfo camInfoB; //depth or right
|
||||
|
||||
camInfoA.K.assign(0);
|
||||
camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
|
||||
camInfoA.R.assign(0);
|
||||
camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
|
||||
camInfoA.P.assign(0);
|
||||
camInfoA.P[10] = 1;
|
||||
|
||||
camInfoA.header.frame_id = cameraFrameId;
|
||||
camInfoA.header.stamp = time;
|
||||
|
||||
camInfoB = camInfoA;
|
||||
|
||||
int type = -1;
|
||||
if(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1))
|
||||
{
|
||||
//depth
|
||||
camInfoA.D.resize(5,0);
|
||||
|
||||
camInfoA.P[0] = fx;
|
||||
camInfoA.K[0] = fx;
|
||||
camInfoA.P[5] = fy;
|
||||
camInfoA.K[4] = fy;
|
||||
camInfoA.P[2] = cx;
|
||||
camInfoA.K[2] = cx;
|
||||
camInfoA.P[6] = cy;
|
||||
camInfoA.K[5] = cy;
|
||||
|
||||
camInfoB = camInfoA;
|
||||
|
||||
type=0;
|
||||
|
||||
if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
|
||||
if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
|
||||
if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
|
||||
if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
|
||||
}
|
||||
else if(!depth.empty() && depth.type() == CV_8U)
|
||||
{
|
||||
//stereo
|
||||
camInfoA.D.resize(8,0);
|
||||
|
||||
camInfoA.P[0] = fx;
|
||||
camInfoA.K[0] = fx;
|
||||
camInfoA.P[5] = fx; // fx = fy
|
||||
camInfoA.K[4] = fx; // fx = fy
|
||||
camInfoA.P[2] = cx;
|
||||
camInfoA.K[2] = cx;
|
||||
camInfoA.P[6] = cy;
|
||||
camInfoA.K[5] = cy;
|
||||
|
||||
camInfoB = camInfoA;
|
||||
camInfoB.P[3] = fy*-fx; // Right_Tx = -baseline*fx
|
||||
|
||||
type=1;
|
||||
|
||||
if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
|
||||
if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
|
||||
if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
|
||||
if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
|
||||
}
|
||||
|
||||
camInfoA.height = image.rows;
|
||||
camInfoA.width = image.cols;
|
||||
camInfoB.height = depth.rows;
|
||||
camInfoB.width = depth.cols;
|
||||
|
||||
// publish transforms first
|
||||
if(publishTf)
|
||||
{
|
||||
ros::Time tfExpiration = time + ros::Duration(1.0/rate);
|
||||
if(!localTransform.isNull())
|
||||
{
|
||||
tf::Transform baseToCamera;
|
||||
rtabmap::transformToTF(localTransform, baseToCamera);
|
||||
tfBroadcaster.sendTransform( tf::StampedTransform (baseToCamera, tfExpiration, frameId, cameraFrameId));
|
||||
}
|
||||
|
||||
if(!pose.isNull())
|
||||
{
|
||||
tf::Transform odomToBase;
|
||||
rtabmap::transformToTF(pose, odomToBase);
|
||||
tfBroadcaster.sendTransform( tf::StampedTransform (odomToBase, tfExpiration, odomFrameId, frameId));
|
||||
}
|
||||
}
|
||||
if(!pose.isNull())
|
||||
{
|
||||
if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
|
||||
|
||||
if(odometryPub.getNumSubscribers())
|
||||
{
|
||||
nav_msgs::Odometry odom;
|
||||
odom.child_frame_id = frameId;
|
||||
odom.header.frame_id = odomFrameId;
|
||||
odom.header.stamp = time;
|
||||
rtabmap::transformToPoseMsg(pose, odom.pose.pose);
|
||||
odometryPub.publish(odom);
|
||||
}
|
||||
}
|
||||
|
||||
if(type >= 0)
|
||||
{
|
||||
if(rgbCamInfoPub.getNumSubscribers() && type == 0)
|
||||
{
|
||||
rgbCamInfoPub.publish(camInfoA);
|
||||
}
|
||||
if(leftCamInfoPub.getNumSubscribers() && type == 1)
|
||||
{
|
||||
leftCamInfoPub.publish(camInfoA);
|
||||
}
|
||||
if(depthCamInfoPub.getNumSubscribers() && type == 0)
|
||||
{
|
||||
depthCamInfoPub.publish(camInfoB);
|
||||
}
|
||||
if(rightCamInfoPub.getNumSubscribers() && type == 1)
|
||||
{
|
||||
rightCamInfoPub.publish(camInfoB);
|
||||
}
|
||||
}
|
||||
|
||||
if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
|
||||
{
|
||||
cv_bridge::CvImage img;
|
||||
if(image.channels() == 1)
|
||||
{
|
||||
img.encoding = sensor_msgs::image_encodings::MONO8;
|
||||
}
|
||||
else
|
||||
{
|
||||
img.encoding = sensor_msgs::image_encodings::BGR8;
|
||||
}
|
||||
img.image = image;
|
||||
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
|
||||
imageRosMsg->header.frame_id = cameraFrameId;
|
||||
imageRosMsg->header.stamp = time;
|
||||
|
||||
if(imagePub.getNumSubscribers())
|
||||
{
|
||||
imagePub.publish(imageRosMsg);
|
||||
}
|
||||
if(rgbPub.getNumSubscribers() && type == 0)
|
||||
{
|
||||
rgbPub.publish(imageRosMsg);
|
||||
}
|
||||
if(leftPub.getNumSubscribers() && type == 1)
|
||||
{
|
||||
leftPub.publish(imageRosMsg);
|
||||
leftCamInfoPub.publish(camInfoA);
|
||||
}
|
||||
}
|
||||
|
||||
if(depthPub.getNumSubscribers() && !depth.empty() && type==0)
|
||||
{
|
||||
cv_bridge::CvImage img;
|
||||
if(depth.type() == CV_32FC1)
|
||||
{
|
||||
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
|
||||
}
|
||||
else
|
||||
{
|
||||
img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
|
||||
}
|
||||
img.image = depth;
|
||||
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
|
||||
imageRosMsg->header.frame_id = cameraFrameId;
|
||||
imageRosMsg->header.stamp = time;
|
||||
|
||||
depthPub.publish(imageRosMsg);
|
||||
depthCamInfoPub.publish(camInfoB);
|
||||
}
|
||||
|
||||
if(rightPub.getNumSubscribers() && !depth.empty() && type==1)
|
||||
{
|
||||
cv_bridge::CvImage img;
|
||||
img.encoding = sensor_msgs::image_encodings::MONO8;
|
||||
img.image = depth;
|
||||
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
|
||||
imageRosMsg->header.frame_id = cameraFrameId;
|
||||
imageRosMsg->header.stamp = time;
|
||||
|
||||
rightPub.publish(imageRosMsg);
|
||||
rightCamInfoPub.publish(camInfoB);
|
||||
}
|
||||
|
||||
ros::spinOnce();
|
||||
|
||||
while(ros::ok() && paused)
|
||||
{
|
||||
uSleep(100);
|
||||
ros::spinOnce();
|
||||
}
|
||||
|
||||
image = cv::Mat();
|
||||
depth = cv::Mat();
|
||||
depth2d = cv::Mat();
|
||||
fx=fy=cx=cy=seq=0;
|
||||
pose.setNull();
|
||||
localTransform.setNull();
|
||||
reader.getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
|
||||
}
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user