C++ TransformListener::waitForTransform方法代码示例

void Explore::transfromRobotToMapPosition(float x_robot, float y_robot, float &x_map, float &y_map){

	ros::Time now = ros::Time::now();

	tf::StampedTransform tfRO;
	tf_listener_.waitForTransform("/base_link", "/odom", now, ros::Duration(1.0));
	tf_listener_.lookupTransform ("/base_link", "/odom", now,  tfRO);
	tf::Transform tfOR = tfRO.inverse();


	tf::StampedTransform tfOM;
	tf_listener_.waitForTransform("/odom", "/map", now, ros::Duration(1.0));
	tf_listener_.lookupTransform ("/odom", "/map", now,  tfOM);
	tf::Transform tfMO = tfOM.inverse();


	tf::Transform tfRM = tfMO*tfOR;

	float noused = 0.0;

	tf::Transform tfRD;
	tfRD.setOrigin(tf::Vector3(x_robot, y_robot, noused));

	tf::Transform tfOD = tfOR * tfRD;


	tf::Transform tfMD = tfMO * tfOD;


	x_map = tfMD.getOrigin ()[0];				//	wspolrzedne docelowe w ukladzie mapy
	y_map = tfMD.getOrigin ()[1];				//	wspolrzedne docelowe w ukladzie mapy


}

	void DepthSensorCallback(const sensor_msgs::PointCloud2ConstPtr msg)
	{
		// Get the current world space tool position
		m_MsgHeaderStamp = msg->header.stamp;
		m_Listener.waitForTransform("/world", "VSV/Tool",
			m_MsgHeaderStamp, ros::Duration(1.0));
		tf::StampedTransform transform;
		m_Listener.lookupTransform("/world", "VSV/Tool",
			m_MsgHeaderStamp, transform);
		m_WorldSpaceToolPosition = transform * tf::Vector3(0.0, 0.0, 0.0);

		// Get the current world space robot position
		m_Listener.waitForTransform("/world", "VSV/base",
			m_MsgHeaderStamp, ros::Duration(1.0));
		m_Listener.lookupTransform("/world", "VSV/base",
			m_MsgHeaderStamp, transform);
		m_WorldSpaceRobotPosition = transform * tf::Vector3(0.0, 0.0, 0.0);
		
		//ROS_INFO("Base position %f %f %f",
			//	m_WorldSpaceRobotPosition.x(), m_WorldSpaceRobotPosition.y(), m_WorldSpaceRobotPosition.z());

		// Get the orientation of the robot
		auto Q = transform.getRotation();
		tf::Matrix3x3 M;
		M.setRotation(Q);
		double roll, pitch;
		M.getRPY(roll, pitch, m_Orientation);
	}

