C++ Transform::setRotation方法代码示例

void PublishTransform(ros::Time stamp, float fX, float fY, float fZ, float fYaw, float fPitch, float fRoll)
{
    static tf::TransformBroadcaster tfBroadcaster;
    static tf::Transform transform; 

    //from world to vehile;
    transform.setOrigin(tf::Vector3(fX, fY, fZ));
    transform.setRotation(tf::Quaternion(fYaw, fPitch, fRoll));
    tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/world", "/vehicle"));

    //from vehile to lms1;
    transform.setOrigin(tf::Vector3(1.26, 0.485, 2.196));
    //transform.setRotation(tf::Quaternion(0.0125+0.0026+0.0034, 0.183011, 0.0+0.0017*7));//roll, pitch, yaw
    transform.setRotation(tf::Quaternion(0.0, 0.183, 0.0));//roll, pitch, yaw
    tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/lms1"));

    //from vehicle to lms2;
    transform.setOrigin(tf::Vector3(1.26, -0.467, 2.208));
    //transform.setRotation(tf::Quaternion(0.0125003, 0.142386, 6.27694+0.0017*5));
    transform.setRotation(tf::Quaternion(0.0, 0.142386, 0.0));
    tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/lms2"));

    //from vehicle to velodyne1;
    transform.setOrigin(tf::Vector3(1.147, 0.477, 2.405));
    //transform.setRotation(tf::Quaternion(0.0, 0.0017, 0.0));  //yaw, pitch, roll
    transform.setRotation(tf::Quaternion(0.0, 0.0, 0.0));  //yaw, pitch, roll
    tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/velodyne1"));

    //from vehicle to velodyne2;
    transform.setOrigin(tf::Vector3(1.152, -0.445,2.45));
    //transform.setRotation(tf::Quaternion(6.28006,0.000175, 0.0));
    transform.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
    tfBroadcaster.sendTransform(tf::StampedTransform(transform, stamp, "/vehicle", "/velodyne2"));

}

void userTracker::loadTfTransformFromFile(string file, tf::Transform &transform) {
    ifstream in(file.data());
    if (!in) {
        cout << "No file found: " << file << endl;

        transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
        transform.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));

        return;
    }

    int columns = 6;
    double tmpVec[6];

    for (int j = 0; j < columns; j++) {
        in >> tmpVec[j];
    }
    in.close();

    // translation
    transform.setOrigin(tf::Vector3(tmpVec[0], tmpVec[1], tmpVec[2]));

    // rotation vector (Rodriguez)
    tf::Vector3 tmpTrans(tmpVec[3], tmpVec[4], tmpVec[5]);
    tf::Quaternion tmpQuat;
    if(tmpTrans.length() > 1e-6)
        tmpQuat.setRotation(tmpTrans.normalized(), tmpTrans.length());
    else {
        tmpQuat.setX(0.); tmpQuat.setY(0.); tmpQuat.setZ(0.); tmpQuat.setW(1.);
    }

    transform.setRotation(tmpQuat);

    return;
}

void imuCallback(const boost::shared_ptr<const sensor_msgs::Imu>& msg)
{
    //imu_msg = *msg;

    geometry_msgs::PoseStamped pose;
    pose.header.frame_id = msg->header.frame_id;
    pose.header.stamp = msg->header.stamp;
    pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);

    geometry_msgs::PoseStamped imu_pose;
            
    try 
    {
        tf_listener->transformPose(odom_frame_id, pose, imu_pose);
    }
    catch(tf::TransformException &ex) 
    {
        ROS_ERROR("Squirtle Localization - %s - Error: %s", __FUNCTION__, ex.what());
        return;
    }
    
    tf::Quaternion q_imu;

    tf::quaternionMsgToTF(msg->orientation, q_imu);
    t_world_imu.setOrigin(tf::Vector3(0.0, 0.0, 0.0));

    tf::Quaternion temp = q_imu * tf::createQuaternionFromYaw(M_PI/2.0 + true_north_compensation);
    t_world_imu.setRotation(temp.normalized());
    
    ROS_INFO("Squirtle Localization - %s - IMU yaw in UTM - yaw:%lf", __FUNCTION__, tf::getYaw(msg->orientation) + M_PI/2.0 + true_north_compensation);

    tf::quaternionMsgToTF(imu_pose.pose.orientation, q_imu);
    t_odom_imu.setOrigin(tf::Vector3(imu_pose.pose.position.x, imu_pose.pose.position.y, imu_pose.pose.position.z));
    t_odom_imu.setRotation(q_imu);
}

