Skip to content

GitLab

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

added parameter for root link rather than /world only

parents c3e159a0 e8a5f401
Hide whitespace changes
Inline Side-by-side
Showing with 107 additions and 41 deletions
graph_localization/launch/3d_mcl_ncfm2018.launch
View file @ 446c08af
......@@ -3,11 +3,12 @@
<!-- Start rosbag with launchfile-->
<arg name="run_bag" default="false" />
<arg name="publish_map" default="false" />
<!-- Absolute Path to map to be used for localization. The map needs to be built by graph_fuser package -->
<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="root_id" default="/world" />
<arg name="root_id" default="/map_laser2d" />
<!-- Include visualization in rviz -->
<group if="$(arg rviz_enabled)">
......@@ -46,6 +47,8 @@
<param name="pose_init_t" value="0" />
<!-- world frame id -->
<param name="root_tf" value="$(arg root_id)" />
<!-- odometry frame id-->
<param name="odom_tf" value="$(arg robot_prefix)/odom" />
......@@ -63,8 +66,11 @@
<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"/> <!--frame_id map_velodyne-->
<node name="occupancy_map_server" pkg="map_server" type="map_server" args=" $(find graph_map)/maps/ncfm.yaml" output="screen" if="$(arg publish_map)">
<remap from="/map" to="/maps/map_laser2d"/>
<param name="~frame_id" value="map_laser2d"/>
</node>
<group if="$(arg rviz_enabled)">
<node name="graph_map_publisher" pkg="graph_map" type="graph_map_publisher" output="screen">
......@@ -81,7 +87,11 @@
<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_laser2d 100"/>
<node pkg="tf" type="static_transform_publisher" name="occupancy_map_static_broadcaster" args="0 0 0 0 0 0 world map 100"/>
<!-- 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>
graph_localization/rviz/localization_oru_lab.rviz
View file @ 446c08af
......@@ -65,66 +65,76 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
map:
asus_fork_depth_frame:
Value: true
asus_fork_depth_optical_frame:
Value: true
asus_fork_link:
Value: true
asus_fork_rgb_frame:
Value: true
asus_fork_rgb_optical_frame:
Value: true
map_laser2d:
Value: true
robot4/base_footprint:
robot5/asus_fork_base_link:
Value: true
robot4/base_fork:
robot5/asus_fork_depth_optical_frame:
Value: true
robot4/base_link:
robot5/asus_fork_link:
Value: true
robot4/base_link_ground_truth:
robot5/asus_fork_rgb_optical_frame:
Value: true
robot4/camera_depth_frame:
robot5/base_footprint:
Value: true
robot4/camera_depth_optical_frame:
robot5/base_footprint_ground_truth:
Value: true
robot4/camera_link:
robot5/base_fork:
Value: true
robot4/camera_rgb_frame:
robot5/base_link:
Value: true
robot4/camera_rgb_optical_frame:
robot5/fixed_left_wheel_link:
Value: true
robot4/fixed_left_wheel_link:
robot5/fixed_right_wheel_link:
Value: true
robot4/fixed_right_wheel_link:
robot5/kinect2_base_link:
Value: true
robot4/kinect2_depth_optical_frame:
robot5/kinect2_depth_optical_frame:
Value: true
robot4/kinect2_ir_optical_frame:
robot5/kinect2_ir_optical_frame:
Value: true
robot4/kinect2_link:
robot5/kinect2_link:
Value: true
robot4/kinect2_rgb_optical_frame:
robot5/kinect2_rgb_optical_frame:
Value: true
robot4/kinect_link:
robot5/laser2d_floor_base_link:
Value: true
robot4/laser2d_floor:
robot5/laser2d_floor_link:
Value: true
robot4/laser2d_floor_link:
robot5/laser2d_top_base_link:
Value: true
robot4/laser2d_top:
robot5/laser2d_top_link:
Value: true
robot4/laser2d_top_link:
robot5/left_fork:
Value: true
robot4/left_fork:
robot5/odom:
Value: true
robot4/odom:
robot5/right_fork:
Value: true
robot4/right_fork:
robot5/sd_wheel_link:
Value: true
robot4/robot4/velodyne:
robot5/steer_link:
Value: true
robot4/robot4/velodyne_base_link:
robot5/velodyne:
Value: true
robot4/sd_wheel_link:
robot5/velodyne_base_link:
Value: true
robot4/steer_link:
robot5/velodyne_link:
Value: true
submap_node0:
Value: true
velodyne:
Value: true
world:
Value: true
Marker Scale: 1
......@@ -133,13 +143,52 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
world:
map:
{}
map_laser2d:
{}
map_laser2d:
robot5/odom:
robot5/base_footprint:
robot5/base_link:
robot5/asus_fork_base_link:
robot5/asus_fork_link:
robot5/asus_fork_depth_optical_frame:
{}
robot5/asus_fork_rgb_optical_frame:
{}
robot5/base_fork:
robot5/left_fork:
{}
robot5/right_fork:
{}
robot5/fixed_left_wheel_link:
{}
robot5/fixed_right_wheel_link:
{}
robot5/kinect2_base_link:
robot5/kinect2_link:
robot5/kinect2_depth_optical_frame:
{}
robot5/kinect2_rgb_optical_frame:
robot5/kinect2_ir_optical_frame:
{}
robot5/laser2d_floor_base_link:
robot5/laser2d_floor_link:
{}
robot5/laser2d_top_base_link:
robot5/laser2d_top_link:
{}
robot5/steer_link:
robot5/sd_wheel_link:
{}
robot5/velodyne_base_link:
robot5/velodyne_link:
robot5/velodyne:
{}
velodyne:
{}
submap_node0:
{}
world:
robot5/base_footprint_ground_truth:
{}
Update Interval: 0
Value: true
- Class: rviz/MarkerArray
......@@ -355,7 +404,7 @@ Visualization Manager:
Global Options:
Background Color: 130; 130; 130
Default Light: true
Fixed Frame: world
Fixed Frame: map_laser2d
Frame Rate: 70
Name: root
Tools:
......
graph_localization/src/mcl_ndt/mcl_ndt.cpp
View file @ 446c08af
......@@ -176,8 +176,8 @@ bool MCLNDTType::UpdateAndPredict(pcl::PointCloud<pcl::PointXYZ> &cloud, const E
Eigen::Affine3d Tlast=graph_map_->GetCurrentNodePose()*pf.getMean();
bool disable_prediciton_noise=false; // if there os
if( (Tlast.translation()-pose_last_update_.translation()).norm()<0.01 && (pose_last_update_.inverse()*Tlast).rotation().eulerAngles(0,1,2).norm()<0.005 )
disable_prediciton_noise=true;
//if( (Tlast.translation()-pose_last_update_.translation()).norm()<0.01 && (pose_last_update_.inverse()*Tlast).rotation().eulerAngles(0,1,2).norm()<0.005 )
// disable_prediciton_noise=true;
OdometryPrediction(Tmotion,disable_prediciton_noise);
Eigen::Affine3d Tinit=graph_map_->GetCurrentNodePose()*pf.getMean();
......
graph_map/launch/open_map/graph_map_server.launch
View file @ 446c08af
......@@ -8,7 +8,7 @@
<param name="file_name" value="$(arg file_name)"/>
</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" />
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 world map 100" />
</launch>
......
graph_map/maps/ncfm.yaml 0 → 100644
View file @ 446c08af
image: occupancy_map.pgm
resolution: 0.150000
origin: [-17.85, -37.350002, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
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