void PSMpositionNode::getMotion_can(const sensor_msgs::LaserScan& scan, std::vector <depthPoint> *depthLine)//*********************************************getMotion_can()
{
ROS_DEBUG("Received scan");
// **** if this is the first scan, initialize and leave the function here
if (!initialized_can)
{
initialized_can = initializeCan(scan);
if (initialized_can) ROS_INFO("Matcher initialized");
return;
}
// **** attmempt to match the two scans
// CSM is used in the following way:
// The reference scan (prevLDPcan_) always has a pose of 0
// The new scan (currLDPScan) has a pose equal to the movement
// of the laser in the world frame since the last scan (btTransform change)
// The computed correction is then propagated using the tf machinery
prevLDPScan_->odometry[0] = 0;
prevLDPScan_->odometry[1] = 0;
prevLDPScan_->odometry[2] = 0;
prevLDPScan_->estimate[0] = 0;
prevLDPScan_->estimate[1] = 0;
prevLDPScan_->estimate[2] = 0;
prevLDPScan_->true_pose[0] = 0;
prevLDPScan_->true_pose[1] = 0;
prevLDPScan_->true_pose[2] = 0;
btTransform currWorldToBase;
btTransform change;
change.setIdentity();
// what odometry model to use
if (useTfOdometry_)
{
// get the current position of the base in the world frame
// if no transofrm is available, we'll use the last known transform
getCurrentEstimatedPose(currWorldToBase, scan);
change = laserToBase_ * prevWorldToBase_.inverse() * currWorldToBase * baseToLaser_;
}
else if (useImuOdometry_)
{
imuMutex_.lock();
double dTheta = currImuAngle_ - prevImuAngle_;
prevImuAngle_ = currImuAngle_;
change.getRotation().setRPY(0.0, 0.0, dTheta);
imuMutex_.unlock();
}
geometry_msgs::Pose2D p;
tfToPose2D(change, p);
LDP currLDPScan = rosToLDPScan(scan, p);
input_.laser_ref = prevLDPScan_;
input_.laser_sens = currLDPScan;
input_.first_guess[0] = 0;
input_.first_guess[1] = 0;
input_.first_guess[2] = 0;
sm_icp(&input_, &output_);
if (!output_.valid)
{
ROS_WARN("Error in scan matching");
ld_free(prevLDPScan_);
prevLDPScan_ = currLDPScan;
return;
}
// **** calculate change in position
double dx = output_.x[0];
double dy = output_.x[1];
double da = output_.x[2];
// change = scan match result for how much laser moved between scans,
// in the world frame
change.setOrigin(btVector3(dx, dy, 0.0));
btQuaternion q;
q.setRPY(0, 0, da);
change.setRotation(q);
frameP_ = worldFrame_;
heightLine = ransac(depthLine);
// **** publish the new estimated pose as a tf
if(take_vicon == true) {
prevWorldToBase_.setOrigin(btVector3(pos_vicon[0],pos_vicon[1], pos_vicon[2]));
btQuaternion vicon_q(quat_vicon.x(),quat_vicon.y(),quat_vicon.z(),quat_vicon.w());
prevWorldToBase_.setRotation(vicon_q);
}
currWorldToBase = prevWorldToBase_ * baseToLaser_ * change * laserToBase_;
if (publishTf_ ) publishTf (currWorldToBase, scan.header.stamp);
if (publishPose_) publishPose(currWorldToBase, scan.header.stamp, frameP_, heightLine);
// **** swap old and new
ld_free(prevLDPScan_);
prevLDPScan_ = currLDPScan;
prevWorldToBase_ = currWorldToBase;
// **** timing information - needed for profiling only
}
void initializeAllCan() {
for(int i = 0; i < getCanBusCount(); i++) {
initializeCan(&(getCanBuses()[i]));
}
}
