Commit 70ec971e authored by dla.adolfsson@gmail.com's avatar dla.adolfsson@gmail.com
Browse files

localization is ready, working on tf for mapper

parent 6b5810a8
......@@ -251,6 +251,7 @@ void MCLNDTType::InitializeLocalization(const Eigen::Affine3d &pose,const Vector
initialized_=true;
}
MCLNDTParam::MCLNDTParam(){
/*motion_model.push_back(0.05);
......
......@@ -172,6 +172,10 @@ class localisation_node {
sleep(1);
initialized=true;
}
void InitializeUniform(const geometry_msgs::Pose &pose_init,const ros::Time &t_init){
initialized=true;
}
void VeloCallback(const velodyne_msgs::VelodyneScan::ConstPtr &msg){
pcl::PointCloud<pcl::PointXYZ> cloud;
ros::Time t0=ros::Time::now();
......
......@@ -67,6 +67,10 @@
<param name="use_tf_listener" value="false" />
<param name="gt_topic" value="$(arg namespace_prefix)/control/state" />
<!-- odometry frame id-->
<param name="odom_tf" value="$(arg namespace_prefix)/odom" />
<param name="base_tf" value="$(arg namespace_prefix)/base_footprint" />
<!-- check if the registration pose output is similar to odometry prediction within max_translation_norm or max_rotation_norm -->
<param name="check_consistency" value="true" />
<param name="max_translation_norm" value="0.2" />
......@@ -98,7 +102,7 @@
<param name="map_directory" value="$(find graph_map)/maps" />
<param name="visualize" value="true" />
<param name="disable_map_visualization" value="true" />
<param name="disable_map_visualization" value="false" />
<param name="T_map" value="3.0" />
<!-- a minimum movement is required before fusing frames -->
......@@ -125,5 +129,6 @@
<node pkg="rosbag" type="play" name="player" output="screen" args="--clock -r 1.0 -q $(arg path)$(arg file_1)"/>
</group>
<node pkg="tf" type="static_transform_publisher" name="velodyne_map_static_broadcaster" args="0 0 0 0 0 0 world map_velodyne 100" />
</launch>
......@@ -20,6 +20,8 @@
#include "message_filters/subscriber.h"
#include "tf/message_filter.h"
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <boost/circular_buffer.hpp>
#include <laser_geometry/laser_geometry.h>
......@@ -46,6 +48,7 @@
#include <velodyne_pointcloud/rawdata.h>
#include "ndt_generic/pcl_utils.h"
#include "graph_map/visualization/graph_visualization.h"
#include <pcl_ros/transforms.h>
#ifndef SYNC_FRAMES
#define SYNC_FRAMES 20
......@@ -86,6 +89,7 @@ protected:
tf::TransformListener tf_listener_;
ros::Publisher output_pub_;
Eigen::Affine3d pose_, T, sensorPose_;
tf::StampedTransform T_odom;
unsigned int frame_nr_;
......@@ -95,7 +99,7 @@ protected:
std::string map_name="graph_map";
std::string points_topic="", map_dir, odometry_topic,odometry_adjusted_topic;
std::string file_format_map=".JFF";
std::string world_link_id, odometry_link_id, fuser_base_link_id,laser_link_id, init_pose_frame, gt_topic, bag_name,state_base_link_id;
std::string world_link_id, fuser_base_link_id,laser_link_id, init_pose_frame, gt_topic, bag_name,state_base_link_id;
double size_x, size_y, size_z, resolution, max_range, min_range;
bool visualize, match2D, matchLaser, beHMT, useOdometry,
initPoseFromGT, initPoseFromTF, initPoseSet, gt_mapping;
......@@ -127,6 +131,8 @@ protected:
perception_oru::MotionModel2d::Params motion_params;
boost::mutex m;
GraphMapNavigatorPtr graph_map;
std::string odometry_link_id, base_footprint_id;
public:
// Constructor
GraphMapFuserNode(ros::NodeHandle param_nh) : frame_nr_(0)
......@@ -235,7 +241,9 @@ public:
param_nh.param("useOdometry",useOdometry,true);
param_nh.param<bool>("use_tf_listener", use_tf_listener_, false);
param_nh.param<std::string>("odometry_frame_id", odometry_link_id, std::string("/odom_base_link"));
param_nh.param<std::string>("odom_tf", odometry_link_id, std::string("/odom_base_link"));
param_nh.param<std::string>("base_tf", base_footprint_id, std::string("/base_footprint"));
initPoseSet = false;
fuser_odom.header.frame_id="/world";
......@@ -347,16 +355,23 @@ public:
}
pcl::PointCloud<pcl::PointXYZ> registered_cloud=cloud;
ros::Time tplot=ros::Time::now();
ros::Time ts;
pcl_conversions::fromPCL(cloud.header.stamp,ts);
bool uppdated=fuser_->ProcessFrame<pcl::PointXYZ>(cloud,pose_,Tmotion);
tf::Transform Transform;
tf::transformEigenToTF(pose_,Transform);
tf_.sendTransform(tf::StampedTransform(Transform, tplot, world_link_id, fuser_base_link_id));
tf_.sendTransform(tf::StampedTransform(tf_sensor_pose_, tplot, fuser_base_link_id, laser_link_id));
fuser_odom.header.stamp=tplot;
tf::poseEigenToMsg( pose_,fuser_odom.pose.pose);
tf::transformEigenToTF(pose_, Transform);
tf_.sendTransform(tf::StampedTransform(Transform, ts, world_link_id, fuser_base_link_id));
tf_.sendTransform(tf::StampedTransform(tf_sensor_pose_, ts, fuser_base_link_id, laser_link_id));
if(use_tf_listener_){
tf::Transform tf_pose;
tf::poseEigenToTF(pose_,tf_pose);
tf::Transform Tcorr_tf = tf_pose*T_odom.inverse();
tf_.sendTransform(tf::StampedTransform(Tcorr_tf, ts, world_link_id, odometry_link_id));
}
fuser_odom.header.stamp = ts;
tf::poseEigenToMsg( pose_, fuser_odom.pose.pose);
fuser_odom_publisher_.publish(fuser_odom);
plotPointcloud2(registered_cloud, tplot);
plotPointcloud2(registered_cloud, ts);
frame_nr_++;
}
......@@ -398,9 +413,7 @@ public:
if(save_occupancy){
std::string save_command = (std::string("rosrun map_server map_saver map:=")+occupancy_topic+std::string(" -f \"$(rospack find graph_map)/maps/occupancy_map\"")) ;
int status= system(save_command.c_str());
}
return true;
}
else
......@@ -408,16 +421,14 @@ public:
return false;
}
inline bool getAffine3dTransformFromTF(const ros::Time &time,const std::string &link_id,Eigen::Affine3d& ret,const ros::Duration &wait) {
bool GetTransformFromTF(const ros::Time &time, tf::StampedTransform& transform) {
static tf::TransformListener tf_listener;
tf::StampedTransform transform;
tf_listener_.waitForTransform(world_link_id, link_id, time ,wait);
tf_listener.waitForTransform(odometry_link_id, base_footprint_id, time, ros::Duration(0.1));
try{
tf_listener_.lookupTransform(world_link_id, link_id, time, transform);
tf::poseTFToEigen(transform, ret);
tf_listener.lookupTransform(odometry_link_id, base_footprint_id, time, transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
catch(tf::TransformException ex){
ROS_ERROR("%s", ex.what());
return false;
}
return true;
......@@ -450,7 +461,7 @@ public:
{
sensor_msgs::PointCloud2 cloud;
pcl::PointCloud<pcl::PointXYZ> pcl_cloud, pcl_cloud_unfiltered;
Eigen::Affine3d Tm=GetOdomdiff(odo_in->pose.pose);
Eigen::Affine3d Tm=GetOdomdiff(odo_in->pose.pose);
projector_.projectLaser(*msg_in, cloud);
pcl::fromROSMsg (cloud, pcl_cloud_unfiltered);
......@@ -479,34 +490,29 @@ public:
this->processFrame(cloud,Tm);
}
Eigen::Affine3d GetOdomdiff(const geometry_msgs::Pose odom_msg){
Eigen::Affine3d this_odom;
tf::poseMsgToEigen(odom_msg,this_odom);
static Eigen::Affine3d last_odom=this_odom;
Eigen::Affine3d Tm = last_odom.inverse()*this_odom;
last_odom = this_odom;
Eigen::Affine3d GetOdomdiff(const geometry_msgs::Pose &odom_msg){
Eigen::Affine3d T;
tf::poseMsgToEigen(odom_msg, T);
return GetOdomdiff(T);
}
Eigen::Affine3d GetOdomdiff(const Eigen::Affine3d &T){
static Eigen::Affine3d last_odom=T;
Eigen::Affine3d Tm = last_odom.inverse()*T;
last_odom = T;
return Tm;
}
void points2OdomCallbackTF(const velodyne_msgs::VelodyneScan::ConstPtr& msg_in){//this callback is used to look up tf transformation for scan data
Eigen::Affine3d Tm;
static bool last_odom_found=false;
static Eigen::Affine3d last_odom;
Eigen::Affine3d this_odom;
pcl::PointCloud<pcl::PointXYZ> cloud;
ndt_generic::UnwarpCloudSimple<pcl::PointXYZ>(velodyne_parser,msg_in,cloud);
bool found_odom= getAffine3dTransformFromTF((msg_in->header.stamp-ros::Duration(sensor_offset_t_)),odometry_link_id,this_odom,ros::Duration(0.1));
if (frame_nr_ =0 || !found_odom||!last_odom_found)
Tm.setIdentity();
else {
Tm = last_odom.inverse()*this_odom;
}
last_odom_found=found_odom;
last_odom = this_odom;
this->processFrame(cloud,Tm);
ros::Time t_cloud_end = msg_in->packets[msg_in->packets.size()-1].stamp;
ndt_generic::UnwarpCloudSimple<pcl::PointXYZ>(velodyne_parser, msg_in, cloud);
Eigen::Affine3d Tm, Todo_eig;
GetTransformFromTF(t_cloud_end, T_odom);
tf::poseTFToEigen(T_odom, Todo_eig);
Tm = GetOdomdiff(Todo_eig);
cout<<"use listener, Tm="<<Tm.translation().transpose()<<endl;
this->processFrame(cloud, Tm);
}
void VelodyneCallback(const velodyne_msgs::VelodyneScan::ConstPtr& msg_in){//this callback is used to look up tf transformation for scan data
pcl::PointCloud<pcl::PointXYZ> cloud;
......@@ -570,6 +576,8 @@ public:
}
public:
private:
// map publishing function
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
......
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