//  void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
  void callback (const sensor_msgs::PointCloud2ConstPtr& pc, const sensor_msgs::ImageConstPtr& im)
  {
    if (!cloud_and_image_received_)
    {
      //Write cloud
      //transform to target frame
      bool found_transform = tf_.waitForTransform(pc->header.frame_id, to_frame_,
                                                  pc->header.stamp, ros::Duration(10.0));
      tf::StampedTransform transform;
      if (found_transform)
      {
        //ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame");
        tf_.lookupTransform(to_frame_,pc->header.frame_id, pc->header.stamp, transform);
        ROS_DEBUG("[TransformPointcloudNode:] Point cloud published in frame %s", pc->header.frame_id.c_str());
      }
      else
      {
        ROS_ERROR("No transform for pointcloud found!!!");
        return;
      }
      bag_.write(input_cloud_topic_, pc->header.stamp, *pc);
      geometry_msgs::TransformStamped transform_msg;
      tf::transformStampedTFToMsg(transform, transform_msg);
      bag_.write(input_cloud_topic_ + "/transform", transform_msg.header.stamp, transform_msg);
      ROS_INFO("Wrote cloud to %s", bag_name_.c_str());

      //Write image
      found_transform = tf_.waitForTransform(im->header.frame_id, to_frame_,
                                                  im->header.stamp, ros::Duration(10.0));
      if (found_transform)
      {
        //ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame");
        tf_.lookupTransform(to_frame_,im->header.frame_id, im->header.stamp, transform);
        ROS_DEBUG("[TransformPointcloudNode:] Point cloud published in frame %s", im->header.frame_id.c_str());
      }
      else
      {
        ROS_ERROR("No transform for image found!!!");
        return;
      }
      bag_.write(input_image_topic_, im->header.stamp, im);
      tf::transformStampedTFToMsg(transform, transform_msg);
      bag_.write(input_image_topic_ + "/transform", transform_msg.header.stamp, transform_msg);
      ROS_INFO("Wrote image to %s", bag_name_.c_str());

      cam_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(input_camera_info_topic_);
      bag_.write(input_camera_info_topic_, cam_info_->header.stamp, cam_info_);
      ROS_INFO("Wrote Camera Info to %s", bag_name_.c_str());
      cloud_and_image_received_ = true;
    }
  }

    bool checkObjectInMap(robot_msgs::checkObjectInMap::Request &req, robot_msgs::checkObjectInMap::Response &res) {
        int width = 16;

        geometry_msgs::PointStamped tf_object, obj;
        obj.point.x = req.point.x;
        obj.point.y = req.point.y;
        obj.header.frame_id = "camera";
        listener.waitForTransform("/camera", "/map", ros::Time(0), ros::Duration(1));
        listener.transformPoint("map", obj, tf_object);

        int x = floor(tf_object.point.x/resolution);
        int y = floor(tf_object.point.y/resolution);

        //ROS_INFO("COORDINATE OBJECT x: %d y: %d", x, y);
        for(int i = x-width/2; i <= x+width/2; i++)
        {
            //ROS_INFO("CHECK COORDINATE OBJECT x: %d", i);

            for(int j = y-width/2; j <= y+width/2; j++)
            {
                //ROS_INFO("Value %d", final_map[i + width_map*j]);
                if(final_map[i + width_map*j] == -106)
                {
                    res.inMap = true;
                    return true;
                }
            }
        }

        res.inMap = false;
        return true;
    }

 void doubleLaserCallback(const sensor_msgs::LaserScan& scan1, const sensor_msgs::LaserScan& scan2)
 {
   //transform both to the base frame, and republish this to the laser line extractor.
   tf::StampedTransform laser1_to_base;
   tf::StampedTransform laser2_to_base;
   try{
     tf_listener_->waitForTransform("/laser1", "/base_link",scan1.header.stamp, ros::Duration(0.6));
     tf_listener_->lookupTransform("/laser1","/base_link",scan1.header.stamp, laser1_to_base);
     tf_listener_->waitForTransform("/laser2", "/base_link",scan2.header.stamp, ros::Duration(0.6));
     tf_listener_->lookupTransform("/laser2","/base_link",scan2.header.stamp, laser2_to_base);
   } catch(tf::TransformException ex){
     ROS_ERROR("Laser Merge Callback failure: %s",ex.what());
   }
   
   
 }

Eigen::Matrix4f TrajectoryPublisher::getMatrixForTf(const tf::TransformListener& tfListener,
                                                    const std::string& target_frame, const std::string& source_frame,
                                                    const ros::Time& time)
{
  Eigen::Matrix4f eigenTransform;
  tf::StampedTransform tfTransform;

  try
  {
    ros::Time tm;
    std::string err;

    {
      if (tfListener.waitForTransform(target_frame, source_frame, time, ros::Duration(0.2f)))
      {
        tfListener.lookupTransform(target_frame, source_frame, time, tfTransform);
        pcl_ros::transformAsMatrix(tfTransform, eigenTransform);
      }
    }
  }
  catch (tf::TransformException ex)
  {
    ROS_WARN("transofrm error: %s", ex.what());
  }
  return eigenTransform;
}

