C++ TransformBroadcaster::sendTransform方法代码示例

/* This is just for reference
void poseCallback(const turtlesim::PoseConstPtr& msg){
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
*/
int main(int argc, char** argv){
    ros::init(argc, argv, "kinect_broadcaster");
    

    ros::NodeHandle node;
    ROS_INFO("Started kinect_broadcaster node");
    ros::Rate rate(10.0);
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(0,0,0));
    tf::Quaternion q;
    q.setRPY(0,0,0);
    transform.setRotation(q);

    static tf::TransformBroadcaster world_openni;
    tf::Transform transform2;
    transform2.setOrigin(tf::Vector3(0,0,0));
    tf::Quaternion q2;
    q2.setRPY(0,0,0);
    transform2.setRotation(q);

    while (node.ok()){
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),"world","camera_link"));
	world_openni.sendTransform(tf::StampedTransform(transform2,
		    ros::Time::now(),"world","openni_depth_frame"));
	rate.sleep();
    
    }
    return 0;
};

    // now that we are calibrated, we are free to send the transforms:
    void send_frames(void)
	{
	    ROS_DEBUG("Sending frames");
	    tf::Transform transform;

	    transform.setOrigin(tf::Vector3(cal_pos(0),
					    cal_pos(1),
					    cal_pos(2)));
	    transform.setRotation(tf::Quaternion(0,0,0,1));
	    br.sendTransform(tf::StampedTransform(transform, tstamp,
						  "/oriented_optimization_frame",
						  "/optimization_frame"));

	    
	    // Publish /map frame based on robot calibration
	    transform.setOrigin(tf::Vector3(cal_pos(0),
					    0,
					    cal_pos(2)));
	    transform.setRotation(tf::Quaternion(.707107,0.0,0.0,-0.707107));
	    br.sendTransform(tf::StampedTransform(transform, tstamp,
						  "/oriented_optimization_frame",
						  "/map"));
	    
	    // publish one more frame that is the frame the robot
	    // calculates its odometry in.
	    transform.setOrigin(tf::Vector3(0,0,0));
	    transform.setRotation(tf::Quaternion(1,0,0,0));
	    br.sendTransform(tf::StampedTransform(transform, tstamp,
						  "/map",
						  "/robot_odom_pov"));
	    ROS_DEBUG("frames sent");
	    return;
	}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "set_tfs");

	ros::NodeHandle n;
	ros::Subscriber pose_sub = n.subscribe<gazebo_msgs::ModelStates>("gazebo/model_states", 10, poseCallback);

	static tf::TransformBroadcaster roomba_base1;
	tf::Transform transform1;
	static tf::TransformBroadcaster roomba_base2;
	tf::Transform transform2;

	ros::Rate r(1000);

	while(ros::ok()){
		transform1.setOrigin( tf::Vector3(roomba_pose1.position.x, roomba_pose1.position.y, roomba_pose1.position.z ));
		transform1.setRotation( tf::Quaternion(roomba_pose1.orientation.x, roomba_pose1.orientation.y, roomba_pose1.orientation.z, roomba_pose1.orientation.w));
		transform2.setOrigin( tf::Vector3(roomba_pose2.position.x, roomba_pose2.position.y, roomba_pose2.position.z ));
		transform2.setRotation( tf::Quaternion(roomba_pose2.orientation.x, roomba_pose2.orientation.y, roomba_pose2.orientation.z, roomba_pose2.orientation.w));

		roomba_base1.sendTransform(tf::StampedTransform(transform1, ros::Time::now(), "odom", "roomba_base1"));
		roomba_base2.sendTransform(tf::StampedTransform(transform2, ros::Time::now(), "odom", "roomba_base2"));
		
		ros::spinOnce();
		r.sleep();
	}
	return 0;
}

    // this is a function for sending all of the global transforms
    // that we want to send:
    void send_global_transforms(ros::Time tstamp)
	{
	    tf::Transform transform;
	    // first publish the optimization frame:
	    transform.setOrigin(tf::Vector3(cal_pos(0),
					    cal_pos(1),
					    cal_pos(2)));
	    transform.setRotation(tf::Quaternion(0,0,0,1));
	    br.sendTransform(tf::StampedTransform(transform, tstamp,
						  "oriented_optimization_frame",
						  "optimization_frame"));

	    
	    // publish the map frame at same height as the Kinect
	    transform.setOrigin(tf::Vector3(cal_pos(0),
					    0,
					    cal_pos(2)));
	    transform.setRotation(tf::Quaternion(.707107,0.0,0.0,-0.707107));
	    br.sendTransform(tf::StampedTransform(transform, tstamp,
						  "oriented_optimization_frame",
						  "map"));
	    
	    // publish one more frame that is the frame the robot
	    // calculates its odometry in.
	    transform.setOrigin(tf::Vector3(0,0,0));
	    transform.setRotation(tf::Quaternion(1,0,0,0));
	    br.sendTransform(tf::StampedTransform(transform, tstamp,
						  "map",
						  "robot_odom_pov"));
	    
	    return;
	}

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"));

}

