i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 18:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Pose goals can be sent directly to rtabmap node, Added SetGoal service (set target node id)

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-06 13:56:20 -05:00
parent 19f667b3f4
commit cd4975849a
12 changed files with 181 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+59 -17
View File
@@ -132,8 +132,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
mapGraph_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
// planning topics
goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_pose", 1);
goalSub_ = nh.subscribe("in_goal", 1, &CoreWrapper::goalCallback, this);
goalGlobalSub_ = nh.subscribe("in_goal_global", 1, &CoreWrapper::goalGlobalCallback, this);
nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("out_goal", 1);
goalReachedPub_ = nh.advertise<std_msgs::Empty>("goal_reached", 1);
globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
localPathPub_ = nh.advertise<nav_msgs::Path>("local_path", 1);
@@ -280,6 +281,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
@@ -946,16 +948,22 @@ void CoreWrapper::process(
}
else
{
Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathGoalId());
Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
if(!updatedGoalPose.isNull())
{
// Adjust the target pose relative to last node
if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back())
{
updatedGoalPose *= rtabmap_.getPathTransformToGoal();
}
// detect if the goal has changed or local map
// has changed so much that current goal drifted
if(currentMetricGoal_.getDistance(updatedGoalPose) > rtabmap_.getGoalReachedRadius()/2.0f)
{
currentMetricGoal_ = updatedGoalPose;
publishGoal(timeNow);
publishCurrentGoal(timeNow);
}
// publish local path
@@ -963,7 +971,7 @@ void CoreWrapper::process(
}
else
{
ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
}
}
}
@@ -984,19 +992,15 @@ void CoreWrapper::process(
rtabmap_.getWMSize()+rtabmap_.getSTMSize());
}
void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> > & poses)
{
currentMetricGoal_.setNull();
int id = msg->data;
ROS_INFO("Planning: set goal %d", id);
std::list<std::pair<int, Transform> > poses = rtabmap_.computePath(id);
if(poses.size())
{
currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathGoalId());
currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
if(currentMetricGoal_.isNull())
{
ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
rtabmap_.clearPath();
}
else
@@ -1013,7 +1017,7 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
path.poses.resize(poses.size());
int oi = 0;
std::stringstream stream;
for(std::list<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
for(std::list<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
path.poses[oi].header = path.header;
rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose);
@@ -1024,7 +1028,13 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
globalPathPub_.publish(path);
}
publishGoal(now);
// Adjust the target pose relative to last node
if(rtabmap_.getPathCurrentGoalId() == poses.back().first)
{
currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
}
publishCurrentGoal(now);
publishLocalPath(now);
}
}
@@ -1035,6 +1045,30 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
}
}
void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
{
Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose);
if(targetPose.isNull())
{
ROS_ERROR("Pose received is null!");
return;
}
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
goalCommonCallback(rtabmap_.computePath(targetPose, false));
}
void CoreWrapper::goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
{
Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose);
if(targetPose.isNull())
{
ROS_ERROR("Pose received is null!");
return;
}
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
goalCommonCallback(rtabmap_.computePath(targetPose, true));
}
bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -1278,6 +1312,14 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
return true;
}
bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
{
int id = req.target_node_id;
ROS_INFO("Planning: set goal %d", id);
goalCommonCallback(rtabmap_.computePath(id, req.in_global_graph));
return !currentMetricGoal_.isNull();
}
void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp)
{
if(infoPub_.getNumSubscribers())
@@ -1331,19 +1373,19 @@ void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp
}
}
void CoreWrapper::publishGoal(const ros::Time & stamp)
void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
{
if(!currentMetricGoal_.isNull())
{
ROS_INFO("Planning: Publishing next goal: Location %d pose=%s",
rtabmap_.getPathGoalId(), currentMetricGoal_.prettyPrint().c_str());
rtabmap_.getPathCurrentGoalId(), currentMetricGoal_.prettyPrint().c_str());
if(nextMetricGoalPub_.getNumSubscribers())
{
geometry_msgs::PoseStamped goalMsg;
goalMsg.header.frame_id = mapFrameId_;
goalMsg.header.stamp = ros::Time::now();
rtabmap_ros::transformToPoseMsg(currentMetricGoal_, goalMsg.pose);
ROS_INFO("Publishing next goal: %d", rtabmap_.getPathGoalId());
ROS_INFO("Publishing next goal: %d", rtabmap_.getPathCurrentGoalId());
nextMetricGoalPub_.publish(goalMsg);
}
}
src/CoreWrapper.h
+10 -3
View File
@@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/GetMap.h"
#include "rtabmap_ros/PublishMap.h"
#include "rtabmap_ros/SetGoal.h"
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
@@ -93,7 +94,10 @@ private:
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg);
void goalNodeCallback(const std_msgs::Int32ConstPtr & msg);
void goalCommonCallback(const std::list<std::pair<int, rtabmap::Transform> > & poses);
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void updateGoal(const ros::Time & stamp);
void process(
@@ -119,6 +123,7 @@ private:
bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& rep);
bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
rtabmap::ParametersMap loadParameters(const std::string & configFile);
void saveParameters(const std::string & configFile);
@@ -126,7 +131,7 @@ private:
void publishLoop(double tfDelay);
void publishStats(const rtabmap::Statistics & stats, const ros::Time & stamp);
void publishGoal(const ros::Time & stamp);
void publishCurrentGoal(const ros::Time & stamp);
void publishLocalPath(const ros::Time & stamp);
private:
@@ -151,7 +156,8 @@ private:
ros::Publisher mapGraph_;
//Planning stuff
ros::Subscriber goalNodeSub_;
ros::Subscriber goalSub_;
ros::Subscriber goalGlobalSub_;
ros::Publisher nextMetricGoalPub_;
ros::Publisher goalReachedPub_;
ros::Publisher globalPathPub_;
@@ -226,6 +232,7 @@ private:
ros::ServiceServer setModeMappingSrv_;
ros::ServiceServer getMapDataSrv_;
ros::ServiceServer publishMapDataSrv_;
ros::ServiceServer setGoalSrv_;
boost::thread* transformThread_;
src/GridMapAssemblerNode.cpp
+1 -1
View File
@@ -95,7 +95,7 @@ public:
if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
{
poses = rtabmap::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
poses = rtabmap::graph::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
}
if(gridMap_.getNumSubscribers())
src/MapAssemblerNode.cpp
+1 -1
View File
@@ -209,7 +209,7 @@ public:
}
if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
{
poses = rtabmap::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
poses = rtabmap::graph::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
}
if(assembledMapClouds_.getNumSubscribers())
src/MapOptimizerNode.cpp
+3 -3
View File
@@ -212,12 +212,12 @@ public:
{
if(optimizeFromLastNode_)
{
std::map<int, int> depthGraph = rtabmap::generateDepthGraph(constraints, poses.rbegin()->first);
rtabmap::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(constraints, poses.rbegin()->first);
rtabmap::graph::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
}
else
{
rtabmap::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
rtabmap::graph::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
}
mapToOdomMutex_.lock();
src/rviz/MapCloudDisplay.cpp
+1 -1
View File
@@ -325,7 +325,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
{
poses = rtabmap::radiusPosesFiltering(poses,
poses = rtabmap::graph::radiusPosesFiltering(poses,
node_filtering_radius_->getFloat(),
node_filtering_angle_->getFloat()*CV_PI/180.0);
}