void Explore::transFromOdomToMapPosition(float x_odom_pose, float y_odom_pose, float theta,
		float &x_map_pose, float &y_map_pose, tf::Quaternion& q){

	ros::Time now = ros::Time(0);
	tf::StampedTransform tfMO;								//	odom w map
	tf_listener_.waitForTransform("/map", "/odom", now, ros::Duration(1.0));
	tf_listener_.lookupTransform ("/map", "/odom", now,  tfMO);


	tf::Transform tfOD;

	tf::Quaternion quat;
	quat.setRPY(0, 0, theta);

	tfOD.setOrigin(tf::Vector3(x_odom_pose, y_odom_pose, 0.0));			//	docelowe w odom
	tfOD.setRotation( quat );

	tf::Transform tfMD = tfMO * tfOD;							//	docelowe w map


	x_map_pose = tfMD.getOrigin ()[0];				//	wspolrzedne docelowe w ukladzie map
	y_map_pose = tfMD.getOrigin ()[1];				//	wspolrzedne docelowe w ukladzie map
	q = tfMD.getRotation();
	
	//ROS_INFO("ODOM TO MAP x_odom = %f,  y_odom = %f, x_map = %f,  y_map = %f", x_odom_pose, y_odom_pose, x_map_pose, y_map_pose);
}

    void simulateOneCycle()
    {
	tf::StampedTransform transform;
        try{
	  listener.waitForTransform("map","EEframe", ros::Time(0), ros::Duration(1.0));
          listener.lookupTransform( "map","EEframe", ros::Time(0), transform);
        }
        catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
        }
	// This is correct, but does not work.
	// state_ = transform;

	KDL::Frame state_copy;
	tf::transformTFToKDL(state_, state_copy);

	nh_.getParamCached("/cds_controller_pr2/run_controller", run_controller_flag);
	if (run_controller_flag == 1.0){  // Controller running -> Update next desired position
	    tf::transformKDLToTF(cds_.update(state_copy), state_);
	} else {
	    state_ = transform;
	}

	broadcastTF();
    }

    bool getTransform(const std::string& refFrame,
                      const std::string& childFrame,
                      tf::StampedTransform& transform)
    {
        std::string errMsg;

        if ( !_tfListener.waitForTransform(refFrame,
                                           childFrame,
                                           ros::Time(0),
                                           ros::Duration(0.5),
                                           ros::Duration(0.01),
                                           &errMsg)
           )
        {
            ROS_ERROR_STREAM("Unable to get pose from TF: " << errMsg);
            return false;
        }
        else
        {
            try
            {
                _tfListener.lookupTransform( refFrame, childFrame,
                                             ros::Time(0),                  //get latest available
                                             transform);
            }
            catch ( const tf::TransformException& e)
            {
                ROS_ERROR_STREAM("Error in lookupTransform of " << childFrame << " in " << refFrame);
                return false;
            }

        }
        return true;
    }

void Node::laser_1_cb(const sensor_msgs::LaserScan::ConstPtr& scan_in)
{

  if(!listener_.waitForTransform("/laser1", "/world", scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment), ros::Duration(1.0))) {
    return;
  }

  sensor_msgs::PointCloud2 cloud;
  projector_.transformLaserScanToPointCloud("/world", *scan_in, cloud, listener_);

  pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
  pcl::fromROSMsg(cloud, pcl_cloud);

  crop_cloud(pcl_cloud, 1, scan_in->header.stamp);

  if (!starting_config && fabs(angle) < 0.1)
  {
    starting_config = 1;
    ROS_INFO("Ready to Scan.");
  }

  if (starting_config)
    cloud1 += pcl_cloud;

  sensor_msgs::PointCloud2 cloud_out;
  pcl::toROSMsg(cloud1, cloud_out);
  cloud_out.header = cloud.header;

  pc_1_pub_.publish(cloud_out);
}

void SpencerRobotReader::setRobotJointLocation(tf::TransformListener &listener, Joint* joint) {
    tf::StampedTransform transform;
    std::string jointId = "/";
    std::vector<double> jointOrientation;
    bg::model::point<double, 3, bg::cs::cartesian> jointPosition;
    jointId.append(joint->getName());
    try {
        ros::Time now = ros::Time::now();
        listener.waitForTransform("/map", jointId,
                now, ros::Duration(3.0));
        listener.lookupTransform("/map", jointId,
                now, transform);

        //Joint position
        jointPosition.set<0>(transform.getOrigin().x());
        jointPosition.set<1>(transform.getOrigin().y());
        jointPosition.set<2>(transform.getOrigin().z());

        //Joint orientation
        //curRobot->orientation.push_back(tf::getRoll(transform.getRotation()));
        //curRobot->orientation.push_back(tf::getPitch(transform.getRotation()));
        jointOrientation.push_back(0.0);
        jointOrientation.push_back(0.0);
        jointOrientation.push_back(tf::getYaw(transform.getRotation()));

        joint->setTime(now.toNSec());
        joint->setPosition(jointPosition);
        joint->setOrientation(jointOrientation);

    } catch (tf::TransformException ex) {
        ROS_ERROR("%s", ex.what());
    }
}

