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

Goal msg and SetGoal srv: added frame_id parameter to specify in which frame on the robot we want the goal

Browse Source
This commit is contained in:
matlabbe
2019-01-18 18:05:07 -05:00
parent b09ff6c511
commit c09eabd291
5 changed files with 67 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CoreWrapper.h
+7 -1
View File
@@ -143,7 +143,12 @@ private:
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
void goalCommonCallback(int id,
const std::string & label,
const std::string & frameId,
const rtabmap::Transform & pose,
const ros::Time & stamp,
double * planningTime = 0);
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
void updateGoal(const ros::Time & stamp);
@@ -250,6 +255,7 @@ private:
ros::Publisher goalReachedPub_;
ros::Publisher globalPathPub_;
ros::Publisher localPathPub_;
std::string goalFrameId_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_;
msg/Goal.msg
+3
View File
@@ -4,3 +4,6 @@ Header header
# Set either node_id or node_label
int32 node_id
string node_label
# optional: if not set, the base frame of the robot is used
string frame_id
scripts/patrol.py
+6 -1
View File
@@ -8,10 +8,12 @@ pub = rospy.Publisher('rtabmap/goal_node', Goal, queue_size=1)
waypoints = []
currentIndex = 0
waitingTime = 1.0
frameId = ""
def callback(data):
global currentIndex
global waitingTime
global frameId
if data.data:
rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
else:
@@ -23,6 +25,7 @@ def callback(data):
rospy.sleep(waitingTime)
msg = Goal()
msg.frame_id = frameId
try:
int(waypoints[currentIndex])
is_dig = True
@@ -43,7 +46,9 @@ def main():
rospy.init_node('patrol', anonymous=False)
sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback)
global waitingTime
global frameId
waitingTime = rospy.get_param('~time', waitingTime)
frameId = rospy.get_param('~frame_id', frameId)
rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]'))
@@ -74,7 +79,7 @@ def main():
if __name__ == '__main__':
if len(sys.argv) < 3:
print("usage: patrol.py waypointA waypointB waypointC ... [_time:=1] [topic remaps] (at least 2 waypoints, can be node id, landmark or label)")
print("usage: patrol.py waypointA waypointB waypointC ... [_time:=1 frame_id:=base_footprint] [topic remaps] (at least 2 waypoints, can be node id, landmark or label)")
else:
waypoints = sys.argv[1:]
waypoints = [x for x in waypoints if not x.startswith('/') and not x.startswith('_')]
src/CoreWrapper.cpp
+44 -7
View File
@@ -2098,6 +2098,7 @@ void CoreWrapper::process(
}
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
goalFrameId_.clear();
latestNodeWasReached_ = false;
}
}
@@ -2113,7 +2114,16 @@ void CoreWrapper::process(
rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius())
{
latestNodeWasReached_ = true;
currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
Transform goalLocalTransform = Transform::getIdentity();
if(!goalFrameId_.empty() && goalFrameId_.compare(frameId_) != 0)
{
Transform localT = rtabmap_ros::getTransform(frameId_, goalFrameId_, ros::Time::now(), tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
if(!localT.isNull())
{
goalLocalTransform = localT.inverse().to3DoF();
}
}
currentMetricGoal_ *= rtabmap_.getPathTransformToGoal()*goalLocalTransform;
}
}
@@ -2139,6 +2149,7 @@ void CoreWrapper::process(
}
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
goalFrameId_.clear();
latestNodeWasReached_ = false;
}
}
@@ -2260,6 +2271,7 @@ void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceSta
void CoreWrapper::goalCommonCallback(
int id,
const std::string & label,
const std::string & frameId,
const Transform & pose,
const ros::Time & stamp,
double * planningTime)
@@ -2302,6 +2314,7 @@ void CoreWrapper::goalCommonCallback(
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
goalFrameId_.clear();
latestNodeWasReached_ = false;
if(poses.size() == 0)
{
@@ -2322,6 +2335,7 @@ void CoreWrapper::goalCommonCallback(
if(!currentMetricGoal_.isNull())
{
NODELET_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
goalFrameId_ = frameId;
// Adjust the target pose relative to last node
if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
@@ -2329,7 +2343,16 @@ void CoreWrapper::goalCommonCallback(
if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius())
{
latestNodeWasReached_ = true;
currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
Transform goalLocalTransform = Transform::getIdentity();
if(!goalFrameId_.empty() && goalFrameId_.compare(frameId_) != 0)
{
Transform localT = rtabmap_ros::getTransform(frameId_, goalFrameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
if(!localT.isNull())
{
goalLocalTransform = localT.inverse().to3DoF();
}
}
currentMetricGoal_ *= rtabmap_.getPathTransformToGoal() * goalLocalTransform;
}
}
@@ -2414,7 +2437,7 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
targetPose = t * targetPose;
}
goalCommonCallback(0, "", targetPose, msg->header.stamp);
goalCommonCallback(0, "", "", targetPose, msg->header.stamp);
}
void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
@@ -2424,7 +2447,7 @@ void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
NODELET_ERROR("Node id or label should be set!");
return;
}
goalCommonCallback(msg->node_id, msg->node_label, Transform(), msg->header.stamp);
goalCommonCallback(msg->node_id, msg->node_label, msg->frame_id, Transform(), msg->header.stamp);
}
bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -2477,6 +2500,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
lastPoseIntermediate_ = false;
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
goalFrameId_.clear();
latestNodeWasReached_ = false;
mapsManager_.clear();
previousStamp_ = ros::Time(0);
@@ -2538,6 +2562,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
lastPose_.setIdentity();
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
goalFrameId_.clear();
latestNodeWasReached_ = false;
userDataMutex_.lock();
userData_ = cv::Mat();
@@ -3011,7 +3036,7 @@ bool CoreWrapper::getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::Get
bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
{
double planningTime = 0.0;
goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now(), &planningTime);
goalCommonCallback(req.node_id, req.node_label, req.frame_id, Transform(), ros::Time::now(), &planningTime);
const std::vector<std::pair<int, Transform> > & path = rtabmap_.getPath();
res.path_ids.resize(path.size());
res.path_poses.resize(path.size());
@@ -3032,6 +3057,7 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
rtabmap_.clearPath(0);
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
goalFrameId_.clear();
latestNodeWasReached_ = false;
if(goalReachedPub_.getNumSubscribers())
{
@@ -3331,6 +3357,7 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
rtabmap_.clearPath(1);
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
goalFrameId_.clear();
latestNodeWasReached_ = false;
}
}
@@ -3395,10 +3422,20 @@ void CoreWrapper::publishGlobalPath(const ros::Time & stamp)
rtabmap_ros::transformToPoseMsg(t*iter->second, path.poses[oi].pose);
++oi;
}
if(!rtabmap_.getPathTransformToGoal().isIdentity())
Transform goalLocalTransform = Transform::getIdentity();
if(!goalFrameId_.empty() && goalFrameId_.compare(frameId_) != 0)
{
Transform localT = rtabmap_ros::getTransform(frameId_, goalFrameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
if(!localT.isNull())
{
goalLocalTransform = localT.inverse().to3DoF();
}
}
if(!rtabmap_.getPathTransformToGoal().isIdentity() && !goalLocalTransform.isIdentity())
{
path.poses.resize(path.poses.size()+1);
Transform p = t * rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal();
Transform p = t * rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal() * goalLocalTransform;
rtabmap_ros::transformToPoseMsg(p, path.poses[path.poses.size()-1].pose);
}
globalPathPub_.publish(path);
srv/SetGoal.srv
+6
View File
@@ -1,8 +1,14 @@
#request
# Set either node_id or node_label
int32 node_id
string node_label
# optional: if not set, the base frame of the robot is used
string frame_id
---
#response
int32[] path_ids
geometry_msgs/Pose[] path_poses