| 106 | } |
| 107 | |
| 108 | void LocalizationNode::InitialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &init_pose_msg) { |
| 109 | |
| 110 | if (init_pose_msg->header.frame_id == "") { |
| 111 | LOG_WARNING << "Received initial pose with empty frame_id."; |
| 112 | } // Only accept initial pose estimates in the global frame |
| 113 | else if (tf_listener_ptr_->resolve(init_pose_msg->header.frame_id) != |
| 114 | tf_listener_ptr_->resolve(global_frame_)) { |
| 115 | LOG_ERROR << "Ignoring initial pose in frame \" " |
| 116 | << init_pose_msg->header.frame_id |
| 117 | << "\"; initial poses must be in the global frame, \"" |
| 118 | << global_frame_; |
| 119 | return; |
| 120 | } |
| 121 | |
| 122 | // In case the client sent a pose estimate in the past, integrate the |
| 123 | // intervening odometric change. |
| 124 | tf::StampedTransform tx_odom; |
| 125 | try { |
| 126 | ros::Time now = ros::Time::now(); |
| 127 | tf_listener_ptr_->waitForTransform(base_frame_, |
| 128 | init_pose_msg->header.stamp, |
| 129 | base_frame_, |
| 130 | now, |
| 131 | odom_frame_, |
| 132 | ros::Duration(0.5)); |
| 133 | tf_listener_ptr_->lookupTransform(base_frame_, |
| 134 | init_pose_msg->header.stamp, |
| 135 | base_frame_, |
| 136 | now, |
| 137 | odom_frame_, tx_odom); |
| 138 | } |
| 139 | catch (tf::TransformException &e) { |
| 140 | tx_odom.setIdentity(); |
| 141 | } |
| 142 | tf::Pose pose_new; |
| 143 | tf::Pose pose_old; |
| 144 | tf::poseMsgToTF(init_pose_msg->pose.pose, pose_old); |
| 145 | pose_new = pose_old * tx_odom; |
| 146 | |
| 147 | // Transform into the global frame |
| 148 | DLOG_INFO << "Setting pose " << ros::Time::now().toSec() << ", " |
| 149 | << pose_new.getOrigin().x() << ", " << pose_new.getOrigin().y(); |
| 150 | |
| 151 | Vec3d init_pose_mean; |
| 152 | Mat3d init_pose_cov; |
| 153 | init_pose_mean.setZero(); |
| 154 | init_pose_cov.setZero(); |
| 155 | double yaw, pitch, roll; |
| 156 | init_pose_mean(0) = pose_new.getOrigin().x(); |
| 157 | init_pose_mean(1) = pose_new.getOrigin().y(); |
| 158 | pose_new.getBasis().getEulerYPR(yaw, pitch, roll); |
| 159 | init_pose_mean(2) = yaw; |
| 160 | init_pose_cov = math::MsgCovarianceToMat3d(init_pose_msg->pose.covariance); |
| 161 | |
| 162 | amcl_ptr_->HandleInitialPoseMessage(init_pose_mean, init_pose_cov); |
| 163 | } |
| 164 | |
| 165 | void LocalizationNode::LaserScanCallback(const sensor_msgs::LaserScan::ConstPtr &laser_scan_msg_ptr){ |
nothing calls this directly
no test coverage detected