Esempi in C++ (Cpp) per get_carmen_orientation_from_tf_transform

Esempio n. 1

File: tf_test.cpp Progetto: ulyssesrr/carmen_lcad

static void print_transform(tf::Transform transform)
{	
	carmen_orientation_3D_t orientation = get_carmen_orientation_from_tf_transform(transform);
	printf("y:% lf p:% lf r:% lf\n", orientation.yaw, orientation.pitch, orientation.roll);
	printf("x:% lf y:% lf z:% lf\n", transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
	printf("\n");
}

Esempio n. 2

File: tf_helper.cpp Progetto: ulyssesrr/carmen_lcad

static carmen_pose_3D_t 
tf_transform_to_carmen_pose_3D(tf::Transform transform)
{
	carmen_pose_3D_t pose;

	pose.position = tf_vector3_to_carmen_vector3(transform.getOrigin());
	pose.orientation = get_carmen_orientation_from_tf_transform(transform);
	
	return pose;
}

Esempio n. 3

File: xsens_xyz_handler.cpp Progetto: LCAD-UFES/carmen_lcad

static carmen_orientation_3D_t 
get_orientation_car_reference_from_message(tf::Quaternion xsens_reading)
{
	tf::StampedTransform xsens_to_car;
	transformer.lookupTransform(xsens_tf_name, car_tf_name, tf::Time(0), xsens_to_car);

	tf::Transform xsens_matrix;
	xsens_matrix.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
	xsens_matrix.setRotation(xsens_reading);
	
	tf::Transform car_pose = xsens_matrix * xsens_to_car;
	
	carmen_orientation_3D_t orientation = get_carmen_orientation_from_tf_transform(car_pose);

	return orientation;
}