Commit 3c685eda authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel
Browse files

wip movement model and laser odom

parent 4004e60a
......@@ -54,7 +54,12 @@ catkin_package(
LIBRARIES
)
add_executable(homer_mapping src/slam_node.cpp src/scan_map.cpp)
add_executable(homer_mapping
src/slam_node.cpp
src/scan_map.cpp
src/LaserOdom.cpp
src/MovementModel.cpp
)
target_link_libraries(
homer_mapping
......
/homer_mapping/laser_odom/max_icp_iterations: 30
/homer_mapping/laser_odom/max_ceres_iterations: 50
/homer_mapping/laser_odom/loss_parameter: 0.5
/homer_mapping/laser_odom/max_search_radius: 0.5
/homer_mapping/laser_odom/sampling_points: 250
/homer_mapping/movement_model/max_acceleration_xy: 5 # m/s^2
/homer_mapping/movement_model/max_acceleration_theta: 30 # rad/s^2
/homer_mapping/quad_map/resolution: 0.03 # m meter per cell
/homer_mapping/quad_map/depth: 16 # tree depth 0 < depth < 65 (max_map_size = resolution*2^(depth-1))
/homer_mapping/scan_map/points_per_cell: 5 # points per cell
/homer_mapping/scan_map/max_icp_iterations: 30
/homer_mapping/scan_map/max_ceres_iterations: 50
......@@ -7,8 +17,6 @@
/homer_mapping/scan_map/max_search_radius: 20
/homer_mapping/scan_map/search_radius: 2
/homer_mapping/scan_map/sampling_points: 250
/homer_mapping/scan_map/max_acceleration_xy: 5 # m/s^2
/homer_mapping/scan_map/max_acceleration_theta: 30 # rad/s^2
/homer_mapping/base_frame: base_link
/homer_mapping/max_rotation_per_second: 0.5 # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
......
#pragma once
#include <sensor_msgs/LaserScan.h>
#include <tf/tf.h>
tf::Pose matchScans(sensor_msgs::LaserScan::ConstPtr last_scan, sensor_msgs::LaserScan::ConstPtr currnet_scan, tf::Pose pose_hint, double max_error_x = 0.1, double max_error_y = 0.1, double max_error_theta = 0.1);
#pragma once
#include <ros/ros.h>
#include <tf/tf.h>
struct MovementModelGuess
{
tf::Pose pose;
double error[3]; // x, y, theta
};
class MovementModel
{
public:
MovementModel();
void insertPose(tf::Pose pose, ros::Time stamp);
MovementModelGuess getNextPose(ros::Time stamp);
private:
std::pair<tf::Pose, ros::Time> m_last_poses[3];
};
......@@ -23,6 +23,7 @@
#include <tf/transform_broadcaster.h>
#include <homer_mapping/scan_map.h>
#include <homer_mapping/SafeQueue.h>
#include <homer_mapping/MovementModel.h>
class SlamNode
{
......@@ -33,7 +34,6 @@ public:
private:
void callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
void callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg);
void callbackResetMap(const std_msgs::Empty::ConstPtr& msg);
void callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
......@@ -41,12 +41,8 @@ private:
void sendTfAndPose(ros::Time stamp);
tf::Pose getInterpolatedPose(nav_msgs::Odometry::ConstPtr pose1, nav_msgs::Odometry::ConstPtr pose2, ros::Time time);
tf::Pose m_last_used_odom_pose;
ros::Time m_last_map_send_time;
SafeQueue<sensor_msgs::LaserScan::ConstPtr> m_laser_queue;
SafeQueue<nav_msgs::Odometry::ConstPtr> m_odom_queue;
ScanMap m_scan_map;
tf::Pose m_last_pose;
......@@ -54,6 +50,8 @@ private:
mutable std::mutex m_last_pose_mutex;
bool m_first_run;
MovementModel m_movement_model;
tf::TransformListener m_tf_listener;
tf::TransformBroadcaster m_tf_broadcaster;
......@@ -65,6 +63,5 @@ private:
ros::Publisher m_pose_publisher;
ros::Publisher m_map_publisher;
const unsigned int MAX_ODOM_QUEUE_SIZE = 50;
const unsigned int MAX_LASER_QUEUE_SIZE = 20;
};
#include <homer_mapping/LaserOdom.h>
#include <algorithm>
#include <random>
#include <vector>
#include <ros/ros.h>
#include <ceres/ceres.h>
class MatchScanCostFunctor {
public:
MatchScanCostFunctor(Eigen::Vector2d new_scan_point, Eigen::Vector2d old_scan_point): sp1(new_scan_point), sp2(old_scan_point) {}
template <typename T>
bool operator()(const T* const x, const T* const y, const T* const theta, T* e) const
{
Eigen::Matrix<T, 2, 1> p;
p.x() = x[0] + sp1.x() * cos(theta[0]) - sp1.y() * sin(theta[0]);
p.y() = y[0] + sp1.y() * cos(theta[0]) + sp1.x() * sin(theta[0]);
p = p - sp2;
e[0] = T(p.squaredNorm());
return true;
}
private:
Eigen::Vector2d sp1;
Eigen::Vector2d sp2;
};
void scanToVector2d(sensor_msgs::LaserScan::ConstPtr scan, std::vector<Eigen::Vector2d> &scan_points)
{
scan_points.resize(scan->ranges.size());
size_t used_points = 0;
for( size_t i = 0; i < scan->ranges.size(); i++ )
{
const float range = scan->ranges[i];
if(std::isnan(range) || range < scan->range_min || range > scan->range_max)
{
continue;
}
const float angle = scan->angle_min + i * scan->angle_increment;
scan_points[used_points].x() = cos(angle) * range;
scan_points[used_points].y() = sin(angle) * range;
used_points++;
}
scan_points.resize(used_points);
}
tf::Pose matchScans(sensor_msgs::LaserScan::ConstPtr last_scan, sensor_msgs::LaserScan::ConstPtr currnet_scan, tf::Pose pose_hint, double max_error_x, double max_error_y, double max_error_theta)
{
std::vector<Eigen::Vector2d> old_scan_points(last_scan->ranges.size());
std::vector<Eigen::Vector2d> new_scan_points(currnet_scan->ranges.size());
scanToVector2d(last_scan, old_scan_points);
scanToVector2d(currnet_scan, new_scan_points);
const double min_x = pose_hint.getOrigin().x() - max_error_x;
const double max_x = pose_hint.getOrigin().x() + max_error_x;
const double min_y = pose_hint.getOrigin().y() - max_error_y;
const double max_y = pose_hint.getOrigin().y() + max_error_y;
const double min_theta = tf::getYaw(pose_hint.getRotation()) - max_error_theta;
const double max_theta = tf::getYaw(pose_hint.getRotation()) + max_error_theta;
double last_x = pose_hint.getOrigin().x();
double last_y = pose_hint.getOrigin().y();
double last_theta = tf::getYaw(pose_hint.getRotation());
int max_icp_iterations = 30;
ros::param::getCached("/homer_mapping/laser_odom/max_icp_iterations", max_icp_iterations);
int max_ceres_iterations = 50;
ros::param::getCached("/homer_mapping/laser_odom/max_ceres_iterations", max_ceres_iterations);
double max_search_radius = 0.5;
ros::param::getCached("/homer_mapping/laser_odom/max_search_radius", max_search_radius);
int sampling_points = 200;
ros::param::getCached("/homer_mapping/laser_odom/sampling_points", sampling_points);
double loss_parameter = 0.5;
ros::param::getCached("/homer_mapping/laser_odom/loss_parameter", loss_parameter);
std::vector<Eigen::Vector2d> sampled_points;
sampled_points.reserve(sampling_points);
std::sample(new_scan_points.begin(), new_scan_points.end(), std::back_inserter(sampled_points), sampling_points, std::mt19937{std::random_device{}()});
for( size_t iteration = 0; iteration < max_icp_iterations; iteration++ )
{
double x = last_x;
double y = last_y;
double theta = last_theta;
std::shared_ptr<ceres::Problem> problem(new ceres::Problem);
const double max_search_radius_squared = max_search_radius * max_search_radius;
for( const auto& point : sampled_points )
{
Eigen::Vector2d p;
p.x() = x + (point.x() * cos(theta) - point.y() * sin(theta));
p.y() = y + (point.y() * cos(theta) + point.x() * sin(theta));
std::vector<std::pair<Eigen::Vector2d, double>> tmp_points;
for( const auto& old_point : old_scan_points )
{
const double squaredDistance = (old_point - p).squaredNorm();
if( squaredDistance < max_search_radius_squared )
{
tmp_points.push_back(std::make_pair(old_point, sqrt(squaredDistance)));
}
}
if( tmp_points.size() == 0 )
{
continue;
}
std::sort(tmp_points.begin(), tmp_points.end(), [](const std::pair<Eigen::Vector2d, double> &a, const std::pair<Eigen::Vector2d, double> &b) -> bool { return a.second < b.second; });
size_t num_points = std::min(5UL, tmp_points.size());
Eigen::Vector2d old_scan_point(0.0, 0.0);
for(size_t i = 0; i < num_points; i++)
{
old_scan_point += tmp_points[i].first;
}
old_scan_point.x() /= num_points;
old_scan_point.y() /= num_points;
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<MatchScanCostFunctor, 1, 1, 1, 1>(
new MatchScanCostFunctor(point, old_scan_point));
problem->AddResidualBlock(cost_function, new ceres::CauchyLoss( loss_parameter ), &x, &y, &theta);
}
problem->SetParameterLowerBound(&x, 0, min_x);
problem->SetParameterUpperBound(&x, 0, max_x);
problem->SetParameterLowerBound(&y, 0, min_y);
problem->SetParameterUpperBound(&y, 0, max_y);
problem->SetParameterLowerBound(&theta, 0, min_theta);
problem->SetParameterUpperBound(&theta, 0, max_theta);
ceres::Solver::Options options;
options.max_num_iterations = max_ceres_iterations;
ceres::Solver::Summary summary;
ceres::Solve(options, problem.get(), &summary);
if(!summary.IsSolutionUsable())
{
ROS_INFO_STREAM("solution not usable");
break;
}
const double delta_x = last_x - x;
last_x = x;
const double delta_y = last_y - y;
last_y = y;
const double delta_theta = last_theta - theta;
last_theta = theta;
const double distance = sqrt(delta_x * delta_x + delta_y * delta_y);
if(distance > 0.005)
{
//ROS_INFO_STREAM("distance from last pose: " << distance);
continue;
}
if(abs(delta_theta) > 0.005)
{
//ROS_INFO_STREAM("angle to last pose: " << delta_theta);
continue;
}
ROS_INFO_STREAM("done after " << iteration+1 << " iterations");
break;
}
return tf::Pose(tf::createQuaternionFromYaw(-last_theta), tf::Vector3(last_x, last_y, 0));
}
#include <homer_mapping/MovementModel.h>
MovementModel::MovementModel()
{
}
void MovementModel::insertPose(tf::Pose pose, ros::Time stamp)
{
m_last_poses[2] = m_last_poses[1];
m_last_poses[1] = m_last_poses[0];
m_last_poses[0] = std::make_pair(pose, stamp);
}
MovementModelGuess MovementModel::getNextPose(ros::Time stamp)
{
double max_acceleration_xy = 5;
ros::param::getCached("/homer_mapping/movement_model/max_acceleration_xy", max_acceleration_xy);
double max_acceleration_theta = 20;
ros::param::getCached("/homer_mapping/movement_model/max_acceleration_theta", max_acceleration_theta);
//guess = s_0 + v_0 * t + 1/2 * a * t^2;
double v_x[2] = { 0.0, 0.0 };
double v_y[2] = { 0.0, 0.0 };
double v_theta[2] = { 0.0, 0.0 };
for(int i = 0; i < 2; i++)
{
double delta_t = (m_last_poses[i].second - m_last_poses[i+1].second).toSec();
if(delta_t > 0.0001)
{
tf::Vector3 delta_s = m_last_poses[i].first.getOrigin() - m_last_poses[i+1].first.getOrigin();
double delta_theta = tf::getYaw(m_last_poses[i].first.getRotation()) - tf::getYaw(m_last_poses[i+1].first.getRotation());
v_x[i] = delta_s.x()/delta_t;
v_y[i] = delta_s.y()/delta_t;
v_theta[i] = delta_theta/delta_t;
}
}
double a_x = 0.0;
double a_y = 0.0;
double a_theta = 0.0;
{
double delta_t = (m_last_poses[0].second - m_last_poses[1].second).toSec();
if(delta_t > 0.0001)
{
double delta_v_x = v_x[1] - v_x[0];
double delta_v_y = v_y[1] - v_y[0];
double delta_v_theta = v_theta[1] - v_theta[0];
a_x = delta_v_x/delta_t;
a_y = delta_v_y/delta_t;
a_theta = delta_v_theta/delta_t;
}
}
MovementModelGuess guess;
double delta_t = (stamp - m_last_poses[0].second).toSec();
double guess_x = m_last_poses[0].first.getOrigin().x() + v_x[0] * delta_t + 0.5 * a_x * delta_t * delta_t;
double guess_y = m_last_poses[0].first.getOrigin().y() + v_y[0] * delta_t + 0.5 * a_y * delta_t * delta_t;
double guess_theta = tf::getYaw(m_last_poses[0].first.getRotation()) + v_theta[0] * delta_t + 0.5 * a_theta * delta_t * delta_t;
guess.pose = tf::Pose(tf::createQuaternionFromYaw(guess_theta), tf::Vector3(guess_x, guess_y, 0));
double max_error_xy = max_acceleration_xy*delta_t*delta_t/2;
double max_error_theta = max_acceleration_theta*delta_t*delta_t/2;
guess.error[0] = max_error_xy;
guess.error[1] = max_error_xy;
guess.error[2] = max_error_theta;
return guess;
}
......@@ -16,7 +16,7 @@ class MyCostFunctor {
p.x() = x[0] + sp.x() * cos(theta[0]) - sp.y() * sin(theta[0]);
p.y() = y[0] + sp.y() * cos(theta[0]) + sp.x() * sin(theta[0]);
p = p - mp;
e[0] = T(p.norm());
e[0] = T(p.squaredNorm());
return true;
}
......@@ -89,7 +89,6 @@ tf::Pose ScanMap::localizeScan(sensor_msgs::LaserScan::ConstPtr scan, tf::Pose p
double y = last_pose.getOrigin().y();
double theta = tf::getYaw(last_pose.getRotation());
std::shared_ptr<ceres::Problem> problem(new ceres::Problem);
ceres::LossFunction* loss_function = new ceres::CauchyLoss( loss_parameter );
size_t skipped_points = 0;
for( const auto& point : sampled_points )
......@@ -153,12 +152,14 @@ tf::Pose ScanMap::localizeScan(sensor_msgs::LaserScan::ConstPtr scan, tf::Pose p
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<MyCostFunctor, 1, 1, 1, 1>(
new MyCostFunctor(point, map_point));
problem->AddResidualBlock(cost_function, loss_function, &x, &y, &theta);
problem->AddResidualBlock(cost_function, new ceres::CauchyLoss( loss_parameter ), &x, &y, &theta);
}
/*
if( skipped_points > 0 )
{
ROS_INFO_STREAM("skipped " << skipped_points << " points");
}
*/
problem->SetParameterLowerBound(&x, 0, min_x);
problem->SetParameterUpperBound(&x, 0, max_x);
......@@ -182,14 +183,14 @@ tf::Pose ScanMap::localizeScan(sensor_msgs::LaserScan::ConstPtr scan, tf::Pose p
if(current_pose.getOrigin().distance(last_pose.getOrigin()) > 0.005)
{
ROS_INFO_STREAM("distance from last pose: " << current_pose.getOrigin().distance(last_pose.getOrigin()));
//ROS_INFO_STREAM("distance from last pose: " << current_pose.getOrigin().distance(last_pose.getOrigin()));
last_pose = current_pose;
continue;
}
if(current_pose.getRotation().angle(last_pose.getRotation()) > 0.005)
{
ROS_INFO_STREAM("angle to last pose: " << current_pose.getRotation().angle(last_pose.getRotation()));
//ROS_INFO_STREAM("angle to last pose: " << current_pose.getRotation().angle(last_pose.getRotation()));
last_pose = current_pose;
continue;
}
......@@ -393,9 +394,9 @@ void ScanMap::insertScan(sensor_msgs::LaserScan::ConstPtr scan, tf::Pose pose)
continue;
if(cell->points.size() >= max_points_per_cell)
continue;
//if(!cell->free)
// cell->points.push_back(p_world);
cell->points.push_back(p_world);
if(!cell->free)
cell->points.push_back(p_world);
//cell->points.push_back(p_world);
// Mark free cells
auto points_on_line = m_quad_map.createNodesOnLine(laser_position, p_end_world);
......
#include <homer_mapping/slam_node.h>
#include <homer_mapping/LaserOdom.h>
#include <thread>
// TODO
......@@ -9,16 +10,12 @@ SlamNode::SlamNode(ros::NodeHandle* nh, int max_depth, double resolution):
m_scan_map(max_depth, resolution),
m_last_pose(tf::Quaternion(0, 0, 0, 1)),
m_first_run(true),
m_laser_queue(MAX_LASER_QUEUE_SIZE),
m_odom_queue(MAX_ODOM_QUEUE_SIZE)
m_laser_queue(MAX_LASER_QUEUE_SIZE)
{
// subscribe to topics
std::string scan_topic;
nh->param<std::string>("/homer_scan_topic", scan_topic, "/scan");
m_laser_scan_subscriber = nh->subscribe<sensor_msgs::LaserScan>(scan_topic, 1, &SlamNode::callbackLaserScan, this);
std::string odom_topic;
nh->param<std::string>("/homer_odom_topic", odom_topic, "/odom");
m_odometry_subscriber = nh->subscribe<nav_msgs::Odometry>(odom_topic, 1, &SlamNode::callbackOdometry, this);
m_reset_maps_subscriber = nh->subscribe<std_msgs::Empty>("/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this);
m_initial_pose_subscriber = nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, &SlamNode::callbackInitialPose, this);
......@@ -80,6 +77,9 @@ void SlamNode::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStampe
m_last_pose = m_scan_map.localizeScan(laser_data, pose);
m_last_pose_time = laser_data->header.stamp;
m_movement_model.insertPose(m_last_pose, m_last_pose_time);
m_movement_model.insertPose(m_last_pose, m_last_pose_time);
m_movement_model.insertPose(m_last_pose, m_last_pose_time);
}
}
......@@ -101,41 +101,6 @@ void SlamNode::sendTfAndPose(ros::Time stamp)
m_tf_broadcaster.sendTransform(tf::StampedTransform(m_last_pose, stamp, "map", base_frame));
}
void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
{
m_odom_queue.enqueue(msg);
}
tf::Pose SlamNode::getInterpolatedPose(nav_msgs::Odometry::ConstPtr pose1, nav_msgs::Odometry::ConstPtr pose2, ros::Time time)
{
ros::Time current_odometry_time = pose2->header.stamp;
ros::Time last_odometry_time = pose1->header.stamp;
tf::Pose last_odometry_pose;
tf::poseMsgToTF(pose1->pose.pose, last_odometry_pose);
tf::Pose current_odometry_pose;
tf::poseMsgToTF(pose2->pose.pose, current_odometry_pose);
ros::Duration d1 = time - last_odometry_time;
ros::Duration d2 = current_odometry_time - last_odometry_time;
float time_factor;
if (d1.toSec() == 0.0)
time_factor = 0.0f;
else if (d2.toSec() == 0.0)
time_factor = 1.0f;
else
time_factor = d1.toSec() / d2.toSec();
tf::Quaternion interpolated_quaternion = last_odometry_pose.getRotation().slerp(current_odometry_pose.getRotation(), time_factor);
tf::Pose interpolated_pose(interpolated_quaternion, last_odometry_pose.getOrigin().lerp(current_odometry_pose.getOrigin(), time_factor));
return interpolated_pose;
}
void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr& msg)
{
m_scan_map.reset();
......@@ -143,24 +108,11 @@ void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr& msg)
void SlamNode::workerThread()
{
nav_msgs::Odometry::ConstPtr last_odom = m_odom_queue.dequeue();
nav_msgs::Odometry::ConstPtr current_odom = m_odom_queue.dequeue();
sensor_msgs::LaserScan::ConstPtr currnet_scan = m_laser_queue.dequeue();
while( currnet_scan->header.stamp < last_odom->header.stamp && ros::ok() )
{
currnet_scan = m_laser_queue.dequeue();
}
sensor_msgs::LaserScan::ConstPtr last_scan = currnet_scan;
while(ros::ok())
{
while( current_odom->header.stamp < currnet_scan->header.stamp )
{
last_odom = current_odom;
current_odom = m_odom_queue.dequeue();
}
std::lock_guard<std::mutex> lock(m_last_pose_mutex);
bool do_mapping = true;
ros::param::getCached("/homer_mapping/do_mapping", do_mapping);
......@@ -172,34 +124,36 @@ void SlamNode::workerThread()
{
m_scan_map.insertScan(currnet_scan, m_last_pose);
}
tf::poseMsgToTF(last_odom->pose.pose, m_last_used_odom_pose);
m_last_pose_time = currnet_scan->header.stamp;
{
std::lock_guard<std::mutex> lock(m_last_pose_mutex);
m_last_pose_time = currnet_scan->header.stamp;
m_movement_model.insertPose(m_last_pose, m_last_pose_time);
m_movement_model.insertPose(m_last_pose, m_last_pose_time);
m_movement_model.insertPose(m_last_pose, m_last_pose_time);
}
}
else
{
tf::Pose last_interpolated_pose = getInterpolatedPose(last_odom, current_odom, currnet_scan->header.stamp);
tf::Transform trans = m_last_used_odom_pose.inverseTimes(last_interpolated_pose);
m_last_used_odom_pose = last_interpolated_pose;
ros::Duration time_diff = currnet_scan->header.stamp - m_last_pose_time;
double max_acceleration_xy = 5;
ros::param::getCached("/homer_mapping/scan_map/max_acceleration_xy", max_acceleration_xy);
double max_acceleration_theta = 20;
ros::param::getCached("/homer_mapping/scan_map/max_acceleration_theta", max_acceleration_theta);
double max_error_xy = max_acceleration_xy*time_diff.toSec()*time_diff.toSec()/2;
double max_error_theta = max_acceleration_theta*time_diff.toSec()*time_diff.toSec()/2;
//ROS_INFO_STREAM("time_diff: " << time_diff.toSec());
//ROS_INFO_STREAM("max_error xy: " << max_error_xy << " theta: " << max_error_theta);
tf::Pose current_pose = m_scan_map.localizeScan(currnet_scan, m_last_pose * trans, max_error_xy, max_error_xy, max_error_theta);
MovementModelGuess guess = m_movement_model.getNextPose(currnet_scan->header.stamp);
ROS_INFO_STREAM("model guess: x: " << guess.pose.getOrigin().x() << " y: " << guess.pose.getOrigin().y() << " theta: " << tf::getYaw(guess.pose.getRotation()));
ROS_INFO_STREAM("error x: " << guess.error[0] << " y: " << guess.error[1] << " theta: " << guess.error[2]);
tf::Transform odom = matchScans(last_scan, currnet_scan, guess.pose, guess.error[0], guess.error[1], guess.error[2]);
tf::Pose pose_hint = m_last_pose * odom;
ROS_INFO_STREAM("hint: x: " << pose_hint.getOrigin().x() << " y: " << pose_hint.getOrigin().y() << " theta: " << tf::getYaw(pose_hint.getRotation()));
tf::Pose current_pose = m_scan_map.localizeScan(currnet_scan, pose_hint, guess.error[0], guess.error[1], guess.error[2]);
//tf::Pose current_pose = pose_hint;
ROS_INFO_STREAM("pose: x: " << current_pose.getOrigin().x() << " y: " << current_pose.getOrigin().y() << " theta: " << tf::getYaw(current_pose.getRotation()));
tf::Transform diff = m_last_pose.inverseTimes(current_pose);
ros::Duration time_diff = currnet_scan->header.stamp - m_last_pose_time;
const double delta_theta = tf::getYaw(diff.getRotation());
const double speed_theta = delta_theta/time_diff.toSec();
m_last_pose = current_pose;
m_last_pose_time = currnet_scan->header.stamp;
{
std::lock_guard<std::mutex> lock(m_last_pose_mutex);
m_last_pose = current_pose;
m_last_pose_time = currnet_scan->header.stamp;
m_movement_model.insertPose(current_pose, currnet_scan->header.stamp);
}
double max_rotation_per_second = 0.1;
ros::param::getCached("/homer_mapping/max_rotation_per_second", max_rotation_per_second);
......@@ -220,6 +174,7 @@ void SlamNode::workerThread()
m_last_map_send_time = currnet_scan->header.stamp;
}
last_scan = currnet_scan;
currnet_scan = m_laser_queue.dequeue();
if( m_laser_queue.size() > 10 )
{
......
Markdown is supported
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