C++ tf::StampedTransform类代码示例

	bool poseTracker::newPose(tf::StampedTransform transform){
		
		Eigen::Vector3f curr(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
		pose.push(curr);
		if(pose.size() < 50){
			return true;
		}

		float squaredNorm = (pose.front() - curr).squaredNorm();
		pose.pop();
		bool tmp = squaredNorm > 0.2;
		std::cout << tmp << "  Max: " << squaredNorm << std::endl;
		return squaredNorm > 0.2;

	}

// compute nav fn (using wavefront assumption)
void getNavFn(double* cost_map, tf::StampedTransform &robot_to_map_transform, double* nav_fn) {
  Point robot_pt(robot_to_map_transform.getOrigin().getX() / MAP_RESOLUTION,robot_to_map_transform.getOrigin().getY() / MAP_RESOLUTION);
  Point goal_pt(goal_pose.getOrigin().getX() / MAP_RESOLUTION, goal_pose.getOrigin().getY() / MAP_RESOLUTION);
  ROS_INFO("[getNavFn]\trobot_pt: (%d,%d), goal_pt(%d,%d)",robot_pt.x,robot_pt.y,goal_pt.x,goal_pt.y);

  // setup init map
  bool init_set_map[MAP_CELLS];
  for (int i=0; i<MAP_CELLS;i++) {
    init_set_map[i] = false;
  }
  setVal(init_set_map,goal_pt.x,goal_pt.y,true);

  //updateMapDijkstra(nav_fn, init_set_map, goal_pt, cost_map, INT_MAX);
  LPN2(nav_fn, cost_map, goal_pt, robot_pt);
}

bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap)
{
  size_t size = pointCloud.points.size();
  
  dataContainer.clear();
  
  tf::Vector3 laserPos (laserTransform.getOrigin());
  dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y())*scaleToMap);
  
  for (size_t i = 0; i < size; ++i)
    {
      
      const geometry_msgs::Point32& currPoint(pointCloud.points[i]);
      
      float dist_sqr = currPoint.x*currPoint.x + currPoint.y* currPoint.y;
      
      if ( (dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_) ){
	
	if ( (currPoint.x < 0.0f) && (dist_sqr < 0.50f)){
	  continue;
	}
	
	tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
	
	float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();
	
	if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
	  {
	    dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(),pointPosBaseFrame.y())*scaleToMap);
	  }
      }
    }
  
  return true;
}

/**
 * Convert tf::StampedTransform to string
 */
template<> std::string toString<tf::StampedTransform>(const tf::StampedTransform& p_transform)
{
  std::stringstream ss;

  ss << "StampedTransform(Quaternion=" << toString(p_transform.getRotation()) << "; Vector3=" << toString(p_transform.getOrigin()) << ")";

  return ss.str();
}

 void poseStampedToStampedTF(geometry_msgs::PoseStamped &msg, tf::StampedTransform &stampedTF, std::string child)
 {
   tf::Transform btTrans;
   stampedTF.stamp_ = msg.header.stamp;
   stampedTF.frame_id_ = msg.header.frame_id;
   stampedTF.child_frame_id_ = child;
   
   tf::poseMsgToTF(msg.pose, btTrans);
   stampedTF.setData(btTrans);
 }

void StigmergyPlanner::makeViewFacingMarker(tf::StampedTransform robot_pose)
{
  int_marker.header.frame_id = robot_pose.frame_id_;
  int_marker.header.stamp = robot_pose.stamp_;

  int_marker.pose.position.x = robot_pose.getOrigin().getX();
  int_marker.pose.position.y = robot_pose.getOrigin().getY();
  int_marker.pose.position.z = robot_pose.getOrigin().getZ();
  int_marker.scale = 1;

  int_marker.name = "view_facing";
  int_marker.description = "3D Planning pencil";

  InteractiveMarkerControl control;

  // make a control that rotates around the view axis
  //control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
  //control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
  //control.orientation.w = 1;
  //control.name = "rotate";

  //int_marker.controls.push_back(control);

  // create a box in the center which should not be view facing,
  // but move in the camera plane.
  control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
  control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
  control.independent_marker_orientation = true;
  control.name = "move";

  control.markers.push_back( makeBox(int_marker) );
  control.always_visible = true;

  int_marker.controls.push_back(control);

  control.interaction_mode = InteractiveMarkerControl::MENU;
  control.name = "menu";
  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, boost::bind(&StigmergyPlanner::processFeedback,this,_1));
  menu_handler.apply(*server, int_marker.name);
}

