void SparseTrackerAM::broadcastTF(const PointCloudT::ConstPtr cloud_in_ptr)
{
tf::StampedTransform transform_msg(
f2b_, cloud_in_ptr->header.stamp, fixed_frame_, base_frame_);
tf_broadcaster_.sendTransform (transform_msg);
visualization_msgs::MarkerArray marker_array;
for (int i = 0; i < 20; ++i)
{
visualization_msgs::Marker marker;
marker.header.frame_id = fixed_frame_;
marker.header.stamp = cloud_in_ptr->header.stamp;
marker.ns = "particle_poses";
marker.id = i;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = visualization_msgs::Marker::SPHERE;
// Set the marker action. Options are ADD and DELETE
marker.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
//tf::quaternionTFToMsg(pf2b_[i].getRotation() marker.pose.orientation);
tf::poseTFToMsg(pf2b_[i], marker.pose);
// Set the scale of the marker
marker.scale.x = 0.01;
marker.scale.y = 0.01;
marker.scale.z = 0.01;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 1.0f;
marker.color.g = (i / 20.0);
marker.color.b = 0.0f;
marker.color.a = 1.0;
marker.lifetime = ros::Duration();
// Publish the marker
marker_array.markers.push_back(marker);
}
marker_pub_.publish(marker_array);
}
void VisualOdometry::publishTf(const std_msgs::Header& header)
{
tf::StampedTransform transform_msg(
f2b_, header.stamp, fixed_frame_, base_frame_);
tf_broadcaster_.sendTransform (transform_msg);
}
void gps_callback(const sensor_msgs::NavSatFixConstPtr& msg)
{
if (!g_got_imu) {
ROS_WARN_STREAM_THROTTLE(1, "No IMU data yet");
return;
}
if (msg->status.status < sensor_msgs::NavSatStatus::STATUS_FIX) {
ROS_WARN_STREAM_THROTTLE(1, "No GPS fix");
return;
}
if (!g_geodetic_converter.isInitialised()) {
ROS_WARN_STREAM_THROTTLE(1, "No GPS reference point set, not publishing");
return;
}
double x, y, z;
g_geodetic_converter.geodetic2Enu(msg->latitude, msg->longitude, msg->altitude, &x, &y, &z);
// (NWU -> ENU) for simulation
if (g_is_sim) {
double aux = x;
x = y;
y = -aux;
//z = z;
}
// Fill up pose message
geometry_msgs::PoseWithCovarianceStampedPtr pose_msg(
new geometry_msgs::PoseWithCovarianceStamped);
pose_msg->header = msg->header;
pose_msg->header.frame_id = "world";
pose_msg->pose.pose.position.x = x;
pose_msg->pose.pose.position.y = y;
pose_msg->pose.pose.position.z = z;
pose_msg->pose.pose.orientation = g_latest_imu_msg.orientation;
// Fill up position message
geometry_msgs::PointStampedPtr position_msg(
new geometry_msgs::PointStamped);
position_msg->header = pose_msg->header;
position_msg->header.frame_id = "world";
position_msg->point = pose_msg->pose.pose.position;
// If external altitude messages received, include in pose and position messages
if (g_got_altitude) {
pose_msg->pose.pose.position.z = g_latest_altitude_msg.data;
position_msg->point.z = g_latest_altitude_msg.data;
}
pose_msg->pose.covariance.assign(0);
// Set default covariances
pose_msg->pose.covariance[6 * 0 + 0] = g_covariance_position_x;
pose_msg->pose.covariance[6 * 1 + 1] = g_covariance_position_y;
pose_msg->pose.covariance[6 * 2 + 2] = g_covariance_position_z;
pose_msg->pose.covariance[6 * 3 + 3] = g_covariance_orientation_x;
pose_msg->pose.covariance[6 * 4 + 4] = g_covariance_orientation_y;
pose_msg->pose.covariance[6 * 5 + 5] = g_covariance_orientation_z;
// Take covariances from GPS
if (g_trust_gps) {
if (msg->position_covariance_type == sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN
|| msg->position_covariance_type == sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED) {
// Fill in completely
for (int i = 0; i <= 2; i++) {
for (int j = 0; j <= 2; j++) {
pose_msg->pose.covariance[6 * i + j] = msg->position_covariance[3 * i + j];
}
}
} else if (msg->position_covariance_type
== sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN) {
// Only fill in diagonal
for (int i = 0; i <= 2; i++) {
pose_msg->pose.covariance[6 * i + i] = msg->position_covariance[3 * i + i];
}
}
}
g_gps_pose_pub.publish(pose_msg);
g_gps_position_pub.publish(position_msg);
// Fill up transform message
geometry_msgs::TransformStampedPtr transform_msg(new geometry_msgs::TransformStamped);
transform_msg->header = msg->header;
transform_msg->header.frame_id = "world";
transform_msg->transform.translation.x = x;
transform_msg->transform.translation.y = y;
transform_msg->transform.translation.z = z;
transform_msg->transform.rotation = g_latest_imu_msg.orientation;
if (g_got_altitude) {
transform_msg->transform.translation.z = g_latest_altitude_msg.data;
}
g_gps_transform_pub.publish(transform_msg);
}
void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
{
ros::WallTime start = ros::WallTime::now();
// CSM is used in the following way:
// The scans are always in the laser frame
// The reference scan (prevLDPcan_) has a pose of [0, 0, 0]
// The new scan (currLDPScan) has a pose equal to the movement
// of the laser in the laser frame since the last scan
// The computed correction is then propagated using the tf machinery
prev_ldp_scan_->odometry[0] = 0.0;
prev_ldp_scan_->odometry[1] = 0.0;
prev_ldp_scan_->odometry[2] = 0.0;
prev_ldp_scan_->estimate[0] = 0.0;
prev_ldp_scan_->estimate[1] = 0.0;
prev_ldp_scan_->estimate[2] = 0.0;
prev_ldp_scan_->true_pose[0] = 0.0;
prev_ldp_scan_->true_pose[1] = 0.0;
prev_ldp_scan_->true_pose[2] = 0.0;
input_.laser_ref = prev_ldp_scan_;
input_.laser_sens = curr_ldp_scan;
// **** estimated change since last scan
double dt = (time - last_icp_time_).toSec();
double pr_ch_x, pr_ch_y, pr_ch_a;
getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);
// the predicted change of the laser's position, in the fixed frame
tf::Transform pr_ch;
createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch);
// account for the change since the last kf, in the fixed frame
pr_ch = pr_ch * (f2b_ * f2b_kf_.inverse());
// the predicted change of the laser's position, in the laser frame
tf::Transform pr_ch_l;
pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ;
input_.first_guess[0] = pr_ch_l.getOrigin().getX();
input_.first_guess[1] = pr_ch_l.getOrigin().getY();
input_.first_guess[2] = tf::getYaw(pr_ch_l.getRotation());
// *** scan match - using point to line icp from CSM
sm_icp(&input_, &output_);
tf::Transform corr_ch;
if (output_.valid)
{
// the correction of the laser's position, in the laser frame
tf::Transform corr_ch_l;
createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
// the correction of the base's position, in the base frame
corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;
// update the pose in the world frame
f2b_ = f2b_kf_ * corr_ch;
// **** publish
if (publish_pose_)
{
// unstamped Pose2D message
geometry_msgs::Pose2D::Ptr pose_msg;
pose_msg = boost::make_shared<geometry_msgs::Pose2D>();
pose_msg->x = f2b_.getOrigin().getX();
pose_msg->y = f2b_.getOrigin().getY();
pose_msg->theta = tf::getYaw(f2b_.getRotation());
pose_publisher_.publish(pose_msg);
}
if (publish_pose_stamped_)
{
// stamped Pose message
geometry_msgs::PoseStamped::Ptr pose_stamped_msg;
pose_stamped_msg = boost::make_shared<geometry_msgs::PoseStamped>();
pose_stamped_msg->header.stamp = time;
pose_stamped_msg->header.frame_id = fixed_frame_;
tf::poseTFToMsg(f2b_, pose_stamped_msg->pose);
pose_stamped_publisher_.publish(pose_stamped_msg);
}
if (publish_tf_)
{
tf::StampedTransform transform_msg (f2b_, time, fixed_frame_, base_frame_);
tf_broadcaster_.sendTransform (transform_msg);
}
}
else
{
corr_ch.setIdentity();
ROS_WARN("Error in scan matching");
}
// **** swap old and new
if (newKeyframeNeeded(corr_ch))
{
// generate a keyframe
ld_free(prev_ldp_scan_);
prev_ldp_scan_ = curr_ldp_scan;
f2b_kf_ = f2b_;
}
else
{
ld_free(curr_ldp_scan);
}
last_icp_time_ = time;
// **** statistics
double dur = (ros::WallTime::now() - start).toSec() * 1e3;
ROS_DEBUG("Scan matcher total duration: %.1f ms", dur);
}
