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

Fixed rtabmap node not initializing when use_sim_time is true and no clock is yet published

Browse Source
This commit is contained in:
matlabbe
2018-10-02 18:06:46 -04:00
parent 786c481677
commit d0c5951b33
3 changed files with 26 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CoreWrapper.h
+1 -1
View File
@@ -261,7 +261,7 @@ private:
ros::ServiceServer octomapFullSrv_;
#endif
MoveBaseClient mbClient_;
MoveBaseClient * mbClient_;
boost::thread* transformThread_;
bool tfThreadRunning_;
launch/demo/demo_turtlebot_mapping.launch
+9 -3
View File
@@ -65,12 +65,20 @@
<param name="subscribe_scan" type="bool" value="true"/>
<param name="map_negative_poses_ignored" type="bool" value="true"/>
<!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base -->
<param name="use_action_for_goal" type="bool" value="true"/>
<remap from="move_base" to="/move_base"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap unless="$(arg rgbd_odometry)" from="odom" to="/odom"/>
<!-- Fix odom covariance as in simulation the covariance in /odom topic is high (0.1 for linear and 0.05 for angular) -->
<param unless="$(arg rgbd_odometry)" name="odom_frame_id" value="odom"/>
<param unless="$(arg rgbd_odometry)" name="odom_tf_linear_variance" value="0.001"/>
<param unless="$(arg rgbd_odometry)" name="odom_tf_angular_variance" value="0.001"/>
<!-- output -->
<remap from="grid_map" to="/map"/>
@@ -88,10 +96,8 @@
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Optimizer/Slam2D" type="string" value="true"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="GridGlobal/MinSize" type="string" value="20"/>
<param name="RGBD/OptimizeMaxError" type="string" value="0.1"/>
<!-- localization mode -->
src/CoreWrapper.cpp
+16 -10
View File
@@ -115,7 +115,7 @@ CoreWrapper::CoreWrapper() :
createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()),
previousStamp_(0),
mbClient_("move_base", true)
mbClient_(0)
{
globalPose_.header.stamp = ros::Time(0);
}
@@ -680,6 +680,8 @@ CoreWrapper::~CoreWrapper()
rtabmap_.close();
printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
delete mbClient_;
}
void CoreWrapper::loadParameters(const std::string & configFile, ParametersMap & parameters)
@@ -1645,9 +1647,9 @@ void CoreWrapper::process(
else
{
NODELET_WARN("Planning: Plan failed!");
if(mbClient_.isServerConnected())
if(mbClient_ && mbClient_->isServerConnected())
{
mbClient_.cancelGoal();
mbClient_->cancelGoal();
}
}
if(goalReachedPub_.getNumSubscribers())
@@ -2542,9 +2544,9 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
goalReachedPub_.publish(result);
}
}
if(mbClient_.isServerConnected())
if(mbClient_ && mbClient_->isServerConnected())
{
mbClient_.cancelGoal();
mbClient_->cancelGoal();
}
return true;
@@ -2753,17 +2755,21 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
if(useActionForGoal_)
{
if(!mbClient_.isServerConnected())
if(mbClient_ == 0 || !mbClient_->isServerConnected())
{
NODELET_INFO("Connecting to move_base action server...");
mbClient_.waitForServer(ros::Duration(5.0));
if(mbClient_ == 0)
{
mbClient_ = new MoveBaseClient("move_base", true);
}
mbClient_->waitForServer(ros::Duration(5.0));
}
if(mbClient_.isServerConnected())
if(mbClient_ && mbClient_->isServerConnected())
{
move_base_msgs::MoveBaseGoal goal;
goal.target_pose = poseMsg;
mbClient_.sendGoal(goal,
mbClient_->sendGoal(goal,
boost::bind(&CoreWrapper::goalDoneCb, this, _1, _2),
boost::bind(&CoreWrapper::goalActiveCb, this),
boost::bind(&CoreWrapper::goalFeedbackCb, this, _1));
@@ -2771,7 +2777,7 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
}
else
{
NODELET_ERROR("Cannot connect to move_base action server!");
NODELET_ERROR("Cannot connect to move_base action server (called \"%s\")!", this->getNodeHandle().resolveName("move_base").c_str());
}
}
if(nextMetricGoalPub_.getNumSubscribers())