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

refactor

parent b849769a
No related branches found
No related tags found
1 merge request!4Recent changes
Hide whitespace changes
Inline Side-by-side
homer_mapping/include/homer_mapping/slam_node.h
+ 8
− 2
View file @ 13f45761
......@@ -93,12 +93,19 @@ private:
*/
void sendMapDataMessage(ros::Time mapTime = ros::Time::now());
void sendTfAndPose(Pose pose);
void sendPoseArray(std::vector<Pose>* poses);
Pose getInterpolatedPose(nav_msgs::Odometry::ConstPtr pose1,
nav_msgs::Odometry::ConstPtr pose2,
ros::Time laserTime);
/**
* This variables stores the last odometry measurement as reference that is
* used
* to compute the pose of the robot during a specific laserscan.
*/
Pose m_ReferenceOdometryPose;
Pose m_lastUsedPose;
Pose m_LastLikeliestPose;
......@@ -117,7 +124,6 @@ private:
*/
ros::Time m_LastMapSendTime;
/**
* This variable stores a pointer to the hyper slam filter
*/
......
homer_mapping/src/slam_node.cpp
+ 85
− 127
View file @ 13f45761
......@@ -33,26 +33,8 @@ SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0)
m_SLAMMapPublisher =
nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ros::Time(0);
poseMsg.header.frame_id = "map";
poseMsg.pose.position.x = 0.0;
poseMsg.pose.position.y = 0.0;
poseMsg.pose.position.z = 0.0;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
// to
poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
//// //broadcast transform map -> base_link
tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
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,
sendTfAndPose(Pose(0, 0, 0));
m_HyperSlamFilter->setRobotPose(Pose(0, 0, 0), m_ScatterVarXY,
m_ScatterVarTheta);
}
......@@ -93,29 +75,12 @@ void SlamNode::resetMaps()
m_HyperSlamFilter = 0;
init();
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ros::Time::now();
poseMsg.header.frame_id = "map";
poseMsg.pose.position.x = 0.0;
poseMsg.pose.position.y = 0.0;
poseMsg.pose.position.z = 0.0;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
// to
poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
sendTfAndPose(Pose(0, 0, 0));
m_LastLikeliestPose.set(0.0, 0.0);
m_LastLikeliestPose.setTheta(0.0f);
//// //broadcast transform map -> base_link
tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
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_HyperSlamFilter->setRobotPose(Pose(0, 0, 0), m_ScatterVarXY,
m_ScatterVarTheta);
// sendMapDataMessage();
......@@ -172,6 +137,49 @@ void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
}
}
void SlamNode::sendTfAndPose(Pose pose)
{
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ros::Time(0);
poseMsg.header.frame_id = "map";
poseMsg.pose.position.x = pose.x();
poseMsg.pose.position.y = pose.y();
poseMsg.pose.position.z = 0.0;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(pose.theta());
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
// to
poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
//// //broadcast transform map -> base_link
tf::Transform transform(quatTF, tf::Vector3(pose.x(), pose.y(), 0.0));
m_tfBroadcaster.sendTransform(tf::StampedTransform(
transform, poseMsg.header.stamp, "map", "base_link"));
}
void SlamNode::sendPoseArray(std::vector<Pose>* poses)
{
// Pose Array publishing
geometry_msgs::PoseArray poseArray = geometry_msgs::PoseArray();
poseArray.header.stamp = ros::Time::now();
poseArray.header.frame_id = "/map";
for (int i = 0; i < poses->size(); i++)
{
geometry_msgs::Pose tmpPose = geometry_msgs::Pose();
tmpPose.position.x = poses->at(i).x();
tmpPose.position.y = poses->at(i).y();
tf::Quaternion quatTF = tf::createQuaternionFromYaw(poses->at(i).theta());
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg);
tmpPose.orientation = quatMsg;
poseArray.poses.push_back(tmpPose);
}
delete poses;
m_PoseArrayPublisher.publish(poseArray);
}
void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
{
m_odom_queue.push_back(msg);
......@@ -226,44 +234,12 @@ void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
last_i = i;
sensor_msgs::LaserScan::ConstPtr laserData = m_laser_queue.at(j);
float odoX = m_odom_queue.at(i)->pose.pose.position.x;
float odoY = m_odom_queue.at(i)->pose.pose.position.y;
float odoTheta = tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
Pose currentOdometryPose(odoX, odoY, odoTheta);
currentOdometryTime = m_odom_queue.at(i)->header.stamp;
odoX = m_odom_queue.at(i - 1)->pose.pose.position.x;
odoY = m_odom_queue.at(i - 1)->pose.pose.position.y;
odoTheta = tf::getYaw(m_odom_queue.at(i - 1)->pose.pose.orientation);
Pose lastOdometryPose(odoX, odoY, odoTheta);
m_ReferenceOdometryPose = lastOdometryPose;
m_ReferenceOdometryTime = m_odom_queue.at(i - 1)->header.stamp;
// laserscan in between current odometry reading and
// m_ReferenceOdometry
// -> calculate pose of robot during laser scan
ros::Duration d1 =
laserData->header.stamp - m_ReferenceOdometryTime;
ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
float timeFactor;
if (d1.toSec() == 0.0)
{
timeFactor = 0.0f;
}
else if (d2.toSec() == 0.0)
{
timeFactor = 1.0f;
}
else
{
timeFactor = d1.toSec() / d2.toSec();
}
last_interpolatedPose = getInterpolatedPose(
m_odom_queue.at(i - 1), m_odom_queue.at(i), laserData->header.stamp);
last_interpolatedPose =
m_ReferenceOdometryPose.interpolate(currentOdometryPose, timeFactor);
Transformation2D trans = last_interpolatedPose - m_lastUsedPose;
// Rotate transformation to pose theta
float x = trans.x() * cos(-last_interpolatedPose.theta()) -
trans.y() * sin(-last_interpolatedPose.theta());
float y = trans.y() * cos(-last_interpolatedPose.theta()) +
......@@ -278,14 +254,7 @@ void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
m_LastLikeliestPose =
m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose();
// broadcast transform map -> base_link
tf::Transform transform(
tf::createQuaternionFromYaw(m_LastLikeliestPose.theta()),
tf::Vector3(m_LastLikeliestPose.x(), m_LastLikeliestPose.y(), 0.0));
tf::TransformBroadcaster tfBroadcaster;
tfBroadcaster.sendTransform(tf::StampedTransform(
transform, laserData->header.stamp, "/map", "/base_link"));
sendTfAndPose(m_LastLikeliestPose);
// send map max. every 500 ms
if ((laserData->header.stamp - m_LastMapSendTime).toSec() > 0.5)
......@@ -293,58 +262,47 @@ void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
sendMapDataMessage(laserData->header.stamp);
m_LastMapSendTime = laserData->header.stamp;
}
sendPoseArray(m_HyperSlamFilter->getBestSlamFilter()->getParticlePoses());
}
}
Pose currentPose = m_LastLikeliestPose;
// currentPose.setX(currentPose.x() - last_interpolatedPose.x());
// currentPose.setY(currentPose.y() - last_interpolatedPose.y());
// currentPose.setTheta(currentPose.theta() -
// last_interpolatedPose.theta());
// Add up Transformations
// 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.setY(currentPose.y() +
// m_odom_queue.at(i)->twist.twist.linear.y);
// currentPose.setTheta(currentPose.theta() +
// m_odom_queue.at(i)->twist.twist.angular.z);
//}
}
// Pose Publishing
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = msg->header.stamp;
poseMsg.header.frame_id = "map";
poseMsg.pose.position.x = currentPose.x();
poseMsg.pose.position.y = currentPose.y();
poseMsg.pose.position.z = 0.0;
tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg);
poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
Pose SlamNode::getInterpolatedPose(nav_msgs::Odometry::ConstPtr pose1,
nav_msgs::Odometry::ConstPtr pose2,
ros::Time laserTime)
{
ros::Time currentOdometryTime = pose2->header.stamp;
ros::Time lastOdometryTime = pose1->header.stamp;
// Pose Array publishing
geometry_msgs::PoseArray poseArray = geometry_msgs::PoseArray();
poseArray.header = poseMsg.header;
std::vector<Pose>* poses =
m_HyperSlamFilter->getBestSlamFilter()->getParticlePoses();
Pose lastOdometryPose(pose1->pose.pose.position.x,
pose1->pose.pose.position.y,
tf::getYaw(pose1->pose.pose.orientation));
for (int i = 0; i < poses->size(); i++)
Pose currentOdometryPose(pose2->pose.pose.position.x,
pose2->pose.pose.position.y,
tf::getYaw(pose2->pose.pose.orientation));
m_ReferenceOdometryTime = pose1->header.stamp;
ros::Duration d1 = laserTime - lastOdometryTime;
ros::Duration d2 = currentOdometryTime - lastOdometryTime;
float timeFactor;
if (d1.toSec() == 0.0)
{
geometry_msgs::Pose tmpPose = geometry_msgs::Pose();
tmpPose.position.x = poses->at(i).x();
tmpPose.position.y = poses->at(i).y();
tf::Quaternion quatTF = tf::createQuaternionFromYaw(poses->at(i).theta());
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg);
tmpPose.orientation = quatMsg;
poseArray.poses.push_back(tmpPose);
timeFactor = 0.0f;
}
delete poses;
m_PoseArrayPublisher.publish(poseArray);
else if (d2.toSec() == 0.0)
{
timeFactor = 1.0f;
}
else
{
timeFactor = d1.toSec() / d2.toSec();
}
return lastOdometryPose.interpolate(currentOdometryPose, timeFactor);
}
void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr& msg)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment