void open_usb() {
ROS_INFO("Connecting to %s...", port.c_str());
ros::Time start = ros::Time::now();
double notify_every = 10.0;
double check_every = 0.25;
std::string last_msg;
while (ros::ok()) {
try {
ser->Open(port.c_str());
ROS_INFO("Connected to %s", port.c_str());
break;
} catch (USBSerial::Exception &e) {
last_msg = e.what();
}
ros::Duration(check_every).sleep();
double dur = (ros::Time::now() - start).toSec();
if (dur > notify_every) {
ROS_WARN_THROTTLE(notify_every,
"Haven't connected to %s in %.2f seconds."
" Last error=\n%s",
port.c_str(), dur, last_msg.c_str());
}
publishTf();
}
}
bool Node::update()
{
// get pose from tf
if (p_use_tf_pose_start_estimate_) {
tf::StampedTransform odom_pose;
try {
getTransformListener().waitForTransform(p_odom_frame_, p_base_frame_, scan_.getStamp(), ros::Duration(1.0));
getTransformListener().lookupTransform(p_odom_frame_, p_base_frame_, scan_.getStamp(), odom_pose);
matcher_->setTransform(map_odom_transform_ * odom_pose);
} catch(tf::TransformException& e) {
ROS_ERROR("Could not get pose from tf: %s", e.what());
return false;
}
}
// match scan
if (!map_->empty()) {
#ifdef USE_HECTOR_TIMING
hector_diagnostics::TimingSection section("scan matcher");
#endif
matcher_->computeCovarianceIf(pose_with_covariance_publisher_ && pose_with_covariance_publisher_.getNumSubscribers() > 0);
matcher_->match(*map_, scan_);
if (!matcher_->valid()) return false;
}
// update map
float_t position_difference, orientation_difference;
matcher_->getPoseDifference(last_map_update_pose_, position_difference, orientation_difference);
if (map_->empty() || position_difference > p_map_update_translational_threshold_ || orientation_difference > p_map_update_angular_threshold_) {
#ifdef USE_HECTOR_TIMING
hector_diagnostics::TimingSection section("map update");
#endif
// ros::WallTime _map_update_start = ros::WallTime::now();
map_->insert(scan_, matcher_->getTransform());
// ROS_DEBUG("Map update took %f seconds.", (ros::WallTime::now() - _map_update_start).toSec());
last_map_update_pose_ = matcher_->getTransform();
last_map_update_time_ = scan_.getStamp();
}
// publish pose
publishPose();
// publish tf
if (p_pub_map_odom_transform_) publishTf();
// publish timing
#ifdef USE_HECTOR_TIMING
timing_publisher_.publish(hector_diagnostics::TimingAggregator::Instance()->update(matcher_->getStamp()));
#endif
return true;
}
void VisualOdometry::RGBDCallback(
const ImageMsg::ConstPtr& rgb_msg,
const ImageMsg::ConstPtr& depth_msg,
const CameraInfoMsg::ConstPtr& info_msg)
{
ros::WallTime start = ros::WallTime::now();
// **** initialize ***************************************************
if (!initialized_)
{
initialized_ = getBaseToCameraTf(rgb_msg->header);
init_time_ = rgb_msg->header.stamp;
if (!initialized_) return;
motion_estimation_.setBaseToCameraTf(eigenAffineFromTf(b2c_));
}
// **** create frame *************************************************
ros::WallTime start_frame = ros::WallTime::now();
rgbdtools::RGBDFrame frame;
createRGBDFrameFromROSMessages(rgb_msg, depth_msg, info_msg, frame);
ros::WallTime end_frame = ros::WallTime::now();
// **** find features ************************************************
ros::WallTime start_features = ros::WallTime::now();
feature_detector_->findFeatures(frame);
ros::WallTime end_features = ros::WallTime::now();
// **** registration *************************************************
ros::WallTime start_reg = ros::WallTime::now();
AffineTransform m = motion_estimation_.getMotionEstimation(frame);
tf::Transform motion = tfFromEigenAffine(m);
f2b_ = motion * f2b_;
ros::WallTime end_reg = ros::WallTime::now();
// **** publish outputs **********************************************
if (publish_tf_) publishTf(rgb_msg->header);
if (publish_odom_) publishOdom(rgb_msg->header);
if (publish_path_) publishPath(rgb_msg->header);
if (publish_pose_) publishPoseStamped(rgb_msg->header);
if (publish_feature_cloud_) publishFeatureCloud(frame);
if (publish_feature_cov_) publishFeatureCovariances(frame);
if (publish_model_cloud_) publishModelCloud();
if (publish_model_cov_) publishModelCovariances();
// **** print diagnostics *******************************************
ros::WallTime end = ros::WallTime::now();
frame_count_++;
int n_features = frame.keypoints.size();
int n_valid_features = frame.n_valid_keypoints;
int n_model_pts = motion_estimation_.getModelSize();
double d_frame = 1000.0 * (end_frame - start_frame ).toSec();
double d_features = 1000.0 * (end_features - start_features).toSec();
double d_reg = 1000.0 * (end_reg - start_reg ).toSec();
double d_total = 1000.0 * (end - start ).toSec();
diagnostics(n_features, n_valid_features, n_model_pts,
d_frame, d_features, d_reg, d_total);
}
//**************************************************************************************************************************getMotion()
void PSMpositionNode::getMotion(const sensor_msgs::LaserScan& scan, std::vector <depthPoint> *depthLine)
{
ROS_DEBUG("Received kinectLine");
// **** attmempt to match the two scans
// PM scan matcher is used in the following way:
// The reference scan (prevPMScan_) always has a pose of 0
// The new scan (currPMScan) 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
if (!initialized_)
{
initialized_ = initialize(scan);
if (initialized_) ROS_INFO("Matcher Horizontal initialized");
return;
}
prevPMScan_->rx = 0;
prevPMScan_->ry = 0;
prevPMScan_->th = 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();
}
PMScan * currPMScan = new PMScan(scan.ranges.size());
rosToPMScan(scan, change, currPMScan);
try
{
matcher_.pm_psm(prevPMScan_, currPMScan);
}
catch(int err)
{
ROS_WARN("Error in scan matching");
delete prevPMScan_;
prevPMScan_ = currPMScan;
return;
};
// **** calculate change in position
// rotate by -90 degrees, since polar scan matcher assumes different laser frame
// and scale down by 100
double dx = currPMScan->ry / ROS_TO_PM;
double dy = -currPMScan->rx / ROS_TO_PM;
double da = currPMScan->th;
// 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);
// **** 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);
prevWorldToBase_.setIdentity();
take_vicon=false;
}
currWorldToBase = prevWorldToBase_ * baseToLaser_ * change * laserToBase_;
heightLine = ransac(depthLine);
frameP_ = worldFrame_;
if (publishTf_ ) publishTf (currWorldToBase, scan.header.stamp);
if (publishPose_) publishPose(currWorldToBase, scan.header.stamp, frameP_, heightLine);
// **** swap old and new
delete prevPMScan_;
prevPMScan_ = currPMScan;
prevWorldToBase_ = currWorldToBase;
}
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 StaticPoseNode::callbackSync(const sensor_msgs::CameraInfoConstPtr &msg) {
publishTf(msg->header.stamp);
}
