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

ros-pkg: Added ResetPose.srv for odometry service "reset_odom_to_pose" to set intialial pose. Updated some method interfaces with latest library changes from trunk.

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1931 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-28 01:23:52 +00:00
parent b3933881ce
commit 52c6bd60eb
10 changed files with 68 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreNode.cpp
+1 -3
View File
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UFile.h>
int main(int argc, char** argv)
{
@@ -60,9 +61,6 @@ int main(int argc, char** argv)
uInsert(parameters,
std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(),
UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
uInsert(parameters,
std::make_pair(rtabmap::Parameters::kRtabmapDatabasePath(),
UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName())); // change default to ~/.ros
if(strcmp(argv[i], "--params") == 0)
{
src/CoreWrapper.cpp
+15 -11
View File
@@ -66,6 +66,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
mapFrameId_("map"),
odomFrameId_(""),
configPath_(""),
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
mapToOdom_(tf::Transform::getIdentity()),
transformThread_(0),
rate_(Parameters::defaultRtabmapDetectionRate()),
@@ -99,6 +100,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
}
pnh.param("config_path", configPath_, configPath_);
pnh.param("database_path", databasePath_, databasePath_);
pnh.param("frame_id", frameId_, frameId_);
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
@@ -122,7 +124,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
// update parameters with user input parameters (private)
uInsert(parameters, std::make_pair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
uInsert(parameters, std::make_pair(Parameters::kRtabmapDatabasePath(), UDirectory::homeDir()+"/.ros/"+Parameters::getDefaultDatabaseName())); // change default to ~/.ros
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string vStr;
@@ -138,10 +139,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
{
iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
}
else if(iter->first.compare(Parameters::kRtabmapDatabasePath()) == 0)
{
iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
}
else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0)
{
iter->second = uReplaceChar(iter->second, '~', UDirectory::homeDir());
@@ -224,13 +221,21 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
}
if(paused_)
{
UWARN("Node paused... dont' forget to call service \"resume\" to start rtabmap.");
ROS_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
}
if(deleteDbOnStart)
{
if(UFile::erase(databasePath_) == 0)
{
ROS_INFO("rtabmap: Deleted database \"%s\" (--delete_db_on_start is set).", databasePath_.c_str());
}
}
// Init RTAB-Map
rtabmap_.init(parameters, deleteDbOnStart);
rtabmap_.init(parameters, databasePath_);
ROS_INFO("rtabmap: using database from \"%s\".", rtabmap_.getDatabasePath().c_str());
ROS_INFO("rtabmap: Using database from \"%s\".", databasePath_.c_str());
// setup services
updateSrv_ = nh.advertiseService("update_parameters", &CoreWrapper::updateRtabmapCallback, this);
@@ -281,8 +286,7 @@ CoreWrapper::~CoreWrapper()
}
nh.deleteParam("is_rtabmap_paused");
std::string databasePath = rtabmap_.getDatabasePath();
printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath.c_str());
printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
}
ParametersMap CoreWrapper::loadParameters(const std::string & configFile)
@@ -861,7 +865,7 @@ bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("rtabmap: Reset");
rtabmap_.resetMemory(true);
rtabmap_.resetMemory();
return true;
}
src/CoreWrapper.h
+2 -1
View File
@@ -63,7 +63,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
class CoreWrapper
{
public:
CoreWrapper(bool deleteDbOnStart = false);
CoreWrapper(bool deleteDbOnStart);
virtual ~CoreWrapper();
private:
@@ -128,6 +128,7 @@ private:
std::string mapFrameId_;
std::string odomFrameId_;
std::string configPath_;
std::string databasePath_;
tf::Transform mapToOdom_;
boost::mutex mapToOdomMutex_;
src/GuiWrapper.cpp
+1 -1
View File
@@ -356,7 +356,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
std_srvs::Empty emptySrv;
rtabmap::RtabmapEventCmd * cmdEvent = (rtabmap::RtabmapEventCmd *)anEvent;
rtabmap::RtabmapEventCmd::Cmd cmd = cmdEvent->getCmd();
if(cmd == rtabmap::RtabmapEventCmd::kCmdDeleteMemory)
if(cmd == rtabmap::RtabmapEventCmd::kCmdResetMemory)
{
if(!ros::service::call("reset", emptySrv))
{
src/MapAssemblerNode.cpp
+2 -2
View File
@@ -107,8 +107,8 @@ public:
float cy = msg->nodes[i].cy;
//uncompress data
util3d::CompressionThread ctImage(msg->nodes[i].image.bytes, true);
util3d::CompressionThread ctDepth(msg->nodes[i].depth.bytes, true);
util3d::CompressionThread ctImage(&msg->nodes[i].image.bytes, true);
util3d::CompressionThread ctDepth(&msg->nodes[i].depth.bytes, true);
ctImage.start();
ctDepth.start();
ctImage.join();
src/OdometryROS.cpp
+32
View File
@@ -64,9 +64,28 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
ros::NodeHandle pnh("~");
Transform initialPose = Transform::getIdentity();
std::string initialPoseStr;
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("publish_tf", publishTf_, publishTf_);
pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
if(initialPoseStr.size())
{
std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
if(values.size() == 6)
{
initialPose = Transform(
atof(values[0].c_str()), atof(values[1].c_str()), atof(values[2].c_str()),
atof(values[3].c_str()), atof(values[4].c_str()), atof(values[5].c_str()));
}
else
{
ROS_ERROR("Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
"Identity will be used...", initialPoseStr.c_str());
}
}
//parameters
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -185,8 +204,13 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
ROS_INFO("Using OdometryBOW");
odometry_ = new rtabmap::OdometryBOW(parameters_);
}
if(!initialPose.isIdentity())
{
odometry_->reset(initialPose);
}
resetSrv_ = nh.advertiseService("reset_odom", &OdometryROS::reset, this);
resetToPoseSrv_ = nh.advertiseService("reset_odom_to_pose", &OdometryROS::resetToPose, this);
pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this);
}
@@ -380,6 +404,14 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
return true;
}
bool OdometryROS::resetToPose(rtabmap::ResetPose::Request& req, rtabmap::ResetPose::Response&)
{
Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
odometry_->reset(pose);
return true;
}
bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(paused_)
src/OdometryROS.h
+3
View File
@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_srvs/Empty.h>
#include <std_msgs/Header.h>
#include <rtabmap/ResetPose.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Parameters.h>
@@ -53,6 +54,7 @@ public:
Transform processData(SensorData & data, const std_msgs::Header & header, int & quality);
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetToPose(rtabmap::ResetPose::Request&, rtabmap::ResetPose::Response&);
bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -77,6 +79,7 @@ private:
ros::Publisher odomLastFrame_;
ros::Publisher odomMatches_;
ros::ServiceServer resetSrv_;
ros::ServiceServer resetToPoseSrv_;
ros::ServiceServer pauseSrv_;
ros::ServiceServer resumeSrv_;
tf::TransformBroadcaster tfBroadcaster_;
src/rviz/MapCloudDisplay.cpp
+2 -2
View File
@@ -258,8 +258,8 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
float cy = map.nodes[i].cy;
//uncompress data
util3d::CompressionThread ctImage(map.nodes[i].image.bytes, true);
util3d::CompressionThread ctDepth(map.nodes[i].depth.bytes, true);
util3d::CompressionThread ctImage(&map.nodes[i].image.bytes, true);
util3d::CompressionThread ctDepth(&map.nodes[i].depth.bytes, true);
ctImage.start();
ctDepth.start();
ctImage.join();