bool kinematic_filter::filter(std::list<foot_with_joints> &data)
{
    jnt_pos_in=kinematics.lwr_legs.joints_value;
    
    for (auto single_step=data.begin();single_step!=data.end();)
    {
        auto StanceFoot_MovingFoot=StanceFoot_World*single_step->World_MovingFoot;
        if (!frame_is_reachable(StanceFoot_MovingFoot,single_step->joints))
            single_step=data.erase(single_step);
        else
        {
            single_step->World_StanceFoot=World_StanceFoot;
            KDL::Frame StanceFoot_Waist;
            KDL::JntArray temp(kinematics.wl_leg.chain.getNrOfJoints());
            for (int i=0;i<temp.rows();i++)
                temp(i)=single_step->joints(i);
            current_fk_solver->JntToCart(temp,StanceFoot_Waist);
            single_step->World_Waist=World_StanceFoot*StanceFoot_Waist;
#ifdef KINEMATICS_OUTPUT
            tf::Transform current_robot_transform;
            tf::transformKDLToTF(single_step->World_Waist,current_robot_transform);
            static tf::TransformBroadcaster br;
            br.sendTransform(tf::StampedTransform(current_robot_transform, ros::Time::now(),  "world","KNEW_WAIST"));
            tf::Transform current_moving_foot_transform;
            tf::transformKDLToTF(World_StanceFoot*StanceFoot_MovingFoot,current_moving_foot_transform);
            br.sendTransform(tf::StampedTransform(current_moving_foot_transform, ros::Time::now(),  "world","Kmoving_foot"));
            tf::Transform fucking_transform;
            tf::transformKDLToTF(World_StanceFoot,fucking_transform);
            br.sendTransform(tf::StampedTransform(fucking_transform, ros::Time::now(), "world", "Kstance_foot"));
#endif
            single_step++;
        }
    }
    return true;
}

void poseCallback(const gazebo_msgs::ModelStates model_states)
{
  static tf::TransformBroadcaster br;
  int object_index = 0;

  for (int i = 0; i < model_states.name.size(); i++) {
    if (!(model_states.name[i] == "robot") && !(model_states.name[i] == "ground_plane")) {
      object_index = i;
      break;
    }
  }

  for (int i = object_index; i < model_states.name.size(); i++) {
    tf::Transform transform;
    transform.setOrigin(
        tf::Vector3(model_states.pose[i].position.x, model_states.pose[i].position.y,
                    model_states.pose[i].position.z));

    tf::Quaternion q;
    q.setX(model_states.pose[i].orientation.x);
    q.setY(model_states.pose[i].orientation.y);
    q.setZ(model_states.pose[i].orientation.z);
    q.setW(model_states.pose[i].orientation.w);
    transform.setRotation(q);
    br.sendTransform(
        tf::StampedTransform(transform, ros::Time::now(), "world",
                             model_states.name[i] + "/base_link"));
    br.sendTransform(
        tf::StampedTransform(transform, ros::Time::now(), "world",
                             model_states.name[i] + "/base_link_inertia"));
  }
}