void LaserscanVirtualizer::virtual_laser_scan_parser()
{
	// LaserScan frames to use for virtualization
    istringstream iss(virtual_laser_scan);
	vector<string> tokens;
	copy(istream_iterator<string>(iss), istream_iterator<string>(), back_inserter<vector<string> >(tokens));

	vector<string> tmp_output_frames;

	for(int i=0;i<tokens.size();++i)
	{
		ros::Time beg = ros::Time::now();
        if(tfListener_.waitForTransform (base_frame, tokens[i] ,ros::Time(0),ros::Duration(1.0))) // Check if TF knows the transform from this frame reference to base_frame reference
		{
			cout << "Elapsed: " << ros::Time::now() - beg << endl;
            cout << "Adding: " << tokens[i]  << endl;
			tmp_output_frames.push_back(tokens[i]);
		}
		else
            cout << "Can't transform: '" << tokens[i] + "' to '" << base_frame << "'" << endl;
	}

	// Sort and remove duplicates
	sort(tmp_output_frames.begin(),tmp_output_frames.end());
	std::vector<string>::iterator last = std::unique(tmp_output_frames.begin(), tmp_output_frames.end());
	tmp_output_frames.erase(last, tmp_output_frames.end());

	// Do not re-advertize if the topics are the same
	if( (tmp_output_frames.size() != output_frames.size()) || !equal(tmp_output_frames.begin(),tmp_output_frames.end(),output_frames.begin()) )
	{
		// Shutdown previous publishers
		for(int i=0; i<virtual_scan_publishers.size(); ++i)
			virtual_scan_publishers[i].shutdown();

		cloud_frame = "";

		output_frames = tmp_output_frames;
		if(output_frames.size() > 0)
		{
            virtual_scan_publishers.resize(output_frames.size());
            ROS_INFO("Publishing: %ld virtual scans", virtual_scan_publishers.size() );
            cout << "Advertising topics: " << endl;
			for(int i=0; i<output_frames.size(); ++i)
			{
                if (output_laser_topic.empty())
                {
                    virtual_scan_publishers[i] = node_.advertise<sensor_msgs::LaserScan> (output_frames[i].c_str(), 1);
                    cout << "\t\t" << output_frames[i] << " on topic " << output_frames[i].c_str() << endl;
                }
                else
                {
                    virtual_scan_publishers[i] = node_.advertise<sensor_msgs::LaserScan> (output_laser_topic.c_str(), 1);
                    cout << "\t\t" << output_frames[i] << " on topic " << output_laser_topic.c_str() << endl;
                }
			}
		}
		else
            ROS_INFO("Not publishing to any topic.");
	}
}

  void commandForce(const geometry_msgs::WrenchStamped::ConstPtr &msg)
  {
    F_des_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, 
              msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z;
    if(msg->header.frame_id == root_name_)
      return;

    geometry_msgs::PoseStamped in_root;
    in_root.pose.orientation.w = 1.0;
    in_root.header.frame_id = msg->header.frame_id;

    try {
      tf_.waitForTransform(root_name_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
      tf_.transformPose(root_name_, in_root, in_root);
    }
    catch (const tf::TransformException &ex)
    {
      ROS_ERROR("Failed to transform: %s", ex.what());
      return;
    }
    
    Eigen::Affine3d t;
    
    tf::poseMsgToEigen(in_root.pose, t);

    F_des_.head<3>() = t.linear() * F_des_.head<3>();
    F_des_.tail<3>() = t.linear() * F_des_.tail<3>();
  }

static int getHumanLocation(tf::TransformListener& listener){
    tf::StampedTransform transform;
    try{
        ros::Time now = ros::Time::now();
        listener.waitForTransform("/map", "/human_base",
                              now, ros::Duration(3.0));
        listener.lookupTransform("/map", "/human_base",
                               now, transform);
        
        humanConf.dof[0] = transform.getOrigin().x();
        humanConf.dof[1] = transform.getOrigin().y();
        humanConf.dof[2] = transform.getOrigin().z()+1.0;
        humanConf.dof[3] = 0.0;
        humanConf.dof[4] = 0.0;
        humanConf.dof[5] = tf::getYaw(transform.getRotation());
        

        if(humanPose_DEBUG){
          printf("%f %f %f %f %f %f\n", transform.getOrigin().x(), transform.getOrigin().y(), 
                transform.getOrigin().z(), humanConf.dof[3], humanConf.dof[4], humanConf.dof[5]);
        }
    }
    catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
    }
  return TRUE;
}

    void receiveInformation(const geometry_msgs::PointStampedConstPtr &person)
    {

        notReceivedNumber = 0;
        hold = false;

        try
        {

            listener.waitForTransform(robot_frame, person->header.stamp, person->header.frame_id, person->header.stamp, fixed_frame_id, ros::Duration(10.0) );
            listener.transformPoint(robot_frame, person->header.stamp, *person, fixed_frame_id, personInRobotBaseFrame);

            cv::Point3d personPoint;
            personPoint.x = personInRobotBaseFrame.point.x;
            personPoint.y = personInRobotBaseFrame.point.y;
            personPoint.z = personInRobotBaseFrame.point.z;

            distanceToPerson = cv::norm(personPoint);
            ROS_ERROR("Person distance: %f", distanceToPerson);
        }
        catch(tf::TransformException ex)
        {
            ROS_WARN("%s",ex.what());
            //ros::Duration(1.0).sleep();
            return;
        }

        personInRobotBaseFrame.header.frame_id = robot_frame;
    }