Skip to content

GitLab

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

fixing initialization

parent 0a8d56d9
Hide whitespace changes
Inline Side-by-side
Showing with 46 additions and 64 deletions
graph_localization/launch/3d_mcl_ncfm2018.launch
View file @ c3e159a0
......@@ -7,7 +7,8 @@
<arg name="map_file_path" default="$(find graph_map)/maps/ndt_map.MAP" />
<arg name="robot_prefix" default="robot5" />
<arg name="rviz_enabled" default="false" />
<arg name="use_sim_time" value="true"/>
<arg name="root_id" default="/world" />
<!-- Include visualization in rviz -->
<group if="$(arg rviz_enabled)">
<include file="$(find graph_map)/launch/visualize_graph_fuser.launch" >
......@@ -45,7 +46,7 @@
<param name="pose_init_t" value="0" />
<!-- world frame id -->
<param name="root_tf" value="world" />
<param name="root_tf" value="$(arg root_id)" />
<!-- odometry frame id-->
<param name="odom_tf" value="$(arg robot_prefix)/odom" />
<param name="base_tf" value="$(arg robot_prefix)/base_footprint" />
......@@ -64,12 +65,13 @@
<node name="occupancy_map_server" pkg="map_server" type="map_server" args=" $(find graph_map)/maps/occupancy_map.yaml" output="screen"/> <!--frame_id map_velodyne-->
<group if="$(arg rviz_enabled)">
<node name="graph_map_publisher" pkg="graph_map" type="graph_map_publisher" output="screen">
<param name="map_rate" value="1" />
<param name="map_file" value="$(arg map_file_path)" />
<param name="map_frame" value="/maps/map_laser2d" />
<param name="map_parent_frame_id" value="/world" />
<param name="map_parent_frame_id" value="$(arg root_id)" />
<param name="map_topic" value="/maps/map_laser2d" />
</node>
</group>
......@@ -79,7 +81,7 @@
<arg name="file_1" default="2018-05-18-09-55-16.bag" />
<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"/>
<node pkg="tf" type="static_transform_publisher" name="velodyne_map_static_broadcaster" args="0 0 0 0 0 0 world map_laser2d 100"/>
<node pkg="tf" type="static_transform_publisher" name="occupancy_map_static_broadcaster" args="0 0 0 0 0 0 world map 100"/>
</launch>
graph_localization/rviz/localization_oru_lab.rviz
View file @ c3e159a0
......@@ -11,9 +11,11 @@ Panels:
- /Odometry1/Shape1
- /Odometry2/Shape1
- /Odometry3/Shape1
- /PoseArray1
- /Odometry4/Shape1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
- /NDT1
- /Map1
Splitter Ratio: 0.5
Tree Height: 815
......@@ -36,7 +38,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
......@@ -65,6 +67,8 @@ Visualization Manager:
All Enabled: false
map:
Value: true
map_laser2d:
Value: true
robot4/base_footprint:
Value: true
robot4/base_fork:
......@@ -121,8 +125,6 @@ Visualization Manager:
Value: true
submap_node0:
Value: true
velodyne:
Value: true
world:
Value: true
Marker Scale: 1
......@@ -132,42 +134,10 @@ Visualization Manager:
Show Names: true
Tree:
world:
robot4/odom:
robot4/base_footprint:
robot4/base_link:
robot4/base_fork:
robot4/left_fork:
{}
robot4/right_fork:
{}
robot4/fixed_left_wheel_link:
{}
robot4/fixed_right_wheel_link:
{}
robot4/kinect2_link:
robot4/kinect2_depth_optical_frame:
{}
robot4/kinect2_rgb_optical_frame:
robot4/kinect2_ir_optical_frame:
{}
robot4/kinect_link:
{}
robot4/laser2d_floor:
{}
robot4/laser2d_floor_link:
{}
robot4/laser2d_top:
{}
robot4/laser2d_top_link:
{}
robot4/robot4/velodyne_base_link:
robot4/robot4/velodyne:
{}
robot4/steer_link:
robot4/sd_wheel_link:
{}
velodyne:
{}
map:
{}
map_laser2d:
{}
submap_node0:
{}
Update Interval: 0
......@@ -406,25 +376,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
Distance: 22.5021648
Distance: 66.5617294
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.926601827
Y: -2.5325017
Z: 3.01108912e-06
X: 7.46893883
Y: -6.24155045
Z: 1.15941584e-05
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.06479573
Pitch: 1.25979626
Target Frame: world
Value: XYOrbit (rviz)
Yaw: 0.494042486
Yaw: 6.12041521
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
......@@ -452,7 +422,7 @@ Window Geometry:
Height: 1028
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001aa000003befc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003be000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003befc0200000003fb0000000a005600690065007700730100000028000003be000000ad00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000064f000000b0fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000047a000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000002a7000003befc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003be000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003befc0200000003fb0000000a005600690065007700730100000028000003be000000ad00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000064f000000b0fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006500000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000037d000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
......
graph_localization/src/ndt_mcl_localization.cpp
View file @ c3e159a0
......@@ -167,9 +167,8 @@ class localisation_node {
tf::poseMsgToEigen(pose_init,pose_init_eig);
cout<<"Initial position set to"<<pose_init_eig.translation().transpose()<<endl;
Vector6d var;
var<<0.5, 0.5, 0.0, 0.0, 0.0, 0.2;
var<<1.0, 1.0, 0.0, 0.0, 0.0, 0.5;
localisation_type_ptr_->InitializeLocalization(pose_init_eig,var);
sleep(1);
initialized=true;
}
void InitializeUniform(const geometry_msgs::Pose &pose_init,const ros::Time &t_init){
......
graph_map/include/graph_map/visualization/graph_visualization.h
View file @ c3e159a0
......@@ -44,6 +44,8 @@ public:
void PlotActiveNodesCloud();
void SetParentFrameId(const std::string &parent_frame_id){parent_frame_id_=parent_frame_id;}
void PlotAllClouds();
void PlotLinks(ros::Time t=ros::Time::now());
......@@ -72,6 +74,7 @@ void PlotGraphThread();
std::string tf_prefix_="/tf";
ros::NodeHandle nh_;
GraphMapNavigatorPtr graph_map_;
std::string parent_frame_id_="/world";
bool visualization_enabled_=false;
bool visualize_map_=false;
......
graph_map/src/graph_map_fuser_node.cpp
View file @ c3e159a0
......@@ -356,12 +356,12 @@ public:
pcl::PointCloud<pcl::PointXYZ> registered_cloud=cloud;
ros::Time ts;
pcl_conversions::fromPCL(cloud.header.stamp,ts);
bool uppdated=fuser_->ProcessFrame<pcl::PointXYZ>(cloud,pose_,Tmotion);
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, ts, world_link_id, fuser_base_link_id));
tf_.sendTransform(tf::StampedTransform(tf_sensor_pose_, ts, fuser_base_link_id, laser_link_id));
//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);
......@@ -421,14 +421,16 @@ public:
return false;
}
bool GetTransformFromTF(const ros::Time &time, tf::StampedTransform& transform) {
bool GetTransformFromTF(const ros::Time &time, tf::StampedTransform &transform) {
static tf::TransformListener tf_listener;
tf_listener.waitForTransform(odometry_link_id, base_footprint_id, time, ros::Duration(0.1));
try{
tf_listener.lookupTransform(odometry_link_id, base_footprint_id, time, transform);
}
catch(tf::TransformException ex){
ROS_ERROR("%s", ex.what());
//ROS_ERROR("%s", ex.what());
cerr<<"No transformation between "<<odometry_link_id<<", and "<<base_footprint_id<<" at time"<<time.toSec()<<endl;
return false;
}
return true;
......@@ -504,13 +506,17 @@ public:
void points2OdomCallbackTF(const velodyne_msgs::VelodyneScan::ConstPtr& msg_in){//this callback is used to look up tf transformation for scan data
pcl::PointCloud<pcl::PointXYZ> cloud;
ros::Time t_cloud_end = msg_in->packets[msg_in->packets.size()-1].stamp;
ros::Time t_cloud_end = msg_in->header.stamp;//msg_in->packets[msg_in->packets.size()-1].stamp;
cerr<<"Time: "<<t_cloud_end<<endl;
ndt_generic::UnwarpCloudSimple<pcl::PointXYZ>(velodyne_parser, msg_in, cloud);
Eigen::Affine3d Tm, Todo_eig;
GetTransformFromTF(t_cloud_end, T_odom);
if(!GetTransformFromTF(t_cloud_end, T_odom))
return;
tf::poseTFToEigen(T_odom, Todo_eig);
cerr<<"FOUND TRANSFORMATION between odometry_link_id= "<<odometry_link_id<<", base_footprint_id="<<base_footprint_id<<" , trans="<<Todo_eig.translation().transpose()<<" at time t="<<time<<endl;
Tm = GetOdomdiff(Todo_eig);
cout<<"use listener, Tm="<<Tm.translation().transpose()<<endl;
cout<<"Tm="<<Tm.translation().transpose()<<endl;
pcl_conversions::toPCL(t_cloud_end,cloud.header.stamp);
this->processFrame(cloud, Tm);
}
......
graph_map/src/graph_map_publisher.cpp
View file @ c3e159a0
......@@ -47,6 +47,8 @@ int main(int argc, char** argv){
LoadGraphMap(mapFile,graph_map_);
graph_map::MapTypePtr map_type = graph_map_->GetCurrentNode()->GetMap();
graphVisualization vis(graph_map_,true);
vis.SetParentFrameId(mapParentFrameId);
NDTMapPtr ptr=boost::dynamic_pointer_cast<NDTMapType>(map_type);
perception_oru::NDTMap *ndt_ptr = ptr->GetNDTMap();
tf::TransformBroadcaster br;
......
graph_map/src/visualization/graph_visualization.cpp
View file @ c3e159a0
......@@ -92,7 +92,7 @@ void graphVisualization::PlotOccupancyMap(const ros::Time t){
void graphVisualization::PlotLinks(ros::Time t){
std::string prev_frame_id="world";
std::string prev_frame_id=parent_frame_id_;
for(mapNodeItr itr=graph_map_->begin(); itr!=graph_map_->end();itr++){
tf::Transform node_tf;
Eigen::Affine3d diff;
......@@ -141,7 +141,7 @@ void graphVisualization::PlotCurrentLinkInfo(ros::Time t){
void graphVisualization::PlotAllClouds(){
pcl::PointCloud<pcl::PointXYZ> cloud;
graph_map_->GetCloud(cloud,true);
cloud.header.frame_id="/world";
cloud.header.frame_id=parent_frame_id_;
pcl_conversions::toPCL(ros::Time::now(), cloud.header.stamp);
cloud_world_pub.publish(cloud);
}
......@@ -167,7 +167,7 @@ void graphVisualization::PlotTrajectory(ros::Time t){
geometry_msgs::PoseArray pose_arr;
geometry_msgs::Pose pose;
pose_arr.header.frame_id="/world";
pose_arr.header.frame_id=parent_frame_id_;
pose_arr.header.stamp=t;
for(mapNodeItr itr=graph_map_->begin(); itr!=graph_map_->end();itr++){
ndt_generic::Affine3dSTLVek vek= (*itr)->GetMap()->GetObservationVector();
......
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