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

working with localisation

parent d136c931
......@@ -39,31 +39,32 @@
<param name="gt_topic" value="/$(arg robot_prefix)/kmo_navserver/state" />
<!-- Choose weather to initiate pose to pose_init_<x,y,t> or the data of /<gt_topic> -->
<param name="initPoseFromGT" value="true" />
<param name="initPoseFromGT" value="false" />
<param name="pose_init_x" value="0" />
<param name="pose_init_y" value="0" />
<param name="pose_init_t" value="0" />
<!-- world frame id -->
<param name="root_tf" value="map_velodyne" />
<param name="root_tf" value="world" />
<!-- odometry frame id-->
<param name="odom_tf" value="$(arg robot_prefix)/odom" />
<param name="base_tf" value="$(arg robot_prefix)/base_link" />
<param name="base_tf" value="$(arg robot_prefix)/base_footprint" />
<!--param name="mcl_tf" value="mcl_pose_est" /-->
<!-- MCL parameters -->
<param name="enable_localisation" value="true" />
<param name="resolution_local_factor" value="1.3" />
<param name="particle_count" value="500" />
<param name="show_pose" value="true"/>
<param name="fraction" value="1.0"/>
<param name="force_SIR" value="true" />
<param name="z_filter_height" value="1.0" />
</node>
<node name="occupancy_map_server" pkg="map_server" type="map_server" args=" $(find graph_map)/maps/occupancy_map.yaml" output="screen"/>
<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" />
......@@ -80,5 +81,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" />
<node pkg="tf" type="static_transform_publisher" name="occupancy_map_static_broadcaster" args="0 0 0 0 0 0 world map 100" />
</launch>
......@@ -11,10 +11,11 @@ Panels:
- /Odometry1/Shape1
- /Odometry2/Shape1
- /Odometry3/Shape1
- /PoseArray1
- /Odometry4/Shape1
- /PointCloud22
- /PointCloud22/Autocompute Value Bounds1
- /NDT1
- /Map1
Splitter Ratio: 0.5
Tree Height: 815
- Class: rviz/Selection
......@@ -36,7 +37,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
......@@ -63,49 +64,9 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
laser_link:
map:
Value: true
mcl_robot_est:
Value: true
odom_base_link:
Value: true
robot4/base_footprint:
Value: true
robot4/base_fork:
Value: true
robot4/base_link:
Value: true
robot4/fixed_left_wheel_link:
Value: true
robot4/fixed_right_wheel_link:
Value: true
robot4/kinect2_depth_optical_frame:
Value: true
robot4/kinect2_ir_optical_frame:
Value: true
robot4/kinect2_link:
Value: true
robot4/kinect2_rgb_optical_frame:
Value: true
robot4/laser2d_floor:
Value: true
robot4/laser2d_floor_link:
Value: true
robot4/laser2d_top:
Value: true
robot4/laser2d_top_link:
Value: true
robot4/left_fork:
Value: true
robot4/right_fork:
Value: true
robot4/robot4/velodyne:
Value: true
robot4/robot4/velodyne_base_link:
Value: true
robot4/sd_wheel_link:
Value: true
robot4/steer_link:
map_velodyne:
Value: true
submap_node0:
Value: true
......@@ -118,43 +79,10 @@ Visualization Manager:
Show Names: true
Tree:
world:
mcl_robot_est:
laser_link:
{}
odom_base_link:
map:
{}
map_velodyne:
{}
robot4/base_link:
robot4/base_footprint:
{}
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/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:
{}
submap_node0:
{}
Update Interval: 0
......@@ -382,12 +310,22 @@ Visualization Manager:
- Alpha: 0.400000006
Class: ndt_rviz/NDT
Color: 204; 51; 204
Enabled: false
Enabled: true
History Length: 1
Name: NDT
Topic: /graph_map_publisher/NDTOmMap
Unreliable: false
Value: false
Value: true
- Alpha: 0.699999988
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
Enabled: true
Global Options:
Background Color: 130; 130; 130
......@@ -413,25 +351,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
Distance: 5.93597698
Distance: 45.4304314
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -12.0478163
Y: 10.5119715
Z: 0.94999975
X: -5.52330637
Y: -10.7744989
Z: 0
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.704796195
Target Frame: <Fixed Frame>
Pitch: 1.14520371
Target Frame: world
Value: XYOrbit (rviz)
Yaw: 1.87858832
Yaw: 5.13040686
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
......
......@@ -172,9 +172,10 @@ class localisation_node {
sleep(1);
initialized=true;
}
void VeloCallback(const velodyne_msgs::VelodyneScan::ConstPtr& msg){
cout<<"velodyne callback"<<endl;
void VeloCallback(const velodyne_msgs::VelodyneScan::ConstPtr &msg){
pcl::PointCloud<pcl::PointXYZ> cloud;
ros::Time t0=ros::Time::now();
ros::Time t_cloud_end=msg->packets[msg->packets.size()-1].stamp;
for(size_t next = 0; next < msg->packets.size(); ++next){
velodyne_rawdata::VPointCloud pnts;
dataParser.unpack(msg->packets[next], pnts);
......@@ -187,7 +188,8 @@ class localisation_node {
}
pnts.clear();
}
this->processFrame(cloud,msg->header.stamp);
this->processFrame(cloud,t_cloud_end);
cout<<"Tdiff="<<ros::Time::now()-t0<<endl;
}
void PCCallback(const sensor_msgs::PointCloud2::ConstPtr& msg){
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
......@@ -216,7 +218,7 @@ class localisation_node {
this->processFrame(pcl_cloud,msg->header.stamp);
}
void processFrame(pcl::PointCloud<pcl::PointXYZ> &cloud, ros::Time ts){
void processFrame(pcl::PointCloud<pcl::PointXYZ> &cloud, const ros::Time &ts){
static tf::TransformListener tf_listener;
pcl::PointCloud<pcl::PointXYZ> cloud_localized;
......@@ -225,8 +227,8 @@ class localisation_node {
return;
else if(!initialized && initial_pose_set){
geometry_msgs::Pose pose_init_geom;
tf::poseEigenToMsg(initial_pose,pose_init_geom);
Initialize(pose_init_geom,ts);
tf::poseEigenToMsg(initial_pose, pose_init_geom);
Initialize(pose_init_geom, ts);
}
......@@ -247,7 +249,7 @@ class localisation_node {
Eigen::Affine3d Todom;
tf::poseTFToEigen(transform,Todom);
tf::poseTFToEigen(transform, Todom);
//Eigen::Affine3d T = getAsAffine(x, y, yaw);
if(firstLoad){
tOld = Todom;
......@@ -275,9 +277,9 @@ class localisation_node {
tf::poseEigenToTF(Tcorr, Tcorr_tf);
trans_pub.sendTransform(tf::StampedTransform( Tcorr_tf, ts, rootTF, odomTF) );
tf::poseEigenToTF(Tsens, sensor_tf);
trans_pub.sendTransform( tf::StampedTransform( sensor_tf, ts, odomTF, laser_link_id) );
trans_pub.sendTransform( tf::StampedTransform( sensor_tf, ts, baseTF, laser_link_id) );
cloud_localized.header.frame_id=laser_link_id;
pcl_conversions::toPCL(ts,cloud_localized.header.stamp);
pcl_conversions::toPCL(ts, cloud_localized.header.stamp);
cloud_pub.publish(cloud_localized);
}
cloud.clear();
......
......@@ -159,7 +159,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(TARGETS graph_map_fuser_node graph_map_publisher graph_mapping_offline show_map optimize_graph graph_map_server ${PROJECT_NAME}
install(TARGETS graph_map_fuser_node graph_mapping_offline show_map optimize_graph graph_map_publisher ${PROJECT_NAME} # graph_map_server
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
......
......@@ -9,6 +9,7 @@
</node-->
<!--node pkg="rviz" type="rviz" name="rviz" args="-d $(find graph_map)/rviz/showMap.rviz" /-->
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 world map_velodyne 100" />
</launch>
......@@ -42,6 +42,7 @@ void MapType::updateMap(const Eigen::Affine3d &Tnow,pcl::PointCloud<pcl::PointXY
observations_++;
if(store_points_)
input_cloud_.push_back(cloud);
//const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> ptr=boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(&cloud);
//octree.setInputCloud(ptr);
//octree.addPointsFromInputCloud();
......
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