void gpsCallback(const boost::shared_ptr<const sensor_msgs::NavSatFix>& msg)
{
    //gps_msg = *msg;

    geometry_msgs::PoseStamped pose;
    pose.header.frame_id = msg->header.frame_id;
    pose.header.stamp = msg->header.stamp;
    pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);

    geometry_msgs::PoseStamped gps_pose;
            
    try 
    {
        tf_listener->transformPose(odom_frame_id, pose, gps_pose);
    }
    catch(tf::TransformException &ex) 
    {
        ROS_ERROR("Squirtle Localization - %s - Error: %s", __FUNCTION__, ex.what());
        return;
    }

    double x, y;
    int zone_number; char zone_letter;
    GPStoUTM(msg->latitude, msg->longitude, y, x, zone_number, zone_letter);
    t_world_gps.setOrigin(tf::Vector3(x, y, 5.0));
    t_world_gps.setRotation(tf::createQuaternionFromYaw(0.0));
    
    ROS_INFO("Squirtle Localization - %s - GPS position in UTM - x:%lf y:%lf", __FUNCTION__, x, y);

    t_odom_gps.setOrigin(tf::Vector3(gps_pose.pose.position.x, gps_pose.pose.position.y, gps_pose.pose.position.z));
    tf::Quaternion q_gps;
    tf::quaternionMsgToTF(gps_pose.pose.orientation, q_gps);
    t_odom_gps.setRotation(q_gps);
}

    Geometric_semantics_component_tutorial_publisher(string const& name)
        : TaskContext(name)
        , object_PoseCoordinatesSemantics("a","a","A","b","b","B","b")
        , xpos(0)
        , ypos(0)
        , zpos(0)
    {
        object_PoseCoordinatesSemantics = PoseCoordinatesSemantics("e1","e1","E1","b1","b1","B1","b1");

        transform.setOrigin( tf::Vector3(1,2,3) );
        transform.setRotation(tf::createQuaternionFromRPY(0.2, 0, 0) );
        object_coordinates_Pose.setOrigin( tf::Vector3(1,2, 0.0) );
        object_coordinates_Pose.setRotation( tf::createQuaternionFromRPY(0.75, 0, 0) );

        //geometric_semantics::Pose<tf::Pose> object_PoseCoordinatesTF(conversions_geometric_msgs::PoseTFFromMsg(*msg))
        object_PoseTF = geometric_semantics::Pose<tf::Pose> (object_PoseCoordinatesSemantics, object_coordinates_Pose);

        propPose= conversions_geometric_msgs::PoseTFToMsg(object_PoseTF);

        /****************************
          Pose
         **************************/
        // Pose
        this->addPort( "outPortPose", outPortPose).doc( "Output Port for Pose." );
        this->addProperty( "propPose", propPose).doc( "Property for Pose." );

        log(Debug) << "Geometric_semantics_component_tutorial_publisher constructed !" <<endlog();

    }

void
fromPose(const geometry_msgs::Pose &source, tf::Transform &dest)
{
    tf::Quaternion q(source.orientation.x, source.orientation.y, source.orientation.z, source.orientation.w);
    dest.setOrigin(tf::Vector3(source.position.x, source.position.y, source.position.z));
    dest.setRotation(q);
}

