Skip to content
Snippets Groups Projects
Commit 0b30e442 authored by Florian Polster's avatar Florian Polster
Browse files

no ros.spin spinOnce instead

parent b80375d2
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
homer_mapping/src/slam_node.cpp
+ 323
− 312
View file @ 0b30e442
#include <homer_mapping/slam_node.h> #include <homer_mapping/slam_node.h>
SlamNode::SlamNode(ros::NodeHandle* nh) SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0) {
: m_HyperSlamFilter( 0 )
{
init(); init();
// subscribe to topics // subscribe to topics
m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>("/scan", 1, &SlamNode::callbackLaserScan, this); m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>(
m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>("/odom", 1, &SlamNode::callbackOdometry, this); "/scan", 1, &SlamNode::callbackLaserScan, this);
m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>("/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this); m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>(
m_InitialPoseSubscriber = nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, &SlamNode::callbackInitialPose, this); "/odom", 1, &SlamNode::callbackOdometry, this);
m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>("/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this); m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>(
m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>("/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this); "/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this); m_InitialPoseSubscriber =
m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this); nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>(
m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>("/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this); "/initialpose", 1, &SlamNode::callbackInitialPose, this);
m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>(
"/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this);
m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>(
"/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this);
m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
"/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this);
m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
"/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this);
m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>(
"/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this);
// advertise topics // advertise topics
m_PoseStampedPublisher = nh->advertise<geometry_msgs::PoseStamped>("/pose", 2); m_PoseStampedPublisher =
m_SLAMMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1); nh->advertise<geometry_msgs::PoseStamped>("/pose", 2);
m_SLAMMapPublisher =
nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ros::Time(0); geometry_msgs::PoseStamped poseMsg;
poseMsg.header.frame_id = "map"; poseMsg.header.stamp = ros::Time(0);
poseMsg.pose.position.x = 0.0; poseMsg.header.frame_id = "map";
poseMsg.pose.position.y = 0.0; poseMsg.pose.position.x = 0.0;
poseMsg.pose.position.z = 0.0; poseMsg.pose.position.y = 0.0;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0); poseMsg.pose.position.z = 0.0;
geometry_msgs::Quaternion quatMsg; tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to geometry_msgs::Quaternion quatMsg;
poseMsg.pose.orientation = quatMsg; tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
m_PoseStampedPublisher.publish(poseMsg); // to
poseMsg.pose.orientation = quatMsg;
//// //broadcast transform map -> base_link m_PoseStampedPublisher.publish(poseMsg);
tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, poseMsg.header.stamp, "map", "base_link")); //// //broadcast transform map -> base_link
Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y, tf::getYaw(poseMsg.pose.orientation)); tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta ); m_tfBroadcaster.sendTransform(tf::StampedTransform(
transform, poseMsg.header.stamp, "map", "base_link"));
Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
tf::getYaw(poseMsg.pose.orientation));
m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
m_ScatterVarTheta);
} }
void SlamNode::init() void SlamNode::init() {
{
double waitTime; double waitTime;
ros::param::get("/particlefilter/wait_time", waitTime); ros::param::get("/particlefilter/wait_time", waitTime);
m_WaitDuration = ros::Duration(waitTime); m_WaitDuration = ros::Duration(waitTime);
ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY); ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta); ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
m_DoMapping = true; m_DoMapping = true;
int particleNum; int particleNum;
ros::param::get("/particlefilter/particle_num", particleNum); ros::param::get("/particlefilter/particle_num", particleNum);
int particleFilterNum; int particleFilterNum;
ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num", particleFilterNum); ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num",
m_HyperSlamFilter = new HyperSlamFilter ( particleFilterNum, particleNum ); particleFilterNum);
m_HyperSlamFilter = new HyperSlamFilter(particleFilterNum, particleNum);
m_ReferenceOdometryTime = ros::Time(0); m_ReferenceOdometryTime = ros::Time(0);
m_LastMapSendTime = ros::Time(0); m_LastMapSendTime = ros::Time(0);
m_LastPositionSendTime = ros::Time(0); m_LastPositionSendTime = ros::Time(0);
m_LastMappingTime = ros::Time(0); m_LastMappingTime = ros::Time(0);
m_ReferenceOdometryTime = ros::Time(0); m_ReferenceOdometryTime = ros::Time(0);
m_laser_queue.clear(); m_laser_queue.clear();
m_odom_queue.clear(); m_odom_queue.clear();
} }
SlamNode::~SlamNode() SlamNode::~SlamNode() { delete m_HyperSlamFilter; }
{
delete m_HyperSlamFilter; void SlamNode::resetMaps() {
} ROS_INFO("Resetting maps..");
delete m_HyperSlamFilter;
m_HyperSlamFilter = 0;
void SlamNode::resetMaps() init();
{ geometry_msgs::PoseStamped poseMsg;
ROS_INFO( "Resetting maps.." ); poseMsg.header.stamp = ros::Time::now();
poseMsg.header.frame_id = "map";
delete m_HyperSlamFilter; poseMsg.pose.position.x = 0.0;
m_HyperSlamFilter = 0; poseMsg.pose.position.y = 0.0;
poseMsg.pose.position.z = 0.0;
init(); tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
geometry_msgs::PoseStamped poseMsg; geometry_msgs::Quaternion quatMsg;
poseMsg.header.stamp = ros::Time::now(); tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
poseMsg.header.frame_id = "map"; // to
poseMsg.pose.position.x = 0.0; poseMsg.pose.orientation = quatMsg;
poseMsg.pose.position.y = 0.0; m_PoseStampedPublisher.publish(poseMsg);
poseMsg.pose.position.z = 0.0;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0); m_LastLikeliestPose.set(0.0, 0.0);
geometry_msgs::Quaternion quatMsg; m_LastLikeliestPose.setTheta(0.0f);
tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to
poseMsg.pose.orientation = quatMsg; //// //broadcast transform map -> base_link
m_PoseStampedPublisher.publish(poseMsg); tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
m_tfBroadcaster.sendTransform(tf::StampedTransform(
m_LastLikeliestPose.set(0.0, 0.0); transform, poseMsg.header.stamp, "map", "base_link"));
m_LastLikeliestPose.setTheta(0.0f); Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
tf::getYaw(poseMsg.pose.orientation));
//// //broadcast transform map -> base_link m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0)); m_ScatterVarTheta);
m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, poseMsg.header.stamp, "map", "base_link"));
Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y, tf::getYaw(poseMsg.pose.orientation)); // sendMapDataMessage();
m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta );
// sendMapDataMessage();
} }
void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg) void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg) {
{ m_HyperSlamFilter->resetHigh();
m_HyperSlamFilter->resetHigh();
} }
void SlamNode::sendMapDataMessage(ros::Time mapTime) {
std::vector<int8_t> mapData;
nav_msgs::MapMetaData metaData;
OccupancyMap* occMap =
m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
occMap->getOccupancyProbabilityImage(mapData, metaData);
void SlamNode::sendMapDataMessage(ros::Time mapTime) // if ( width != height )
{ //{
std::vector<int8_t> mapData; // ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!");
nav_msgs::MapMetaData metaData; //}
// else
OccupancyMap* occMap = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap(); {
occMap->getOccupancyProbabilityImage (mapData, metaData); nav_msgs::OccupancyGrid mapMsg;
std_msgs::Header header;
//if ( width != height ) header.stamp = mapTime;
//{ header.frame_id = "map";
//ROS_ERROR_STREAM("ERROR: Map is not quadratic! can not send map!"); mapMsg.header = header;
//} mapMsg.info = metaData;
//else mapMsg.data = mapData;
{
nav_msgs::OccupancyGrid mapMsg; m_SLAMMapPublisher.publish(mapMsg);
std_msgs::Header header; }
header.stamp = mapTime;
header.frame_id = "map";
mapMsg.header = header;
mapMsg.info = metaData;
mapMsg.data = mapData;
m_SLAMMapPublisher.publish(mapMsg);
}
} }
void SlamNode::callbackUserDefPose( const geometry_msgs::Pose::ConstPtr& msg ) void SlamNode::callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg) {
{ Pose userdef_pose(msg->position.x, msg->position.y,
Pose userdef_pose(msg->position.x, msg->position.y, tf::getYaw(msg->orientation)); tf::getYaw(msg->orientation));
m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta ); m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
m_ScatterVarTheta);
} }
void SlamNode::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) void SlamNode::callbackInitialPose(
{ const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y, tf::getYaw(msg->pose.pose.orientation)); Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y,
m_HyperSlamFilter->setRobotPose( userdef_pose, m_ScatterVarXY, m_ScatterVarTheta ); tf::getYaw(msg->pose.pose.orientation));
m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
m_ScatterVarTheta);
} }
void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg) void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg) {
{ m_laser_queue.push_back(msg);
m_laser_queue.push_back(msg); if (m_laser_queue.size() > 5) // Todo param
if(m_laser_queue.size() > 5) //Todo param {
{ m_laser_queue.erase(m_laser_queue.begin());
m_laser_queue.erase(m_laser_queue.begin()); }
}
} }
void SlamNode::callbackOdometry( const nav_msgs::Odometry::ConstPtr& msg) void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) {
{ m_odom_queue.push_back(msg);
m_odom_queue.push_back(msg); static int last_i = 0;
static int last_i = 0; if (m_odom_queue.size() > 5) // Todo param
if(m_odom_queue.size() > 5) //Todo param {
{ last_i--;
last_i--; if (last_i < 0) {
if(last_i < 0) last_i = 0;
{ }
last_i = 0; m_odom_queue.erase(m_odom_queue.begin());
} }
m_odom_queue.erase(m_odom_queue.begin());
} static Pose last_interpolatedPose(0.0, 0.0, 0.0);
ros::Time currentOdometryTime = msg->header.stamp;
static Pose last_interpolatedPose(0.0, 0.0, 0.0); if ((ros::Time::now() - m_LastMappingTime > m_WaitDuration) &&
ros::Time currentOdometryTime = msg->header.stamp; m_odom_queue.size() > 1 && m_laser_queue.size() > 0) {
if( ( ros::Time::now() - m_LastMappingTime > m_WaitDuration ) && m_odom_queue.size() > 1 && m_laser_queue.size() > 0) int i, j;
{ bool got_match = false;
int i, j;
bool got_match = false; for (i = m_odom_queue.size() - 1; i > 0; i--) {
// Check if we would repeat a calculation which is already done but
for( i = m_odom_queue.size()-1; i > 0 ; i--) // still in the queue
{ if (m_odom_queue.at(i - 1)->header.stamp ==
//Check if we would repeat a calculation which is already done but still in the queue m_ReferenceOdometryTime) {
if(m_odom_queue.at(i-1)->header.stamp == m_ReferenceOdometryTime) break;
{ }
break;
} for (j = m_laser_queue.size() - 1; j > -1; j--) {
// find a laserscan in between two odometry readings (or at
for(j = m_laser_queue.size()-1; j > -1; j--) // the same time)
{ if ((m_laser_queue.at(j)->header.stamp >=
// find a laserscan in between two odometry readings (or at the same time) m_odom_queue.at(i - 1)->header.stamp) &&
if( (m_odom_queue.at(i)->header.stamp >=
(m_laser_queue.at(j)->header.stamp >= m_odom_queue.at(i-1)->header.stamp) m_laser_queue.at(j)->header.stamp)) {
&& got_match = true;
(m_odom_queue.at(i)->header.stamp >= m_laser_queue.at(j)->header.stamp) break;
) }
{ }
got_match = true; if (got_match) {
break; break;
} }
} }
if(got_match)
{ if (got_match) {
break; last_i = i;
} m_LastLaserData = m_laser_queue.at(j);
} m_LastMappingTime = m_LastLaserData->header.stamp;
if(got_match) float odoX = m_odom_queue.at(i)->pose.pose.position.x;
{ float odoY = m_odom_queue.at(i)->pose.pose.position.y;
last_i = i; float odoTheta =
m_LastLaserData = m_laser_queue.at(j); tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
m_LastMappingTime = m_LastLaserData->header.stamp; Pose currentOdometryPose(odoX, odoY, odoTheta);
currentOdometryTime = m_odom_queue.at(i)->header.stamp;
float odoX = m_odom_queue.at(i)->pose.pose.position.x;
float odoY = m_odom_queue.at(i)->pose.pose.position.y; odoX = m_odom_queue.at(i - 1)->pose.pose.position.x;
float odoTheta = tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation); odoY = m_odom_queue.at(i - 1)->pose.pose.position.y;
Pose currentOdometryPose ( odoX, odoY, odoTheta ); odoTheta =
currentOdometryTime = m_odom_queue.at(i)->header.stamp; tf::getYaw(m_odom_queue.at(i - 1)->pose.pose.orientation);
Pose lastOdometryPose(odoX, odoY, odoTheta);
odoX = m_odom_queue.at(i-1)->pose.pose.position.x; m_ReferenceOdometryPose = lastOdometryPose;
odoY = m_odom_queue.at(i-1)->pose.pose.position.y; m_ReferenceOdometryTime = m_odom_queue.at(i - 1)->header.stamp;
odoTheta = tf::getYaw(m_odom_queue.at(i-1)->pose.pose.orientation);
Pose lastOdometryPose ( odoX, odoY, odoTheta ); ros::Time startTime = ros::Time::now();
m_ReferenceOdometryPose = lastOdometryPose; // laserscan in between current odometry reading and
m_ReferenceOdometryTime = m_odom_queue.at(i-1)->header.stamp; // m_ReferenceOdometry
// -> calculate pose of robot during laser scan
ros::Duration d1 =
ros::Time startTime = ros::Time::now(); m_LastLaserData->header.stamp - m_ReferenceOdometryTime;
// laserscan in between current odometry reading and m_ReferenceOdometry ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
// -> calculate pose of robot during laser scan
ros::Duration d1 = m_LastLaserData->header.stamp - m_ReferenceOdometryTime; float timeFactor;
ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime; if (d1.toSec() == 0.0) {
timeFactor = 0.0f;
float timeFactor; } else if (d2.toSec() == 0.0) {
if(d1.toSec() == 0.0) timeFactor = 1.0f;
{ } else {
timeFactor = 0.0f; timeFactor = d1.toSec() / d2.toSec();
} }
else if(d2.toSec() == 0.0) ros::Duration duration = ros::Duration(0);
{
timeFactor = 1.0f; last_interpolatedPose = m_ReferenceOdometryPose.interpolate(
} currentOdometryPose, timeFactor);
else m_HyperSlamFilter->filter(last_interpolatedPose, m_LastLaserData,
{ m_LastLaserData->header.stamp, duration);
timeFactor = d1.toSec() / d2.toSec(); ros::Time finishTime = ros::Time::now();
}
ros::Duration duration = ros::Duration(0); m_LastLikeliestPose =
m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(
last_interpolatedPose = m_ReferenceOdometryPose.interpolate ( currentOdometryPose, timeFactor ); m_LastLaserData->header.stamp);
m_HyperSlamFilter->filter( last_interpolatedPose, m_LastLaserData, m_LastLaserData->header.stamp, duration);
ros::Time finishTime = ros::Time::now(); // TODO löschen
m_LastPositionSendTime = m_LastLaserData->header.stamp;
m_LastLikeliestPose = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(m_LastLaserData->header.stamp); // send map max. every 500 ms
if ((m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() >
//TODO löschen 0.5) {
m_LastPositionSendTime = m_LastLaserData->header.stamp; sendMapDataMessage(m_LastLaserData->header.stamp);
// send map max. every 500 ms m_LastMapSendTime = m_LastLaserData->header.stamp;
if ( (m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() > 0.5 ) }
{
sendMapDataMessage(m_LastLaserData->header.stamp); ROS_DEBUG_STREAM("Pos. data calc time: "
m_LastMapSendTime = m_LastLaserData->header.stamp; << (finishTime - startTime).toSec() << "s");
} ROS_DEBUG_STREAM("Map send Interval: "
<< (finishTime - m_LastPositionSendTime).toSec()
ROS_DEBUG_STREAM("Pos. data calc time: " << (finishTime - startTime).toSec() << "s" ); << "s");
ROS_DEBUG_STREAM("Map send Interval: " << ( finishTime - m_LastPositionSendTime ).toSec() << "s" ); }
} }
} Pose currentPose = m_LastLikeliestPose;
Pose currentPose = m_LastLikeliestPose; // currentPose.setX(currentPose.x() - last_interpolatedPose.x());
//currentPose.setX(currentPose.x() - last_interpolatedPose.x()); // currentPose.setY(currentPose.y() - last_interpolatedPose.y());
//currentPose.setY(currentPose.y() - last_interpolatedPose.y()); // currentPose.setTheta(currentPose.theta() -
//currentPose.setTheta(currentPose.theta() - last_interpolatedPose.theta()); // last_interpolatedPose.theta());
for(int i = (last_i-1<0?0:last_i-1); i < m_odom_queue.size(); i++) for (int i = (last_i - 1 < 0 ? 0 : last_i - 1); i < m_odom_queue.size();
{ i++) {
currentPose.setX(currentPose.x() + m_odom_queue.at(i)->twist.twist.linear.x); currentPose.setX(currentPose.x() +
currentPose.setY(currentPose.y() + m_odom_queue.at(i)->twist.twist.linear.y); m_odom_queue.at(i)->twist.twist.linear.x);
currentPose.setTheta(currentPose.theta() + m_odom_queue.at(i)->twist.twist.angular.z); currentPose.setY(currentPose.y() +
} m_odom_queue.at(i)->twist.twist.linear.y);
currentPose.setTheta(currentPose.theta() +
geometry_msgs::PoseStamped poseMsg; m_odom_queue.at(i)->twist.twist.angular.z);
//header }
poseMsg.header.stamp = msg->header.stamp;
poseMsg.header.frame_id = "map"; geometry_msgs::PoseStamped poseMsg;
// header
//position and orientation poseMsg.header.stamp = msg->header.stamp;
poseMsg.pose.position.x = currentPose.x(); poseMsg.header.frame_id = "map";
poseMsg.pose.position.y = currentPose.y();
poseMsg.pose.position.z = 0.0; // position and orientation
tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta()); poseMsg.pose.position.x = currentPose.x();
geometry_msgs::Quaternion quatMsg; poseMsg.pose.position.y = currentPose.y();
tf::quaternionTFToMsg(quatTF, quatMsg); //conversion from tf::Quaternion to geometry_msgs::Quaternion poseMsg.pose.position.z = 0.0;
poseMsg.pose.orientation = quatMsg; tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
m_PoseStampedPublisher.publish(poseMsg); geometry_msgs::Quaternion quatMsg;
//broadcast transform map -> base_link tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
//tf::Transform transform(quatTF,tf::Vector3(currentPose.x(), currentPose.y(), 0.0)); // to geometry_msgs::Quaternion
//m_tfBroadcaster.sendTransform(tf::StampedTransform(transform, msg->header.stamp, "map", "base_link")); poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
// broadcast transform map -> base_link
// tf::Transform transform(quatTF,tf::Vector3(currentPose.x(),
// currentPose.y(), 0.0));
// m_tfBroadcaster.sendTransform(tf::StampedTransform(transform,
// msg->header.stamp, "map", "base_link"));
} }
void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr &msg) void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr& msg) {
{
m_DoMapping = msg->data; m_DoMapping = msg->data;
m_HyperSlamFilter->setMapping ( m_DoMapping ); m_HyperSlamFilter->setMapping(m_DoMapping);
ROS_INFO_STREAM( "Do mapping is set to " << ( m_DoMapping ) ); ROS_INFO_STREAM("Do mapping is set to " << (m_DoMapping));
} }
void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr &msg) void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr& msg) {
{
resetMaps(); resetMaps();
} }
void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr &msg) void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
{
float res = msg->info.resolution; float res = msg->info.resolution;
int height = msg->info.height; // cell size int height = msg->info.height; // cell size
int width = msg->info.width; //cell size int width = msg->info.width; // cell size
//if(height!=width) { // if(height!=width) {
//ROS_ERROR("Height != width in loaded map"); // ROS_ERROR("Height != width in loaded map");
//return; // return;
//} //}
//convert map vector from ros format to robbie probability array // convert map vector from ros format to robbie probability array
float* map = new float[msg->data.size()]; float* map = new float[msg->data.size()];
//generate exploredRegion // generate exploredRegion
int minX = INT_MIN; int minX = INT_MIN;
int minY = INT_MIN; int minY = INT_MIN;
int maxX = INT_MAX; int maxX = INT_MAX;
int maxY = INT_MAX; int maxY = INT_MAX;
for(size_t y = 0; y < msg->info.height; y++) for (size_t y = 0; y < msg->info.height; y++) {
{ int yOffset = msg->info.width * y;
int yOffset = msg->info.width * y; for (size_t x = 0; x < msg->info.width; x++) {
for(size_t x = 0; x < msg->info.width; x++) int i = yOffset + x;
{ if (msg->data[i] == -1)
int i = yOffset + x; map[i] = 0.5;
if(msg->data[i] == -1 ) else
map[i] = 0.5; map[i] = msg->data[i] / 100.0;
else
map[i] = msg->data[i]/100.0; if (map[i] != 0.5) {
if (minX == INT_MIN || minX > (int)x) minX = (int)x;
if(map[i]!=0.5) { if (minY == INT_MIN || minY > (int)y) minY = (int)y;
if(minX==INT_MIN || minX > (int)x) if (maxX == INT_MAX || maxX < (int)x) maxX = (int)x;
minX = (int)x; if (maxY == INT_MAX || maxY < (int)y) maxY = (int)y;
if(minY==INT_MIN || minY > (int)y) }
minY = (int)y; }
if(maxX==INT_MAX || maxX < (int)x) }
maxX = (int)x; Box2D<int> exploredRegion = Box2D<int>(minX, minY, maxX, maxY);
if(maxY==INT_MAX || maxY < (int)y) OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width,
maxY = (int)y; height, exploredRegion);
} m_HyperSlamFilter->setOccupancyMap(occMap);
} m_HyperSlamFilter->setMapping(
} false); // is this already done by gui message?
Box2D<int> exploredRegion = Box2D<int> ( minX, minY, maxX, maxY ); ROS_INFO_STREAM("Replacing occupancy map");
OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width, height, exploredRegion);
m_HyperSlamFilter->setOccupancyMap( occMap );
m_HyperSlamFilter->setMapping( false ); //is this already done by gui message?
ROS_INFO_STREAM( "Replacing occupancy map" );
} }
void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg) void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
{
m_HyperSlamFilter->applyMasking(msg); m_HyperSlamFilter->applyMasking(msg);
} }
/** /**
* @brief main function * @brief main function
*/ */
int main(int argc, char** argv) int main(int argc, char** argv) {
{
ros::init(argc, argv, "homer_mapping"); ros::init(argc, argv, "homer_mapping");
ros::NodeHandle nh; ros::NodeHandle nh;
SlamNode slamNode(&nh); SlamNode slamNode(&nh);
ros::Rate loop_rate(50); ros::Rate loop_rate(160);
ros::spin(); while (ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
}
return 0; return 0;
} }
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment