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

void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
  double roll, pitch, yaw;
  geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
  tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);

  transformSum[0] = -pitch;
  transformSum[1] = -yaw;
  transformSum[2] = roll;

  transformSum[3] = laserOdometry->pose.pose.position.x;
  transformSum[4] = laserOdometry->pose.pose.position.y;
  transformSum[5] = laserOdometry->pose.pose.position.z;

  transformAssociateToMap();

  geoQuat = tf::createQuaternionMsgFromRollPitchYaw
            (transformMapped[2], -transformMapped[0], -transformMapped[1]);

  laserOdometry2.header.stamp = laserOdometry->header.stamp;
  laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
  laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
  laserOdometry2.pose.pose.orientation.z = geoQuat.x;
  laserOdometry2.pose.pose.orientation.w = geoQuat.w;
  laserOdometry2.pose.pose.position.x = transformMapped[3];
  laserOdometry2.pose.pose.position.y = transformMapped[4];
  laserOdometry2.pose.pose.position.z = transformMapped[5];
  pubLaserOdometry2Pointer->publish(laserOdometry2);

  laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
  laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
  laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));
  tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2);
}

void InputOutputLinControl::updateMapRobotPose(tf::Transform relative_transform, tf::StampedTransform current_icp_robot_pose, tf::StampedTransform& next_icp_robot_pose)
{
	tf::Transform transformation = relative_transform * current_icp_robot_pose;
	next_icp_robot_pose.frame_id_ = current_icp_robot_pose.frame_id_;
	next_icp_robot_pose.stamp_ = ros::Time::now();
	next_icp_robot_pose.child_frame_id_ = current_icp_robot_pose.child_frame_id_;
	next_icp_robot_pose.setBasis(transformation.getBasis());
	next_icp_robot_pose.setOrigin(transformation.getOrigin());
	next_icp_robot_pose.setRotation(transformation.getRotation());
}

void servo_callback(const std_msgs::Float32::ConstPtr& angle_msg)
{
    ROS_INFO("angle: %f", angle_msg->data);
    tf::Quaternion quat = tf::createQuaternionFromRPY(((angle_msg->data+90.0)/180.0)*M_PI, 0.0, 0.0);

    trans.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
    trans.frame_id_ = odom;
    trans.child_frame_id_ = base_link;
    trans.setRotation(quat);
    trans.stamp_ = ros::Time::now(); //probably not valid, need to stamp the angle on the arduino itself
}

void imuMsgCallback(const sensor_msgs::Imu& imu_msg)
{
  tf::quaternionMsgToTF(imu_msg.orientation, tmp_);

  tfScalar yaw, pitch, roll;
  tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw);

  tmp_.setRPY(roll, pitch, 0.0);

  transform_.setRotation(tmp_);

  transform_.stamp_ = imu_msg.header.stamp;

  tfB_->sendTransform(transform_);
}

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::getTrueRobotPose(double displacement, tf::StampedTransform robot_poseB, tf::StampedTransform& true_robot_pose)
{
	true_robot_pose.frame_id_ = robot_poseB.frame_id_;
	true_robot_pose.stamp_ = robot_poseB.stamp_;

	double roll, pitch, yaw;
	robot_poseB.getBasis().getRPY(roll,pitch,yaw);

	tf::Vector3 v;
	v.setX(robot_poseB.getOrigin().getX() - displacement*cos(yaw));
	v.setY(robot_poseB.getOrigin().getY() - displacement*sin(yaw));
	v.setZ(robot_poseB.getOrigin().getZ());

	true_robot_pose.setOrigin(v);

	true_robot_pose.setRotation(robot_poseB.getRotation());
}

void InputOutputLinControl::realRobotPoseB(double displacement, tf::StampedTransform real_robot_pose, tf::StampedTransform& real_robot_poseB)
{
	real_robot_poseB.frame_id_ = real_robot_pose.frame_id_;
	real_robot_poseB.stamp_ = real_robot_pose.stamp_;
	real_robot_poseB.child_frame_id_ = real_robot_pose.child_frame_id_;

	tf::Vector3 v;
	double roll, pitch, yaw;
	real_robot_pose.getBasis().getRPY(roll,pitch,yaw);

	v.setX(real_robot_pose.getOrigin().getX() + displacement*cos(yaw));
	v.setY(real_robot_pose.getOrigin().getY() + displacement*sin(yaw));
	v.setZ(real_robot_pose.getOrigin().getZ());
	real_robot_poseB.setOrigin(v);

	real_robot_poseB.setRotation(real_robot_pose.getRotation());
}

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

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;
}

// Load full content of a given directory
bool TRPBViz::loadDirectory(const string& directory) {
	PointCloud tmp_cloud;
	// Input point cloud
	sensor_msgs::PointCloud2 input_point_cloud;
	bool ipc = deSerializePointCloud(tmp_cloud,
			directory+"/input_point_cloud.csv");
	if (ipc) {
		input_point_cloud = PointMatcher_ros::\
				pointMatcherCloudToRosMsg<float>(tmp_cloud, "/map",
				ros::Time::now());
		input_point_cloud_pub.publish(input_point_cloud);
	}
	// Sparse map point cloud (if any)
	sensor_msgs::PointCloud2 sparse_map;
	bool sm = deSerializePointCloud(tmp_cloud,
			directory+"/sparse_map.csv");
	if (sm) {
		sparse_map = PointMatcher_ros::\
				pointMatcherCloudToRosMsg<float>(tmp_cloud, "/map",
				ros::Time::now());
		sparse_map_pub.publish(sparse_map);
	}
	// Robot position (start)
	geometry_msgs::Pose start;
	// Goal position
	geometry_msgs::Pose goal;
	bool sg = loadStartGoal(directory+"/start_goal.csv", start, goal);
	if (sg) {
		// FIXME correct z by 0.0705
		tf::poseMsgToTF(start, tf_start);
		tf_start.frame_id_ = "/map";
		tf_start.child_frame_id_ = "/base_footprint";
		tf_start.stamp_ = ros::Time::now();
		tf_broadcaster.sendTransform(tf_start);
		visualization_msgs::Marker marker;
		marker.header.frame_id = "/map";
		marker.header.stamp = ros::Time();
		marker.ns = "Goal";
		marker.id = 0;
		marker.type = visualization_msgs::Marker::CUBE;
		marker.action = visualization_msgs::Marker::ADD;
		marker.pose = goal;
		marker.scale.x = 0.60;
		marker.scale.y = 0.60;
		marker.scale.z = 0.5;
		marker.color.r = 0.5;
		marker.color.g = 0.5;
		marker.color.b = 0.5;
		marker.color.a = .75;
		marker.lifetime = ros::Duration();
		goal_pub.publish(marker);
	} else {
		tf_start.setOrigin({0, 0, 0});
		tf_start.setRotation({0, 0, 0, 1});
		tf_start.frame_id_ = "/map";
		tf_start.child_frame_id_ = "/base_footprint";
	}
	// Path
	nav_msgs::Path path;
	bool p = loadPath(directory+"/path.csv", path);
	if (p) {
		ref_pub.publish(path);
	}
	return (ipc||sm||sg||p);
}