// Timer callback
void TRPBViz::timerCallback(const ros::TimerEvent& e) {
	tf_start.stamp_ = ros::Time::now();
	tf_broadcaster.sendTransform(tf_start);
	tf_base_link.stamp_ = ros::Time::now();
	tf_broadcaster.sendTransform(tf_base_link);
	joint_states.header.stamp = ros::Time::now();
	joint_state_pub.publish(joint_states);
}

    void trCallback(const ros::TimerEvent& event) {

        ros::Time now = ros::Time::now() + ros::Duration(0.1);

        if (table_calibration_done_) {
            tr_table_.stamp_ = now;
            br_.sendTransform(tr_table_);
        }
        if (pr2_calibration_done_) {
            tr_pr2_.stamp_ = now;
            br_.sendTransform(tr_pr2_);
        }
    }

  void publish_phantom_state()
  {
    // Construct transforms
    tf::Transform l0, sensable;
    // Distance from table top to first intersection of the axes
    l0.setOrigin(tf::Vector3(0, 0, table_offset_)); // .135 - Omni, .155 - Premium 1.5, .345 - Premium 3.0
    l0.setRotation(tf::createQuaternionFromRPY(0, 0, 0));
    br_.sendTransform(tf::StampedTransform(l0, ros::Time::now(), base_link_name_.c_str(), link_names_[0].c_str()));

    // Displacement from vertical axis towards user.
    // Frame in which OpenHaptics report Phantom coordinates. Valid and useful
    // for Omni only, since other devices do not do calibration.
    sensable.setOrigin(tf::Vector3(-0.2, 0, 0));
    sensable.setRotation(tf::createQuaternionFromRPY(M_PI / 2, 0, -M_PI / 2));
    br_.sendTransform(
        tf::StampedTransform(sensable, ros::Time::now(), link_names_[0].c_str(), sensable_frame_name_.c_str()));

    tf::Transform tf_cur_transform;
    geometry_msgs::PoseStamped phantom_pose;

    // Convert column-major matrix to row-major
    tf_cur_transform.setFromOpenGLMatrix(state_->hd_cur_transform);
    // Scale from mm to m
    tf_cur_transform.setOrigin(tf_cur_transform.getOrigin() / 1000.0);
    // Since hd_cur_transform is defined w.r.t. sensable_frame
    tf_cur_transform = sensable * tf_cur_transform;
    // Rotate end-effector back to base
    tf_cur_transform.setRotation(tf_cur_transform.getRotation() * sensable.getRotation().inverse());

    // Publish pose in link_0
    phantom_pose.header.frame_id = tf::resolve(tf_prefix_, link_names_[0]);
    phantom_pose.header.stamp = ros::Time::now();
    tf::poseTFToMsg(tf_cur_transform, phantom_pose.pose);
    pose_publisher_.publish(phantom_pose);

    if ((state_->buttons[0] != state_->buttons_prev[0]) or (state_->buttons[1] != state_->buttons_prev[1]))
    {
      if ((state_->buttons[0] == state_->buttons[1]) and (state_->buttons[0] == 1))
      {
        state_->lock = !(state_->lock);
      }
      sensable_phantom::PhantomButtonEvent button_event;
      button_event.grey_button = state_->buttons[0];
      button_event.white_button = state_->buttons[1];
      state_->buttons_prev[0] = state_->buttons[0];
      state_->buttons_prev[1] = state_->buttons[1];
      button_publisher_.publish(button_event);
    }
  }

void PlayerTracker::publishPlayerInfo(int player)
{

	player_tracker::PosPrediction pred;


	if(player > -1) {
		PlayerInfo  &it = potentialPlayers[currentPlayer];
		std::vector<float> res = it.playerFilter.getStatus();
		tf::Transform tr;
		tr.setRotation( tf::Quaternion(0,0 , 0, 1) );
		tr.setOrigin(tf::Vector3(res[0],res[1],0.0));
		transformBroadcaster.sendTransform(tf::StampedTransform(tr, ros::Time::now(), "base_link_respondable", "bill_filtered"));
		tr.setOrigin(tf::Vector3(res[2]*2.0,res[3]*2.0,0.0));
		transformBroadcaster.sendTransform(tf::StampedTransform(tr, ros::Time::now(), "bill_filtered", "bill_filtered_vel"));
		pred.userId = player;
		pred.position.x = res[0];
		pred.position.y = res[1];
		pred.velocity.x = res[2];
		pred.velocity.y = res[3];
		pred.unreliability = sqrt(res[4] + res[5]);

		geometry_msgs::PoseStamped playerPose;
		playerPose.header.stamp = ros::Time::now();
		playerPose.header.frame_id =  "base_footprint";
		playerPose.pose.position.x = res[0];
		playerPose.pose.position.y = res[1];
		playerPose.pose.position.z = 0;

		float angle = atan2(res[3],res[2]);
		tf::Quaternion orientation(tf::Vector3(0,0,1),angle);
		geometry_msgs::Quaternion odom_quat;
		tf::quaternionTFToMsg(orientation,odom_quat);
		playerPose.pose.orientation = odom_quat;
		PosePredictionPublisher.publish(playerPose);
	} else {
		pred.userId = -1;
		pred.position.x = 0;
		pred.position.y = 0;
		pred.velocity.x = 0;
		pred.velocity.y = 0;
		pred.unreliability = std::numeric_limits<double>::max();

	}
	//std::cout <<"unrel: "<<pred.unreliability<<" "<<res[4]<<" "<<res[5]<<std::endl;
	predictionPublisher.publish(pred);


}

