Skip to content

GitLab

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

nu fungerar demon i vilket fall

parent f1f45862
Hide whitespace changes
Inline Side-by-side
Showing with 425 additions and 286 deletions
graph_localization/launch/3d_mcl_ncfm2018.launch
View file @ 54257cfa
......@@ -42,10 +42,12 @@
<param name="calibration" value="$(find graph_map)/config/VLP16db.yaml" />
<!-- pass path to graph_map (.map)-->
<param name="map_file" value="$(arg map_file_path)" />
<param name="sensor_pose_x" value="0.80" />
<param name="sensor_pose_y" value="0" />
<param name="sensor_pose_x" value="0.80875" />
<param name="sensor_pose_y" value="-0.0285" />
<param name="sensor_pose_z" value="0.95" />
<param name="sensor_pose_t" value="0" />
<param name="sensor_pose_r" value="0" />
<param name="sensor_pose_p" value="-0.15" />
<param name="sensor_pose_t" value="-0.0284" />
<param name="gt_topic" value="/$(arg robot_id)/kmo_navserver/state" />
<param name="initial_pose_topic" value="/$(arg robot_id)/initialpose" />
......
graph_localization/launch/visualize.launch
View file @ 54257cfa
......@@ -20,7 +20,7 @@
<!--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)">
<!--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)" />
......@@ -28,12 +28,12 @@
<param name="map_parent_frame_id" value="$(arg root_link_id)"/>
<param name="map_topic" value="/maps/map_velodyne" />
</node>
</group>
</group-->
<node name="occupancy_map_server" pkg="map_server" type="map_server" args=" $(find graph_map)/maps/ncfm.yaml" output="screen">
<!--node name="occupancy_map_server" pkg="map_server" type="map_server" args=" $(find graph_map)/maps/ncfm.yaml" output="screen">
<remap from="/map" to="/maps/map_laser2d"/>
<param name="~frame_id" value="map_laser2d"/>
</node>
<param name="~frame_id" value="map_laser2d"/ </node>-->
<!--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 @ 54257cfa
......@@ -9,12 +9,15 @@ Panels:
- /TF1/Frames1
- /TF1/Tree1
- /Odometry1/Shape1
- /Odometry2
- /Odometry2/Status1
- /Odometry2/Shape1
- /Odometry3
- /Odometry3/Shape1
- /Map1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
- /LaserScan1
Splitter Ratio: 0.5
Tree Height: 815
- Class: rviz/Selection
......@@ -180,6 +183,9 @@ Visualization Manager:
{}
submap_node0:
{}
world:
robot5/base_footprint_ground_truth:
{}
Update Interval: 0
Value: true
- Class: rviz/MarkerArray
......@@ -241,7 +247,7 @@ Visualization Manager:
Scale: 1
Value: true
Value: true
Enabled: false
Enabled: true
Keep: 10000
Name: Odometry
Position Tolerance: 0.100000001
......@@ -255,9 +261,9 @@ Visualization Manager:
Shaft Length: 0.100000001
Shaft Radius: 0.00999999978
Value: Arrow
Topic: /robot5/mcl_pose_estimate
Topic: /ndt_mcl_pose
Unreliable: false
Value: false
Value: true
- Angle Tolerance: 0.100000001
Class: rviz/Odometry
Covariance:
......@@ -275,7 +281,7 @@ Visualization Manager:
Scale: 1
Value: true
Value: true
Enabled: true
Enabled: false
Keep: 100
Name: Odometry
Position Tolerance: 0.100000001
......@@ -289,16 +295,16 @@ Visualization Manager:
Shaft Length: 0.100000001
Shaft Radius: 0.00999999978
Value: Arrow
Topic: /robot5/mcl_pose_estimate
Topic: /robot5/control/state
Unreliable: false
Value: true
Value: false
- Alpha: 1
Arrow Length: 0.300000012
Axes Length: 0.300000012
Axes Radius: 0.00999999978
Class: rviz/PoseArray
Color: 255; 25; 0
Enabled: true
Enabled: false
Head Length: 0.0700000003
Head Radius: 0.0299999993
Name: PoseArray
......@@ -307,7 +313,7 @@ Visualization Manager:
Shape: Arrow (Flat)
Topic: /ndt_mcl_localization_graph/pose_particles
Unreliable: false
Value: true
Value: false
- Alpha: 0.400000006
Class: ndt_rviz/NDT
Color: 204; 51; 204
......@@ -339,7 +345,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: true
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 9.8743124
......@@ -352,7 +358,37 @@ Visualization Manager:
Size (Pixels): 3
Size (m): 0.0299999993
Style: Flat Squares
Topic: /robot5/sensors/velodyne_points_unwarped
Topic: /robot5/sensors/velodyne_points
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.0399999991
Style: Flat Squares
Topic: /robot5/sensors/laser2d_top
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
......@@ -381,7 +417,7 @@ Visualization Manager:
Value: true
Views:
Current:
Angle: 1.48000133
Angle: 1.25000155
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
......@@ -391,11 +427,11 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Scale: 54.7555885
Scale: 26.9378643
Target Frame: world
Value: TopDownOrtho (rviz)
X: -5.34080029
Y: -3.81768727
X: -1.47711372
Y: -0.717291355
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
......
graph_map/launch/3d_fuser_ncfm2018_experimental.launch
View file @ 54257cfa
<?xml version="1.0" encoding="utf-8"?>
<launch>
<arg name="run_bag" default="false"/>
<arg name="robot_prefix" default="robot5"/>
<arg name="robot_id" default="robot5"/>
<arg name="output_pointcloud_topic_name" default="/fused_cloud"/>
<arg name="rviz_enabled" default="false"/>
......@@ -13,7 +13,14 @@
</include>
</group>
<!--param name ="/use_sim_time" value="true"/-->
<include file="$(find graph_map)/launch/VelodyneConvertMotionCompensation.launch" >
<arg name="robot_id" value="$(arg robot_id)" />
<arg name="input_points_topic" value="$(arg robot_id)/sensors/velodyne_packets" />
<arg name="output_points_topic" value="$(arg robot_id)/sensors/velodyne_points_unwarped" />
<arg name="link" value="/fuser_laser_link" />
</include>
<!-- start mapping node -->
<node pkg="graph_map" type="graph_map_fuser_node" name="graph_node" output="screen">
......@@ -57,20 +64,21 @@
<!-- if laser_2d is true, a 2d scanner will be used for mapping-->
<param name="laser_2d" value="false" />
<param name="laser_frame_id" value="$(arg robot_prefix)/velodyne " />
<param name="laser_frame_id" value="/fuser_laser_link" />
<param name="use_pointcloud" value="true" />
<!-- Topic of laser scanner -->
<!--param name="points_topic" value="$(arg namespace_prefix)/kmo_navserver/laserscan1" /-->
<param name="points_topic" value="$(arg robot_prefix)/sensors/velodyne_packets" />
<param name="points_topic" value="$(arg robot_id)/sensors/velodyne_points_unwarped" />
<param name="output_pointcloud_topic_name" value="$(arg output_pointcloud_topic_name)" />
<param name="useOdometry" value="true" />
<param name="odometry_topic" value="$(arg robot_prefix)/control/odom"/>
<param name="odometry_topic" value="$(arg robot_id)/control/odom"/>
<param name="use_tf_listener" value="true" />
<param name="gt_topic" value="$(arg robot_prefix)/control/state" />
<param name="gt_topic" value="$(arg robot_id)/control/state" />
<!-- odometry frame id-->
<param name="odom_tf" value="$(arg robot_prefix)/odom" />
<param name="base_tf" value="$(arg robot_prefix)/base_footprint" />
<param name="odom_tf" value="$(arg robot_id)/odom" />
<param name="base_tf" value="$(arg robot_id)/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" />
......@@ -92,11 +100,12 @@
<param name="min_range" value="1.8" />
<!-- Specific sensor offset parameters with respect to the odometry frame -->
<param name="sensor_pose_x" value="0.807" />
<param name="sensor_pose_y" value="-0.003" />
<param name="sensor_pose_x" value="0.808" />
<param name="sensor_pose_y" value="-0.285" />
<param name="sensor_pose_z" value="0.95" />
<param name="sensor_pose_t" value="0" />
<param name="sensor_offset_t" value="0.0" />
<param name="sensor_pose_r" value="0" />
<param name="sensor_pose_p" value="0" />
<param name="sensor_pose_t" value="-0.028" />
<!-- Output directory where the map is stored -->
<!-- invoke rosservice call /graph_ -->
......@@ -114,11 +123,11 @@
<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<remap from="cloud_in" to="$(arg output_pointcloud_topic_name)"/>
<param name="frame_id" value="world"/>
<param name="resolution" value="0.15"/>
<param name="frame_id" value="/world"/>
<param name="resolution" value="0.1"/>
<param name="sensor_model/max_range" value="10.0"/>
<param name="pointcloud_max_z" value="5"/>
<param name="occupancy_min_z" value="0.1"/>
<param name="occupancy_min_z" value="0.4"/>
<param name="occupancy_max_z" value="1.8"/>
</node>
......@@ -130,6 +139,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_laser2d 100" />
<!--node pkg="tf" type="static_transform_publisher" name="velodyne_map_static_broadcaster" args="0 0 0 0 0 0 world map_laser2d 100" /-->
</launch>
graph_map/launch/VelodyneConvertMotionCompensation.launch
View file @ 54257cfa
......@@ -4,6 +4,7 @@
<arg name="robot_id" default="robot5" />
<arg name="input_points_topic" default="/$(arg robot_id)/sensors/velodyne_packets" />
<arg name="output_points_topic" default="/$(arg robot_id)/sensors/velodyne_points_unwarped" />
<arg name="link" default="$(arg robot_id)/velodyne_link" />
<!-- Run monte carlo localization -->
<!--include file="$(find ndt_calibration)/launch/visualize.launch" /-->
......@@ -11,12 +12,12 @@
<!-- select one of the following types-->
<param name="disable_compensation" value="false" />
<param name="laser_tf" value="$(arg robot_id)/velodyne_link" />
<param name="laser_tf" value="$(arg robot_id)/velodyne" />
<!-- Topic of laser scanner -->
<param name="input_points_topic" value="$(arg input_points_topic)" />
<param name="output_points_topic" value="$(arg output_points_topic)" />
<param name="output_points_link" value="$(arg robot_id)/velodyne_link" />
<param name="output_points_link" value="$(arg link)" />
<param name="calibration" value="$(find graph_map)/config/VLP16db.yaml" />
<param name="sensor_offset_topic" value="/sensor_pose" />
......@@ -24,12 +25,14 @@
<param name="odom_link" value="$(arg robot_id)/base_footprint" />
<!-- pass path to graph_map (.map)-->
<param name="sensor_pose_x" value="0.80" />
<param name="sensor_pose_y" value="0" />
<param name="sensor_pose_z" value="0.95" />
<param name="sensor_pose_t" value="0" />
</node>
<param name="sensor_pose_x" value="0.808" />
<param name="sensor_pose_y" value="-0.285" />
<param name="sensor_pose_z" value="0" />
<param name="sensor_pose_r" value="0" />
<param name="sensor_pose_p" value="0" />
<param name="sensor_pose_t" value="-0.028" />
</node>
<!--node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<remap from="cloud_in" to="$(arg output_points_topic)"/>
......
graph_map/launch/visualize_graph_fuser.launch
View file @ 54257cfa
......@@ -11,8 +11,9 @@
<arg name="kitti" default="false" />
<arg name="excavator_laps" default="false" />
<arg name="volvo_2017_12_01" default="false" />
<arg name="michigan" default="false" />
<arg name="michigan" default="false" />
<arg name="lidaronly" default="false" />
<arg name="odometry-ncfm2018" default="false" />
<group if="$(arg mapping)">
<!--node pkg="rviz" type="rviz" name="rviz" args="-d $(find graph_map)/rviaz/mapping_oru.rviz" /-->
......@@ -78,5 +79,8 @@
<group if="$(arg default)">
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find graph_mapping)/rviz/showMap.rviz" />
</group>
<group if="$(arg odometry-ncfm2018)">
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find graph_map)/rviz/odometry_visualization.rviz" />
</group>
</launch>
graph_map/rviz/mapping_oru_lab.rviz
View file @ 54257cfa
......@@ -9,6 +9,7 @@ Panels:
- /TF1/Frames1
- /TF1/Tree1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
- /Odometry1/Shape1
- /Odometry2/Shape1
- /Odometry3/Shape1
......@@ -80,50 +81,72 @@ Visualization Manager:
All Enabled: false
fuser_base_link:
Value: true
laser_link:
map_laser2d:
Value: true
odom_base_link:
Value: true
robot4/base_footprint:
robot5/asus_fork_base_link:
Value: false
robot5/asus_fork_depth_frame:
Value: false
robot5/asus_fork_depth_optical_frame:
Value: false
robot5/asus_fork_link:
Value: false
robot5/asus_fork_rgb_frame:
Value: false
robot5/asus_fork_rgb_optical_frame:
Value: false
robot5/base_footprint:
Value: false
robot4/base_fork:
robot5/base_footprint_ground_truth:
Value: false
robot4/base_link:
robot5/base_fork:
Value: false
robot4/fixed_left_wheel_link:
robot5/base_link:
Value: false
robot4/fixed_right_wheel_link:
robot5/fixed_left_wheel_link:
Value: false
robot4/kinect2_depth_optical_frame:
robot5/fixed_right_wheel_link:
Value: false
robot4/kinect2_ir_optical_frame:
robot5/kinect2_base_link:
Value: false
robot4/kinect2_link:
robot5/kinect2_depth_optical_frame:
Value: false
robot4/kinect2_rgb_optical_frame:
robot5/kinect2_ir_optical_frame:
Value: false
robot4/laser2d_floor:
robot5/kinect2_link:
Value: false
robot4/laser2d_floor_link:
robot5/kinect2_rgb_optical_frame:
Value: false
robot4/laser2d_top:
robot5/laser2d_floor_base_link:
Value: false
robot4/laser2d_top_link:
robot5/laser2d_floor_link:
Value: false
robot4/left_fork:
robot5/laser2d_top_base_link:
Value: false
robot4/right_fork:
robot5/laser2d_top_link:
Value: false
robot4/robot4/velodyne:
robot5/left_fork:
Value: false
robot4/robot4/velodyne_base_link:
robot5/odom:
Value: false
robot4/sd_wheel_link:
robot5/right_fork:
Value: false
robot4/steer_link:
robot5/sd_wheel_link:
Value: false
robot5/steer_link:
Value: false
robot5/velodyne:
Value: true
robot5/velodyne_base_link:
Value: false
robot5/velodyne_link:
Value: false
submap_node0:
Value: true
velodyne:
Value: false
velodyne_new:
Value: true
world:
Value: false
Marker Scale: 1
......@@ -134,42 +157,52 @@ Visualization Manager:
Tree:
world:
fuser_base_link:
laser_link:
velodyne_new:
{}
odom_base_link:
map_laser2d:
{}
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:
robot5/base_footprint_ground_truth:
{}
robot5/odom:
robot5/base_footprint:
robot5/base_link:
robot5/asus_fork_base_link:
robot5/asus_fork_link:
robot5/asus_fork_depth_frame:
robot5/asus_fork_depth_optical_frame:
{}
robot5/asus_fork_rgb_frame:
robot5/asus_fork_rgb_optical_frame:
{}
robot5/base_fork:
robot5/left_fork:
{}
robot5/right_fork:
{}
robot5/fixed_left_wheel_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:
{}
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:
{}
submap_node0:
{}
Update Interval: 0
......@@ -189,18 +222,18 @@ Visualization Manager:
{}
Queue Size: 100
Value: false
- Alpha: 1
- Alpha: 0.150000006
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 9.04336834
Min Value: -0.852163672
Value: true
Max Value: 5.53900051
Min Value: -1
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Decay Time: 1000
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
......@@ -211,10 +244,10 @@ Visualization Manager:
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (Pixels): 2
Size (m): 0.0299999993
Style: Points
Topic: /fused_cloud
Topic: /robot5/sensors/velodyne_points_unwarped
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
......@@ -321,7 +354,7 @@ Visualization Manager:
Topic: /graph_node/ndt_pose_est
Unreliable: false
Value: true
- Alpha: 0.400000006
- Alpha: 1
Class: ndt_rviz/NDT
Color: 204; 51; 204
Enabled: true
......@@ -330,7 +363,7 @@ Visualization Manager:
Topic: /graph_node/NDTOmMap
Unreliable: false
Value: true
- Alpha: 0.300000012
- Alpha: 1
Class: rviz/Map
Color Scheme: map
Draw Behind: true
......@@ -365,25 +398,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/ThirdPersonFollower
Distance: 24.8979836
Distance: 22.194479
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 4.68356228
Y: 6.51511669
Z: 0.0867903084
X: -1.24967933
Y: 0.316982746
Z: 0.0867976025
Focal Shape Fixed Size: false
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.19479752
Pitch: 1.34979367
Target Frame: <Fixed Frame>
Value: ThirdPersonFollower (rviz)
Yaw: 6.13178396
Yaw: 4.52997684
Saved:
- Class: rviz/Orbit
Distance: 39.1665726
......
graph_map/src/VelodyneMotionCompensation.cpp
View file @ 54257cfa
......@@ -113,7 +113,7 @@ bool OdometryTFaUnwarpTransform(const velodyne_msgs::VelodyneScan::ConstPtr &msg
Eigen::Affine3d Todom_first, Todom_last;
if(!Transform(t_first, Todom_first) || !Transform(t_last, Todom_last)){ //tf exists for first and last
std::cout<<"Unwarp without interpolation"<<std::endl;
//std::cout<<"Unwarp without interpolation"<<std::endl;
ndt_generic::UnwarpCloudSimple(dataParser, msg, cloud);
cloud.header.frame_id = output_points_link;
pcl_conversions::toPCL(t_last, cloud.header.stamp);
......@@ -151,7 +151,7 @@ localisation_node(ros::NodeHandle param){