void publish_faro_transform(tf::Transform faro_base)
{
  static tf::TransformBroadcaster br;

  //tf::Transform faro_base;

  //base_at_table - Actual,Circle,-291.052543573765,-100.393272630778,213.8235
  tf::Vector3 faro_origin(213.8235, 291.052543573765, -100.393272630778);
  faro_origin = faro_origin / 1000;
  faro_origin.setZ(faro_origin.z() + 0.10);
  faro_base.setOrigin(faro_origin);

  //+X_Coordinate System 1.i;0.9495;+Y_Coordinate System 1.i;0.3139;+Z_Coordinate System 1.i;-0.0008;
  //+X_Coordinate System 1.j;0.0007;+Y_Coordinate System 1.j;0.0005;+Z_Coordinate System 1.j;1.0000;
  //+X_Coordinate System 1.k;0.3139;+Y_Coordinate System 1.k-0.9495;+Z_Coordinate System 1.k;0.0002;
  tf::Matrix3x3 faro_rot;
  //faro_rot.setValue(0.9495, 0.3139, -0.0008, 0.0007, 0.0005, 1.0000, 0.3139, -0.9495, 0.0002);
  faro_rot.setValue(0.9495, 0.0007, 0.3139, 0.3139, 0.0005, -0.9495, -0.0008, 1.0000, 0.0002);
  tf::Quaternion faro_quat;
  double roll, pitch, yaw;
  faro_rot.getEulerYPR(yaw, pitch, roll);
  faro_quat.setRPY(roll, pitch, yaw);

  faro_base.setRotation(faro_quat);

  br.sendTransform(tf::StampedTransform(faro_base, ros::Time::now(), world_frame_, "faro_base"));

}

    SimObjectListener(std::string pr2_label)
    {
      pr2_transform.model_name = pr2_label;
      sim_manipulator.getModelPose(pr2_transform);
      pr2_gz_tf.setOrigin(tf::Vector3(
            pr2_transform.pose.position.x,
            pr2_transform.pose.position.y,
            pr2_transform.pose.position.z
            ));
      pr2_gz_tf.setRotation(tf::Quaternion(
          pr2_transform.pose.orientation.x,
          pr2_transform.pose.orientation.y,
          pr2_transform.pose.orientation.z,
          pr2_transform.pose.orientation.w
          ));

      ROS_INFO("%s at: [%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f,%4.2f]",
          pr2_transform.model_name.c_str(),
          pr2_transform.pose.position.x,
          pr2_transform.pose.position.y,
          pr2_transform.pose.position.z,
          pr2_transform.pose.orientation.x,
          pr2_transform.pose.orientation.y,
          pr2_transform.pose.orientation.z,
          pr2_transform.pose.orientation.w
      );
    }

// Subscriber functions
void PoseMasterWSCallback(const geometry_msgs::PoseStamped& msg){
  
  Tm.setOrigin(tf::Vector3(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z));
  Tm.setRotation(tf::Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w));
  
  if (!init)	init = true;
}

void PSMNode::pose2DToTf(const geometry_msgs::Pose2D& pose, tf::Transform& t)
{
  t.setOrigin(tf::Vector3(pose.x, pose.y, 0.0));
  tf::Quaternion q;
  q.setRPY(0, 0, pose.theta);
  t.setRotation(q);
}

void TeleopMaximus::poseCallback(const geometry_msgs::PoseStamped::ConstPtr & pose)
{

    temp_pose.pose.position.x = pose->pose.position.x;
    temp_pose.pose.position.y = pose->pose.position.y;
    temp_pose.pose.orientation.x = pose->pose.orientation.x;
    temp_pose.pose.orientation.y = pose->pose.orientation.y;
    temp_pose.pose.orientation.z = pose->pose.orientation.z;
    temp_pose.pose.orientation.w = pose->pose.orientation.w;


    my_maximus_path.header.stamp = ros::Time::now();
    if (my_maximus_path.poses.std::vector < geometry_msgs::PoseStamped >::size() >
        (my_maximus_path.poses.std::vector < geometry_msgs::PoseStamped >::max_size() - 2)) {
        my_maximus_path.poses.std::vector < geometry_msgs::PoseStamped >::pop_back();
    }
    my_maximus_path.poses.std::vector < geometry_msgs::PoseStamped >::push_back(*pose);

    transform.setOrigin(tf::Vector3(pose->pose.position.x, pose->pose.position.y, 0.0));
    transform.setRotation(tf::Quaternion(pose->pose.orientation.x, pose->pose.orientation.y, pose->pose.orientation.z, pose->pose.orientation.w));
    //br.sendTransform(tf::StampedTransform(transform, pose->header.stamp, "/map", "/between_wheels"));
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/map", "/between_wheels"));

    i = i + 5;
    if (i > 180)
        i = -180;
    ROS_INFO("X=%f /Y=%f /T=%f /L=%f", pose->pose.position.x, pose->pose.position.y, pose->pose.orientation.z * 180 / 3.1415, linear_value);

    TeleopMaximus::path_in_map_pub.publish(my_maximus_path);

}

void inline create_transform(tf::Transform &tf, float tx, float ty, float tz, float roll, float pitch, float yaw){
   tf::Quaternion q;
   q.setRPY(roll, pitch, yaw);

   tf.setOrigin(tf::Vector3( tx, ty, tz));
   tf.setRotation(q);
}