int main(int argc, char** argv){
  // Node initialization
  ros::init(argc, argv, "tf_fake");
  //ros::Time::init();
  ros::NodeHandle node;
  if (argc !=1){ROS_ERROR("Error argument");return -1;};
  ros::Rate r(10);

  // For closed-loop simulation
  ros::Subscriber sub = node.subscribe("cmd_vel", 100, drive);

  // Tf variable declaration
	static tf::TransformBroadcaster br;
	tf::Transform transform;
  tf::Quaternion q;

  while(node.ok())
  {
    // Data update
    x += dx;
    y += dy;
    yaw_counter += dyaw;
    if(yaw_counter > (M_PI/2) && yaw_counter < M_PI){
      yaw = yaw_counter - M_PI;
    }
    else if(yaw_counter > M_PI){
      yaw_counter -= M_PI;
      yaw = yaw_counter;
    }
    else{
      yaw = yaw_counter;
    }
    

    // Data transmit
	  transform.setOrigin(tf::Vector3(x, y, 0.0));

	  q.setRPY(0, 0, yaw);
	  transform.setRotation(q);

	  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "fake_tf"));
	  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "fake_ls_tf"));

	  ros::spinOnce();
	  r.sleep();
  };

  return 0;
}

void dynamixel_cb(const dynamixel_msgs::JointStateConstPtr& jstate)
{
	static tf::TransformBroadcaster br;
	tf::Transform transform;
    tf::Quaternion q;
	transform.setOrigin( tf::Vector3(0., 0., 0.) );
    q.setRPY(0, jstate->current_pos, 0);
	transform.setRotation(q);
	br.sendTransform(tf::StampedTransform(transform, jstate->header.stamp, "dynamixel_base", "dynamixel_link"));

	transform.setOrigin( tf::Vector3(dtol_x, dtol_y, dtol_z) );
    q.setRPY(0, 0, 0);
	transform.setRotation(q);
	br.sendTransform(tf::StampedTransform(transform, jstate->header.stamp, "dynamixel_link", "laser"));
}

void tfRegistration (const cv::Mat &camExtMat, const ros::Time& timeStamp)
{
  tf::Matrix3x3 rotation_mat;
  double roll = 0, pitch = 0, yaw = 0;
  tf::Quaternion quaternion;
  tf::Transform transform;
  static tf::TransformBroadcaster broadcaster;

 rotation_mat.setValue(camExtMat.at<double>(0, 0), camExtMat.at<double>(0, 1), camExtMat.at<double>(0, 2),
                        camExtMat.at<double>(1, 0), camExtMat.at<double>(1, 1), camExtMat.at<double>(1, 2),
                        camExtMat.at<double>(2, 0), camExtMat.at<double>(2, 1), camExtMat.at<double>(2, 2));

  rotation_mat.getRPY(roll, pitch, yaw, 1);

  quaternion.setRPY(roll, pitch, yaw);

  transform.setOrigin(tf::Vector3(camExtMat.at<double>(0, 3),
                                  camExtMat.at<double>(1, 3),
                                  camExtMat.at<double>(2, 3)));

  transform.setRotation(quaternion);

  broadcaster.sendTransform(tf::StampedTransform(transform, timeStamp, "velodyne", camera_id_str));
  //broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "velodyne", "camera"));
}

void velPublishtoEKF(void)
{
	geometry_msgs::TwistWithCovarianceStamped v;
	v.header.stamp = ros::Time::now();
	v.header.frame_id = "vel";
	
	//# create a new velocity
	v.twist.twist.linear.x = 0.0;
	v.twist.twist.linear.y = 0.0;
	v.twist.twist.linear.z = 0.0;
	
	//# Only the number in the covariance matrix diagonal 
	//# are used for the updates!
	v.twist.covariance[0] = 0.01;
	v.twist.covariance[7] = 0.01;
	v.twist.covariance[14] = 0.01;
	
	velpublish.publish(v);
	///publish TF
	static tf::TransformBroadcaster publishtf;
	tf::Transform transform;
	transform.setOrigin( tf::Vector3( 0, 0, 0.0) );
	tf::Quaternion q;
	q.setRPY(/**msg->theta**/0, 0, 0);
	transform.setRotation(q);
	publishtf.sendTransform(tf::StampedTransform(transform, v.header.stamp,"robot", v.header.frame_id)); ///parent sikik "robot", njut child (kuwalik karo sing piton)

	
        
}