void Explore::maxForward(){ ROS_INFO("enter maxForward "); int counter = 0; float d_x = 0, d_y=0, x_map, y_map, x_odom, y_odom; transfromRobotToOdomPosition(d_x, d_y, x_odom, y_odom); while (canMove(x_odom, y_odom)){ d_x += 0.1; d_y = 0; transfromRobotToOdomPosition(d_x, d_y, x_odom, y_odom); /* ++counter; if(counter > 10){ ROS_INFO("leave maxForward, can not move forward"); return; }*/ }; // ROS_INFO("d_x = %f", d_x); float robotAngleInMap = getRobotAngleInMap(); transfromRobotToMapPosition(d_x, d_y, x_map, y_map); // ROS_INFO("robot move to (%f,%f)", x_map, y_map); publishPose(x_map, y_map, robotAngleInMap); // ROS_INFO("wait for result"); // ac_.waitForResult(); ROS_INFO("leave maxForward"); }
// Set and publish pose. void Publisher::publishStateAsCallback( const okvis::Time & t, const okvis::kinematics::Transformation & T_WS) { setTime(t); setPose(T_WS); // TODO: provide setters for this hack publishPose(); }
void Explore::randomForward(){ double forward = 0.5 + ((2.0-0.3+1)*rand()/RAND_MAX+1.0); ROS_INFO("forward %f", forward); publishPose(forward, 0, 0, USE_ROBOT); }
void Explore::randomForward(){ ROS_INFO("enter randomForward "); int counter = 0; float rand_x, rand_y, x_odom, y_odom, x_map, y_map; do { rand_x = (rand() % 10) / 20.0; rand_y = 0; transfromRobotToOdomPosition(rand_x, rand_y, x_odom, y_odom); ++counter; if(counter > 10){ ROS_INFO("leave randomForward, can not move forward"); return; } } while (!canMove(x_odom, y_odom)); float robotAngleInMap = getRobotAngleInMap(); transfromRobotToMapPosition(rand_x, rand_y, x_map, y_map); // ROS_INFO("robot move to (%f,%f)", x_map, y_map); publishPose(x_map, y_map, robotAngleInMap); // ROS_INFO("wait for result"); // ac_.waitForResult(); ROS_INFO("leave randomForward"); }
void Explore::randomRotate(){ float angle = 70 + ((360-90+1)*rand()/(RAND_MAX+1.0)); angle = ((angle) * PI) / 180.0; publishPose(0, 0, angle, USE_ROBOT); }
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 VoEstimator::publishUpdate(int64_t utime, Eigen::Isometry3d local_to_head, std::string channel, bool output_alpha_filter){ if ((!pose_initialized_) || (!vo_initialized_)) { std::cout << (int) pose_initialized_ << " pose\n"; std::cout << (int) vo_initialized_ << " vo\n"; std::cout << "pose, vo, zheight not initialized, refusing to publish POSE_HEAD nad POSE_BODY\n"; return; } // Send vo pose to collections: Isometry3dTime local_to_headT = Isometry3dTime(utime, local_to_head); pc_vis_->pose_to_lcm_from_list(60000, local_to_headT); // std::cout << head_rot_rate_.transpose() << " head rot rate out\n"; // std::cout << head_lin_rate_.transpose() << " head lin rate out\n"; // publish local to head pose if (output_alpha_filter){ publishPose(utime, channel + channel_extension_, local_to_head, head_lin_rate_alpha_, head_rot_rate_alpha_); }else{ publishPose(utime, channel + channel_extension_, local_to_head, head_lin_rate_, head_rot_rate_); } }
void Node::reset() { MapBase::UniqueLock lock(map_->getWriteLock()); last_map_update_pose_.setIdentity(); last_map_update_time_ = ros::Time(); map_odom_transform_.setIdentity(); if (map_) map_->reset(); if (matcher_) matcher_->reset(); lock.unlock(); publishMap(); publishPose(); }
void Explore::maxForward(){ float d_x = 0.1, d_y=0.0, x_map, y_map; transfromRobotToMapPosition(d_x, d_y, x_map, y_map); while (canMove(x_map, y_map)){ d_x += 0.1; transfromRobotToMapPosition(d_x, d_y, x_map, y_map); }; ROS_INFO("d_x = %f", d_x); publishPose(x_map, y_map, get, getRobotAngleInMap(), USE_MAP); }
/// @brief The event loop. Basically an ever running while with ros::spinOnce() /// This is a re-implementation taking into care the memory scopes and also processing only points with higher image gradients void ImuDeadReckon::eventLoop() { ros::Rate rate(100); //100Hz while( ros::ok() ) { ros::spinOnce(); if( !(this->isIMUDataReady) ) continue; updateNominalStateWithCurrentMeasurements(); publishPose(); ROS_INFO_STREAM_THROTTLE( 5, "(every5 sec) Lin Acc : "<< lin_acc.transpose() ); ROS_INFO_STREAM_THROTTLE( 5, "Ang Vel : "<< ang_vel.transpose() ); rate.sleep(); } }
void sptam::sptam_node::onImages( const sensor_msgs::ImageConstPtr& l_image_msg, const sensor_msgs::CameraInfoConstPtr& left_info, const sensor_msgs::ImageConstPtr& r_image_msg, const sensor_msgs::CameraInfoConstPtr& right_info ) { ROS_INFO_STREAM("dt: " << (l_image_msg->header.stamp - r_image_msg->header.stamp).toSec()); // Save current Time ros::Time currentTime = l_image_msg->header.stamp; // If using odometry, try to get a new estimate for the current pose. // if not using odometry, camera_to_odom is left blank. tf::StampedTransform camera_to_odom; if ( use_odometry_ ) { if ( not getCameraInOdom(camera_to_odom, currentTime) ) return; TFPose2CameraPose(odom_to_map_ * camera_to_odom, cameraPose_); } else { cv::Point3d currentCameraPosition; cv::Vec4d currentCameraOrientation; motionModel_->PredictNextCameraPose(currentCameraPosition, currentCameraOrientation); cameraPose_ = CameraPose( currentCameraPosition, currentCameraOrientation ); } // If SPTAM has not been initialized yet, do it if ( sptam_ == nullptr ) { ROS_INFO_STREAM("init calib"); loadCameraCalibration(left_info, right_info); } // convert image to OpenCv cv::Mat format cv_bridge::CvImageConstPtr bridgeLeft_ptr = cv_bridge::toCvShare(l_image_msg, "rgb8"); cv_bridge::CvImageConstPtr bridgeRight_ptr = cv_bridge::toCvShare(r_image_msg, "rgb8"); // save images cv::Mat imageLeft = bridgeLeft_ptr->image; cv::Mat imageRight = bridgeRight_ptr->image; #ifdef SHOW_PROFILING double start, end; start = GetSeg(); #endif // SHOW_PROFILING #ifdef SHOW_PROFILING double startStep, endStep; startStep = GetSeg(); #endif FeatureExtractorThread featureExtractorThreadLeft(imageLeft, *featureDetector_, *descriptorExtractor_); FeatureExtractorThread featureExtractorThreadRight(imageRight, *featureDetector_, *descriptorExtractor_); featureExtractorThreadLeft.WaitUntilFinished(); const std::vector<cv::KeyPoint>& keyPointsLeft = featureExtractorThreadLeft.GetKeyPoints(); const cv::Mat& descriptorsLeft = featureExtractorThreadLeft.GetDescriptors(); featureExtractorThreadRight.WaitUntilFinished(); const std::vector<cv::KeyPoint>& keyPointsRight = featureExtractorThreadRight.GetKeyPoints(); const cv::Mat& descriptorsRight = featureExtractorThreadRight.GetDescriptors(); ImageFeatures imageFeaturesLeft = ImageFeatures(imageLeft, keyPointsLeft, descriptorsLeft, mapper_params_.matchingCellSize); ImageFeatures imageFeaturesRight = ImageFeatures(imageRight, keyPointsRight, descriptorsRight, mapper_params_.matchingCellSize); #ifdef SHOW_PROFILING endStep = GetSeg(); WriteToLog(" tk Extract: ", startStep, endStep); #endif // if the map was not initialized, try to build it if( not map_.nMapPoints() ) { ROS_INFO("Trying to intialize map..."); // Create keyFrame StereoFrame* frame = new StereoFrame( cameraPose_, cameraParametersLeft_, stereo_baseline_, cameraParametersRight_, imageFeaturesLeft, imageFeaturesRight, true ); frame->SetId( 0 ); // Set Keyframe ID // Initialize MapMaker // TODO: this bool must be a local variable to do not check not map_.nMapPoints() in the "if" /*bool isMapInitialized = */InitFromStereo( map_, *frame, imageLeft, *rowMatcher_); } // if the map is already initialized, do tracking else { cameraPose_ = sptam_->track(cameraPose_, imageFeaturesLeft, imageFeaturesRight, imageLeft, imageRight); #ifdef SHOW_PROFILING end = GetSeg(); std::cout << GetSeg() << " tk trackingtotal: " << (end - start) << std::endl; std::stringstream message; message << std::fixed << GetSeg() << " tk trackingtotal: " << (end - start) << std::endl; Logger::Write( message.str() ); #endif // fix the error between map and odom to correct the current pose. if ( use_odometry_ ) fixOdomFrame( cameraPose_, camera_to_odom, currentTime ); else // if odometry is disabled, odom_to_map_ actually contains the map->cam transform because I'm too lazy { motionModel_->UpdateCameraPose(cameraPose_.GetPosition(), cameraPose_.GetOrientationQuaternion()); tf::Transform cam_to_map; CameraPose2TFPose(cameraPose_, cam_to_map); tf::StampedTransform base_to_cam; transform_listener_.lookupTransform(camera_frame_, base_frame_, currentTime, base_to_cam); std::lock_guard<std::mutex> lock( odom_to_map_mutex_ ); odom_to_map_ = cam_to_map * base_to_cam; } } // Publish Map To be drawn by rviz visualizer publishMap(); // Publish the camera Pose publishPose( l_image_msg->header.seq, currentTime, cameraPose_ ); }
//**************************************************************************************************************************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 Explore::robotFullRotate(){ float angle = (360 * PI) / 180.0; publishPose(0, 0, angle, USE_ROBOT); }