void
load_localize_transform()
{
	std::ifstream filed_transform;
	std::stringstream file_name;

	std::string path = ros::package::getPath("prx_localizer");
	file_name << path << "/transform/localize_transform.bin";
    filed_transform.open(file_name.str().c_str(), std::ofstream::binary);
    if (filed_transform.is_open())
    {
		tfScalar x, y, z, w;
		filed_transform.read((char *) &x, sizeof(tfScalar));
		filed_transform.read((char *) &y, sizeof(tfScalar));
		filed_transform.read((char *) &z, sizeof(tfScalar));
		filed_transform.read((char *) &w, sizeof(tfScalar));
		g_localize_transform.setRotation(tf::Quaternion(x, y, z, w));

		filed_transform.read((char *) &x, sizeof(tfScalar));
		filed_transform.read((char *) &y, sizeof(tfScalar));
		filed_transform.read((char *) &z, sizeof(tfScalar));
		g_localize_transform.setOrigin(tf::Vector3(x, y, z));

		filed_transform.close();
    }
    else
    	g_localize_transform = tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3(0.0, 0.0, 0.0));

    g_localize_transform_timestamp = ros::Time(0);
}

void TeleopMaximus::get_value_and_do_computation(void) {
	float rotation = 0.0;

	if(ser_fd != -1) {
		my_maximus_pose.pose.position.x = ((float)TeleopMaximus::read_serial_port('x')) / 10000.0;
		my_maximus_pose.pose.position.y = ((float)TeleopMaximus::read_serial_port('y')) / 10000.0;
		rotation =  TeleopMaximus::read_serial_port('t');
		TeleopMaximus::rotate(0, (( (float)rotation) / 10000.0 ), 0, &my_maximus_pose);

		// reset laser scan
		//                      my_laser_scan.ranges.std::vector<float>::clear();
		// set the new values
		//my_laser_scan.ranges.std::vector<float>::push_back(((float)read_serial_port('l')) / 1000.0);
		//my_laser_scan.ranges.std::vector<float>::push_back(((float)read_serial_port('m')) / 1000.0);
		//my_laser_scan.ranges.std::vector<float>::push_back(((float)read_serial_port('r')) / 1000.0);

	}
	else {
		//TeleopMaximus::rotate(0, ((i)*3.1415/180), 0, &my_maximus_pose);
		//rotation = -((i)*3.1415/180) *10000;
		//my_maximus_pose.pose.position.y = ((float)i)/40;
		heading -= (angular_value / 120000 );
		rotation = heading *10000;
		TeleopMaximus::rotate(0, -(heading), 0, &my_maximus_pose);

		my_maximus_pose.pose.position.x += (linear_value * cos(-heading) / 100000);
		my_maximus_pose.pose.position.y += (linear_value * sin(-heading) / 100000);

	}

	// Actualize Robot's position
	my_maximus_pose.header.stamp = ros::Time::now();

	// Actualize Robot's path
	my_maximus_path.header.stamp = ros::Time::now();
	if(my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::size() > (my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::max_size()-2)) {
		my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::pop_back();
	}
	my_maximus_path.poses.std::vector<geometry_msgs::PoseStamped>::push_back(my_maximus_pose);

	marker.lifetime = ros::Duration();

	// Publish the marker
	//marker.header.stamp = ros::Time::now();
	//marker_pub.publish(marker);

	transform.setOrigin( tf::Vector3(my_maximus_pose.pose.position.x, my_maximus_pose.pose.position.y, 0.0) );
	transform.setRotation( tf::Quaternion((( (float)rotation) / 10000.0 ), 0, 0) );
	br.sendTransform(tf::StampedTransform(transform, my_maximus_pose.header.stamp, "/map", "/maximus_robot_tf"));

	marker.header.stamp = ros::Time::now();

	my_laser_scan.header.stamp = ros::Time::now();

	i = i + 5;
	if( i > 180)
		i = -180;
	ROS_INFO("X=%f /Y=%f /T=%f /L=%f", my_maximus_pose.pose.position.x, my_maximus_pose.pose.position.y, my_maximus_pose.pose.orientation.z*180/3.1415, linear_value);

}

void createTfFromXYTheta(
		double x, double y, double theta, tf::Transform& t)
{
	t.setOrigin(btVector3(x, y, 0.0));
	tf::Quaternion q;
	q.setRPY(0.0, 0.0, theta);
	t.setRotation(q);
}