inline bool checkTF(tf::StampedTransform& transform, geometry_msgs::PoseStamped& robo,
					tf::TransformListener& listener)
{
	try{
		//listener.waitForTransform(header_frame, robot_frame, ros::Time(0), ros::Duration(1.0));
	    listener.lookupTransform(header_frame, robot_frame, ros::Time(0), transform);
        //copy robot position
        robo.pose.position.x=transform.getOrigin().x();
        robo.pose.position.y=transform.getOrigin().y();
        robo.pose.position.z=0;
        float angle = tf::getYaw(transform.getRotation());
        robo.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,angle);
		return true;
	}
	catch (tf::TransformException ex){
		// cout<<"waiting for global and robot frame relation"<<endl;
        ROS_ERROR("%s",ex.what());
		return false;
	}
}

void sptam::sptam_node::fixOdomFrame(const CameraPose& cameraPose, const tf::StampedTransform& camera_to_odom, const ros::Time& t)
{
  tf::Pose camera_to_map;
  CameraPose2TFPose( cameraPose, camera_to_map );

  // compute the new difference between map and odom
  tf::Transform odom_to_map = camera_to_map * camera_to_odom.inverse();

  std::lock_guard<std::mutex> lock( odom_to_map_mutex_ );
  odom_to_map_ = odom_to_map;
}

bool
WmObjectCapture::updateObjectFrame(const ros::Time& stamp, tf::StampedTransform& m2c)
{
	std::vector<tf::StampedTransform> valids;

	getValidMarks(valids, stamp);
	if(valids.size()<4)
	{
		//ROS_INFO("No enough marks detected [%zu]", valids.size());
		return false;
	}

	tf::Transform media;
	double stdev;
	getBestTransform(valids, media, stdev);

	double m_tolerance2 = 0.006*0.006;
	if(stdev<m_tolerance2)
	{
		//ROS_INFO("TRANSFORM ACCEPTED ");
		m2c.child_frame_id_ = objectFrameId_;
		m2c.frame_id_ = cameraFrameId_;
		m2c.stamp_ = ros::Time::now();

		m2c.setOrigin(media.getOrigin());
		m2c.setRotation(media.getRotation());

		ROS_DEBUG("NEW FINAL (%lf, %lf, %lf) (%lf, %lf, %lf, %lf)",
				m2c.getOrigin().x(), m2c.getOrigin().y(), m2c.getOrigin().z(),
				m2c.getRotation().x(), m2c.getRotation().y(), m2c.getRotation().z(), m2c.getRotation().w());

		try
		{
			tfBroadcaster_.sendTransform(m2c);

		}catch(tf::TransformException & ex)
		{
			ROS_WARN("WmObjectCapture::updateObjectFrame %s",ex.what());
		}
		return true;
	}

	//ROS_INFO("TRANSFORM REJECTED ");
	return false;

}

void InputOutputLinControl::odomMsgToStampedTransformB(double displacement, double yaw, nav_msgs::Odometry pose_odometry, tf::StampedTransform& pose_stamped)
{
	pose_stamped.stamp_ = pose_odometry.header.stamp;
	pose_stamped.frame_id_ = pose_odometry.header.frame_id;
	pose_stamped.child_frame_id_ = pose_odometry.child_frame_id;

	tf::Vector3 v;
	v.setX(pose_odometry.pose.pose.position.x + displacement*cos(yaw));
	v.setY(pose_odometry.pose.pose.position.y + displacement*sin(yaw));
	v.setZ(pose_odometry.pose.pose.position.z);

	pose_stamped.setOrigin(v);

	tf::Quaternion q;
	q.setX(pose_odometry.pose.pose.orientation.x);
	q.setY(pose_odometry.pose.pose.orientation.y);
	q.setZ(pose_odometry.pose.pose.orientation.z);
	q.setW(pose_odometry.pose.pose.orientation.w);

	pose_stamped.setRotation(q);
}

  void foutAppendTF(tf::StampedTransform &transform, std::ofstream &fout)
  {
    tf::Matrix3x3 rotMatrix = transform.getBasis();
    tf::Vector3 originVector = transform.getOrigin();

    
    tfScalar roll, pitch, yaw;
    
    
    
    
    rotMatrix.getRPY(roll, pitch, yaw);
    
    fout<< originVector.x() << " "
        << originVector.y() << " "
        << originVector.z() << " "
        << pcl::rad2deg(roll) << " "
        << pcl::rad2deg(pitch) << " "
        << pcl::rad2deg(yaw) << std::endl;
    
    
  }

