C++ (Cpp) carmen_rotation_to_tf_quaternionの例

コード例 #1

ファイル: tf_helper.cpp プロジェクト: ulyssesrr/carmen_lcad

static void
initialize_transformations(void)
{
	tf::Time::init();

	tf::Transform velodyne_position_on_board;	
	velodyne_position_on_board.setOrigin(carmen_vector3_to_tf_vector3(velodyne_pose.position));
	velodyne_position_on_board.setRotation(carmen_rotation_to_tf_quaternion(velodyne_pose.orientation));
	tf::StampedTransform board_to_velodyne_transform(velodyne_position_on_board, tf::Time(0), board_tf_name, velodyne_tf_name);
	transformer.setTransform(board_to_velodyne_transform, "board_to_velodyne_transform");

	tf::Transform board_position_on_car;	
	board_position_on_car.setOrigin(carmen_vector3_to_tf_vector3(sensor_board_pose.position));
	board_position_on_car.setRotation(carmen_rotation_to_tf_quaternion(sensor_board_pose.orientation));
	tf::StampedTransform car_to_board_transform(board_position_on_car, tf::Time(0), car_tf_name, board_tf_name);
	transformer.setTransform(car_to_board_transform, "car_to_board_transform");
}

コード例 #2

ファイル: xsens_xyz_handler.cpp プロジェクト: LCAD-UFES/carmen_lcad

static void
initialize_transformations(xsens_xyz_handler *xsens_handler)
{
	tf::Time::init();

	tf::Transform xsens_position_on_board;	
	xsens_position_on_board.setOrigin(carmen_vector3_to_tf_vector3(xsens_handler->xsens_pose.position));
	xsens_position_on_board.setRotation(carmen_rotation_to_tf_quaternion(xsens_handler->xsens_pose.orientation));
	tf::StampedTransform board_to_xsens_transform(xsens_position_on_board, tf::Time(0), board_tf_name, xsens_tf_name);
	transformer.setTransform(board_to_xsens_transform, "board_to_xsens_transform");

	tf::Transform gps_position_on_board;	
	gps_position_on_board.setOrigin(carmen_vector3_to_tf_vector3(xsens_handler->gps_pose_in_the_car.position));
	gps_position_on_board.setRotation(carmen_rotation_to_tf_quaternion(xsens_handler->gps_pose_in_the_car.orientation));
	tf::StampedTransform board_to_gps_transform(gps_position_on_board, tf::Time(0), board_tf_name, gps_tf_name);
	transformer.setTransform(board_to_gps_transform, "board_to_gps_transform");

	tf::Transform board_position_on_car;	
	board_position_on_car.setOrigin(carmen_vector3_to_tf_vector3(xsens_handler->sensor_board_pose.position));
	board_position_on_car.setRotation(carmen_rotation_to_tf_quaternion(xsens_handler->sensor_board_pose.orientation));
	tf::StampedTransform car_to_board_transform(board_position_on_car, tf::Time(0), car_tf_name, board_tf_name);
	transformer.setTransform(car_to_board_transform, "car_to_board_transform");
}

コード例 #3

ファイル: xsens_xyz_handler.cpp プロジェクト: LCAD-UFES/carmen_lcad

static carmen_orientation_3D_t 
get_car_ang_velocity_from_message(carmen_xsens_global_quat_message *xsens_mti_message)
{
	carmen_orientation_3D_t gyr_orientation;
	gyr_orientation.roll = xsens_mti_message->m_gyr.x;
	gyr_orientation.pitch = xsens_mti_message->m_gyr.y;
	gyr_orientation.yaw = xsens_mti_message->m_gyr.z;

	tf::Quaternion tf_quat_xsens = carmen_rotation_to_tf_quaternion(gyr_orientation);
	
	carmen_orientation_3D_t ang_velocity = get_orientation_car_reference_from_message(tf_quat_xsens);

	return ang_velocity;
}