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

mapping works

parent 54257cfa
......@@ -13,7 +13,6 @@
</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)" />
......@@ -127,7 +126,7 @@
<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.4"/>
<param name="occupancy_min_z" value="0.1"/>
<param name="occupancy_max_z" value="1.8"/>
</node>
......
......@@ -34,15 +34,6 @@
</node>
<!--node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<remap from="cloud_in" to="$(arg output_points_topic)"/>
<param name="frame_id" value="map_laser2d"/>
<param name="resolution" value="0.10"/>
<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_max_z" value="1.8"/>
</node-->
</launch>
......
......@@ -446,7 +446,7 @@ public:
}
};
NDTCalibOptimize(NDTCalibScanPairs &pairs, int scoreType, ObjectiveType objectiveType, PoseInterpolationInteface/*PoseInterpolationNavMsgsOdo*/ &poseInterp, const std::string &poseFrameId) : _pairs(pairs), _scoreType(scoreType), _objectiveType(objectiveType), _poseInterp(poseInterp), _poseFrameId(poseFrameId) { InitializePublisher();}
NDTCalibOptimize(NDTCalibScanPairs &pairs, int scoreType, ObjectiveType objectiveType, PoseInterpolationInteface/*PoseInterpolationNavMsgsOdo*/ &poseInterp, const std::string &poseFrameId) : _pairs(pairs), _scoreType(scoreType), _objectiveType(objectiveType), _poseInterp(poseInterp), _poseFrameId(poseFrameId) {}
// Main function to call. Provide the initial sensor pose and time offset. These will be updated with the calibrated results.
bool calibrate(Eigen::Affine3d &initT, double &sensorTimeOffset);
......@@ -463,9 +463,9 @@ public:
// Update the poses with a sensor time offset.
void interpPairPoses(double sensorTimeOffset);
void InitializePublisher(){
void InitializePublisher(const std::string &sensor_output_topic){
ros::NodeHandle nh("~");
sensor_pub = nh.advertise<geometry_msgs::Pose> ("/calibrated_sensor_pose", 1);
sensor_pub = nh.advertise<geometry_msgs::Pose> (sensor_output_topic, 10);
}
private:
......
......@@ -5,12 +5,11 @@ Panels:
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /MarkerArray1
- /MarkerArray1/Namespaces1
- /PointCloud21
- /Marker1
- /PointCloud22
- /TF1
- /TF1/Frames1
- /TF1/Tree1
- /TF1/Tree1/world1
Splitter Ratio: 0.5
Tree Height: 775
- Class: rviz/Selection
......@@ -31,7 +30,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
......@@ -96,9 +95,7 @@ Visualization Manager:
Marker Topic: /visualization_marker
Name: Marker
Namespaces:
ndt_calib_corr_pose_est_sensor_pose: true
ndt_calib_est_sensor_pose: true
ndt_calib_pose: true
{}
Queue Size: 100
Value: true
- Alpha: 1
......@@ -135,69 +132,73 @@ Visualization Manager:
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
map_laser2d:
All Enabled: false
fuser_base_link:
Value: true
robot5/asus_fork_base_link:
laser_link:
Value: true
map_laser2d:
Value: false
robot5/asus_fork_base_link:
Value: false
robot5/asus_fork_depth_frame:
Value: true
Value: false
robot5/asus_fork_depth_optical_frame:
Value: true
Value: false
robot5/asus_fork_link:
Value: true
Value: false
robot5/asus_fork_rgb_frame:
Value: true
Value: false
robot5/asus_fork_rgb_optical_frame:
Value: true
Value: false
robot5/base_footprint:
Value: true
Value: false
robot5/base_footprint_ground_truth:
Value: true
Value: false
robot5/base_fork:
Value: true
Value: false
robot5/base_link:
Value: true
Value: false
robot5/fixed_left_wheel_link:
Value: true
Value: false
robot5/fixed_right_wheel_link:
Value: true
Value: false
robot5/kinect2_base_link:
Value: true
Value: false
robot5/kinect2_depth_optical_frame:
Value: true
Value: false
robot5/kinect2_ir_optical_frame:
Value: true
Value: false
robot5/kinect2_link:
Value: true
Value: false
robot5/kinect2_rgb_optical_frame:
Value: true
Value: false
robot5/laser2d_floor_base_link:
Value: true
Value: false
robot5/laser2d_floor_link:
Value: true
Value: false
robot5/laser2d_top_base_link:
Value: true
Value: false
robot5/laser2d_top_link:
Value: true
Value: false
robot5/left_fork:
Value: true
Value: false
robot5/odom:
Value: true
Value: false
robot5/right_fork:
Value: true
Value: false
robot5/sd_wheel_link:
Value: true
Value: false
robot5/steer_link:
Value: true
Value: false
robot5/velodyne:
Value: true
Value: false
robot5/velodyne_base_link:
Value: true
Value: false
robot5/velodyne_link:
Value: true
Value: false
velodyne:
Value: true
Value: false
world:
Value: true
Marker Scale: 1
......@@ -206,7 +207,12 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
map_laser2d:
world:
fuser_base_link:
laser_link:
{}
robot5/base_footprint_ground_truth:
{}
robot5/odom:
robot5/base_footprint:
robot5/base_link:
......@@ -247,15 +253,13 @@ Visualization Manager:
robot5/velodyne_link:
robot5/velodyne:
{}
velodyne:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map_laser2d
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
......@@ -275,7 +279,7 @@ Visualization Manager:
Value: true
Views:
Current:
Angle: -0.31499961
Angle: -1.38999987
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
......@@ -285,11 +289,11 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Scale: 28.824173
Scale: 124.61657
Target Frame: state_base_link
Value: TopDownOrtho (rviz)
X: -0.833579183
Y: -20.893837
X: 2.55696821
Y: 1.05014157
Saved: ~
Window Geometry:
Displays:
......
......@@ -2,32 +2,43 @@
<!-- 0.803152 -0.0998702 0.95 -0.00179462 -0.0050552 -0.0677064 0.00670143 robot5-->
<arg name="robot_id" default="/robot5"/>
<arg name="root_link" default="/robot5/odom"/>
<arg name="root_link" default="/world "/>
<arg name="velodyne_points_topic" default="$(arg robot_id)/sensors/velodyne_compensated_points"/>
<!--node pkg="rosbag" type="play" name="player" output="screen" args=" --> <!--clock $(find ndt_offline)/bag/volvo_2017_12_01/mapping/S7.bag -r10"/-->
<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_compensated_points" />
<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 velodyne_points_topic)" />
<arg name="link" default="$(arg robot_id)/calibrated_velodyne_link" />
</include>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ndt_calibration)/launch/calib.rviz" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ndt_calibration)/launch/calib.rviz" />
<include file="$(find graph_map)/launch/visualization/3d_odometry_visualization.launch" >
<arg name="robot_id" value="$(arg robot_id)" />
<arg name="input_points_topic" value="$(arg velodyne_points_topic)" />
<arg name="output_laser_link" default="$(arg robot_id)/calibrated_velodyne_link"/>
</include>
<node pkg="ndt_calibration" type="ndt_calib_online_node" name="ndt_calib_online_node" output="screen">
<param name="input_points_topic" value="$(arg robot_id)/sensors/velodyne_compensated_points" />
<param name="resolution" value="0.65"/>
<param name="min_nb_calibration_pairs" value="20" />
<param name="max_nb_calibration_pairs" value="30"/>
<param name="min_laser_range" value="2"/>
<param name="score_type" value="3"/>
<param name="min_rotation" value="0.2"/>
<!--param name="sensor_pose_x" value="0.4" />
<param name="sensor_pose_y" value="0" />
<param name="sensor_pose_z" value="0.95" />
<param name="sensor_pose_r" value="0.0" />
<param name="sensor_pose_p" value="0" />
<param name="sensor_pose_t" value="0.0" /-->
<param name="input_points_topic" value="$(arg velodyne_points_topic)" />
<param name="resolution" value="0.65"/>
<param name="min_nb_calibration_pairs" value="20" />
<param name="max_nb_calibration_pairs" value="30"/>
<param name="min_laser_range" value="2"/>
<param name="score_type" value="3"/>
<param name="min_rotation" value="0.2"/>
<!--param name="sensor_pose_x" value="0.4" />
<param name="sensor_pose_y" value="0" />
<param name="sensor_pose_z" value="0.95" />
<param name="sensor_pose_r" value="0.0" />
<param name="sensor_pose_p" value="0" />
<param name="sensor_pose_t" value="0.0" /-->
<param name="sensor_pose_x" value="0.793" />
<param name="sensor_pose_y" value="-0.022" />
......@@ -36,7 +47,7 @@
<param name="sensor_pose_p" value="0" />
<param name="sensor_pose_t" value="0" />
<param name="visualize" value="true"/>
<param name="visualize" value="false"/>
<param name="visualize_dual" value="false"/>
<param name="cx" value="true"/>
<param name="cy" value="true"/>
......@@ -46,12 +57,12 @@
<param name="cez" value="true"/>
<param name="ct" value="true"/>
<param name="odom_link" value="$(arg robot_id)/base_footprint"/>
<param name="odom_parent_link" value="$(arg robot_id)/odom"/>
<param name="root_link" value="$(arg root_link)"/>
<param name="odom_link" value="$(arg robot_id)/base_footprint"/>
<param name="odom_parent_link" value="$(arg robot_id)/odom"/>
<param name="root_link" value="$(arg root_link)"/>
<!--param name="cy" value="true"/-->
<!--param name="cy" value="true"/-->
......
......@@ -247,7 +247,7 @@ bool NDTCalibOptimize::calibrate(Eigen::Affine3d &Ts, double &sensorTimeOffset)
Eigen::AngleAxis<double>(cez,Eigen::Vector3d::UnitZ()) ;
geometry_msgs::Pose Tsensor_msg;
tf::poseEigenToMsg(Tsens,Tsensor_msg);
std::cout<<"publish: "<<Tsens.translation()<<std::endl;
std::cout<<"publish: "<<Tsens.translation().transpose()<<std::endl;
sensor_pub.publish(Tsensor_msg);
......
......@@ -115,6 +115,7 @@ protected:
boost::mutex run_mutex_;
boost::thread client_thread_;
boost::condition_variable cond_;
std::string sensor_output_topic;
bool b_shutdown_;
bool run_calib_;
......@@ -366,6 +367,7 @@ public:
ROS_INFO_STREAM("done.");
NDTCalibOptimize opt(pairs, score_type_, objective_type_, *pose_interp_, pose_frame_);
opt.InitializePublisher(sensor_output_topic)
ROS_INFO_STREAM("Optimize time : " << objective_type_.optimizeTime());
double delta_sensor_time_offset = 0.;
......
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