示例#1
0
void publishMarker(const tf::Stamped<tf::Point>& target) {

	//create marker object
	visualization_msgs::Marker marker;


	uint32_t shape = visualization_msgs::Marker::SPHERE;

	// Set the frame ID and timestamp.
	marker.header.frame_id = target.frame_id_;
	marker.header.stamp = ros::Time::now();

	marker.ns = "head_target";
	marker.id = 0;

	// Set the marker type.
	marker.type = shape;

	// Set the marker action.
	marker.action = visualization_msgs::Marker::ADD;

	// Set the pose of the marker.
	marker.pose.position.x = target.getX();
	marker.pose.position.y = target.getY();
	marker.pose.position.z = target.getZ();
	marker.pose.orientation.x = 0.0;
	marker.pose.orientation.y = 0.0;
	marker.pose.orientation.z = 0.0;
	marker.pose.orientation.w = 1.0;

	// Set the scale of the marker -- 1x1x1 here means 1m on a side
	marker.scale.x = 0.08;
	marker.scale.y = 0.08;
	marker.scale.z = 0.08;

	// Set the color -- be sure to set alpha to something non-zero!
	marker.color.r = 1.0f;
	marker.color.g = 0.0f;
	marker.color.b = 1.0f;
	marker.color.a = 0.6;

	marker.lifetime = ros::Duration();

	// Publish the marker
	marker_pub_.publish(marker);

}
示例#2
0
void SlamGMapping::writeMinMax(tf::Stamped<tf::Quaternion> & minmax){
	tf::Quaternion quat (minmax.getX(),minmax.getY(),minmax.getZ(),minmax.getW());
	fwrite(&quat,sizeof(tf::Quaternion),1,minmaxfile);
}