bool InputOutputLinControl::updateRobotPoseByModel(double timestep, double linear_vel, double angular_vel, tf::StampedTransform& pose)
{

	pose.stamp_ = ros::Time::now();
	double roll, pitch, yaw;
	pose.getBasis().getRPY(roll,pitch,yaw);

	tf::Vector3 v;

	double theta = yaw + angular_vel * timestep;

	//Eulerian integration
	v.setX(pose.getOrigin().getX() + linear_vel * timestep * cos(theta));
	v.setY(pose.getOrigin().getY() + linear_vel * timestep * sin(theta));
	v.setZ(pose.getOrigin().getZ());

	pose.setOrigin(v);
	pose.setRotation(tf::createQuaternionFromYaw(theta));
	ROS_INFO("New orientation [%f]",theta);

	return true;
}

  void printTF(tf::StampedTransform &transform, std::string str)
  {
    tf::Matrix3x3 rotMatrix = transform.getBasis();
    tf::Vector3 originVector = transform.getOrigin();

    
    tfScalar roll, pitch, yaw;
    
    
    
    
    rotMatrix.getRPY(roll, pitch, yaw);
    
    
    ROS_INFO("TF %s (x y z r p y): %f %f %f %f %f %f", 
             str.c_str(),
             originVector.x(),
             originVector.y(), 
             originVector.z(),
             pcl::rad2deg(roll),
             pcl::rad2deg(pitch),
             pcl::rad2deg(yaw));
  }

void addRead(string hexID, int rssi, tf::StampedTransform trans) {
    if (rssi == -1)  //The tags sometimes see each other; don't record this
        return;
    //Check to see if this tag has been seen yet
    if (heatmaps.find(hexID) == heatmaps.end()) {
        heatmaps[hexID] = PointCloud();
        heatmapPoses[hexID] = PoseArray();
        ChannelFloat32 rgbChannel;
        rgbChannel.name = "rgb";
        heatmaps[hexID].channels.push_back(rgbChannel);
        heatmaps[hexID].header.frame_id = "/map";
        heatmapSelected[hexID] = true;
    }
    double x = trans.getOrigin().x();
    double y = trans.getOrigin().y();
    double z = trans.getOrigin().z();
    Point32 P32;
    P32.x = x;P32.y = y;P32.z = z;
    Point P;
    P.x = x;P.y = y;P.z = z;
    btQuaternion rotation = trans.getRotation();
    Quaternion Q;
    Q.x = rotation.x();Q.y = rotation.y();Q.z = rotation.z();Q.w = rotation.w();
    Pose pose;
    pose.position = P;
    pose.orientation = Q;
    float float_rgb;
    if (rssi > 10) {
        double l = ((double)rssi + 50.0) / 200.0;
        int rgb = getColorHSL(20.0, 240.0, l, rssi);
        float_rgb = *reinterpret_cast<float*>(&rgb);
    }
    heatmaps[hexID].points.push_back(P32);
    //Color of point in RGB channel
    heatmaps[hexID].channels[0].values.push_back(float_rgb);
    heatmapPoses[hexID].poses.push_back(pose);
}

carmen_6d_point 
get_carmen_pose6D_from_matrix(Matrix visual_odometry_transformation_matrix)
{
//	// See: http://www.ros.org/wiki/tf
	tf::Transform visual_odometry_pose;
	tf::Transform carmen_pose;
	carmen_6d_point carmen_pose_6d;

	// convert the visual odometry output matrix to the tf::Transform type.
	visual_odometry_pose = convert_matrix_to_tf_transform(visual_odometry_transformation_matrix);

	// compute the current visual odometry pose with respect to the carmen coordinate system
	carmen_pose = g_car_to_visual_odometry_transform * visual_odometry_pose * g_car_to_visual_odometry_transform.inverse();

	double yaw, pitch, roll;
	tf::Matrix3x3(carmen_pose.getRotation()).getRPY(roll, pitch, yaw);

	carmen_pose_6d.x = carmen_pose.getOrigin().x();
	carmen_pose_6d.y = carmen_pose.getOrigin().y();
	carmen_pose_6d.z = carmen_pose.getOrigin().z();

	carmen_pose_6d.yaw = yaw;
	carmen_pose_6d.pitch = pitch;
	carmen_pose_6d.roll = roll;

	 return carmen_pose_6d;
}