Esempio n. 1
0
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");
}
void 
initialize_transforms()
{
	tf::Transform board_to_camera_pose;
	tf::Transform car_to_board_pose;
	tf::Transform world_to_car_pose;
	tf::Transform ultrasonic_sensor_r1_to_car_pose;
	tf::Transform ultrasonic_sensor_r2_to_car_pose;
	tf::Transform ultrasonic_sensor_l1_to_car_pose;
	tf::Transform ultrasonic_sensor_l2_to_car_pose;

	tf::Time::init();

	// initial car pose with respect to the world
	world_to_car_pose.setOrigin(tf::Vector3(car_pose_g.position.x, car_pose_g.position.y, car_pose_g.position.z));
	world_to_car_pose.setRotation(tf::Quaternion(car_pose_g.orientation.yaw, car_pose_g.orientation.pitch, car_pose_g.orientation.roll));
	tf::StampedTransform world_to_car_transform(world_to_car_pose, tf::Time(0), "/world", "/car");
	tf_transformer.setTransform(world_to_car_transform, "world_to_car_transform");

	// board pose with respect to the car
	car_to_board_pose.setOrigin(tf::Vector3(sensor_board_pose_g.position.x, sensor_board_pose_g.position.y, sensor_board_pose_g.position.z));
	car_to_board_pose.setRotation(tf::Quaternion(sensor_board_pose_g.orientation.yaw, sensor_board_pose_g.orientation.pitch, sensor_board_pose_g.orientation.roll)); 				// yaw, pitch, roll
	tf::StampedTransform car_to_board_transform(car_to_board_pose, tf::Time(0), "/car", "/board");
	tf_transformer.setTransform(car_to_board_transform, "car_to_board_transform");

	// camera pose with respect to the board
	board_to_camera_pose.setOrigin(tf::Vector3(camera_pose_g.position.x, camera_pose_g.position.y, camera_pose_g.position.z));
	board_to_camera_pose.setRotation(tf::Quaternion(camera_pose_g.orientation.yaw, camera_pose_g.orientation.pitch, camera_pose_g.orientation.roll)); 				// yaw, pitch, roll
	tf::StampedTransform board_to_camera_transform(board_to_camera_pose, tf::Time(0), "/board", "/camera");
	tf_transformer.setTransform(board_to_camera_transform, "board_to_camera_transform");

	// initial ultrasonic sensor r1 pose with respect to the car
	ultrasonic_sensor_r1_to_car_pose.setOrigin(tf::Vector3(ultrasonic_sensor_r1_g.position.x, ultrasonic_sensor_r1_g.position.y, ultrasonic_sensor_r1_g.position.z));
	ultrasonic_sensor_r1_to_car_pose.setRotation(tf::Quaternion(ultrasonic_sensor_r1_g.orientation.yaw, ultrasonic_sensor_r1_g.orientation.pitch, ultrasonic_sensor_r1_g.orientation.roll));
	tf::StampedTransform ultrasonic_sensor_r1_to_car_transform(ultrasonic_sensor_r1_to_car_pose, tf::Time(0), "/car", "/ultrasonic_sensor_r1");
	tf_transformer.setTransform(ultrasonic_sensor_r1_to_car_transform, "ultrasonic_sensor_r1_to_car_transform");

	// initial ultrasonic sensor r2 pose with respect to the car
	ultrasonic_sensor_r2_to_car_pose.setOrigin(tf::Vector3(ultrasonic_sensor_r2_g.position.x, ultrasonic_sensor_r2_g.position.y, ultrasonic_sensor_r2_g.position.z));
	ultrasonic_sensor_r2_to_car_pose.setRotation(tf::Quaternion(ultrasonic_sensor_r2_g.orientation.yaw, ultrasonic_sensor_r2_g.orientation.pitch, ultrasonic_sensor_r2_g.orientation.roll));
	tf::StampedTransform ultrasonic_sensor_r2_to_car_transform(ultrasonic_sensor_r2_to_car_pose, tf::Time(0), "/car", "/ultrasonic_sensor_r2");
	tf_transformer.setTransform(ultrasonic_sensor_r2_to_car_transform, "ultrasonic_sensor_r2_to_car_transform");

	// initial ultrasonic sensor l2 pose with respect to the car
	ultrasonic_sensor_l2_to_car_pose.setOrigin(tf::Vector3(ultrasonic_sensor_l2_g.position.x, ultrasonic_sensor_l2_g.position.y, ultrasonic_sensor_l2_g.position.z));
	ultrasonic_sensor_l2_to_car_pose.setRotation(tf::Quaternion(ultrasonic_sensor_l2_g.orientation.yaw, ultrasonic_sensor_l2_g.orientation.pitch, ultrasonic_sensor_l2_g.orientation.roll));
	tf::StampedTransform ultrasonic_sensor_l2_to_car_transform(ultrasonic_sensor_l2_to_car_pose, tf::Time(0), "/car", "/ultrasonic_sensor_l2");
	tf_transformer.setTransform(ultrasonic_sensor_l2_to_car_transform, "ultrasonic_sensor_l2_to_car_transform");

	// initial ultrasonic sensor l1 pose with respect to the car
	ultrasonic_sensor_l1_to_car_pose.setOrigin(tf::Vector3(ultrasonic_sensor_l1_g.position.x, ultrasonic_sensor_l1_g.position.y, ultrasonic_sensor_l1_g.position.z));
	ultrasonic_sensor_l1_to_car_pose.setRotation(tf::Quaternion(ultrasonic_sensor_l1_g.orientation.yaw, ultrasonic_sensor_l1_g.orientation.pitch, ultrasonic_sensor_l1_g.orientation.roll));
	tf::StampedTransform ultrasonic_sensor_l1_to_car_transform(ultrasonic_sensor_l1_to_car_pose, tf::Time(0), "/car", "/ultrasonic_sensor_l1");
	tf_transformer.setTransform(ultrasonic_sensor_l1_to_car_transform, "ultrasonic_sensor_l1_to_car_transform");


}
void 
initialize_pose_6d_transformation_matrix()
{
	visual_odometry_pose_6d_transformation_matrix = Matrix::eye(4);

	tf::Transform board_to_camera_pose_g;
	tf::Transform car_to_board_pose;
	tf::Transform world_to_car_pose_g;
	tf::Transform camera_to_visual_odometry_transform;

	// initial car pose with respect to the world
	world_to_car_pose_g.setOrigin(tf::Vector3(car_pose_g.position.x, car_pose_g.position.y, car_pose_g.position.z));
	world_to_car_pose_g.setRotation(tf::Quaternion(car_pose_g.orientation.yaw, car_pose_g.orientation.pitch, car_pose_g.orientation.roll));
	tf::StampedTransform world_to_car_transform(world_to_car_pose_g, tf::Time(0), "/world", "/car");
	transformer.setTransform(world_to_car_transform, "world_to_car_transform");

	// board pose with respect to the car
	car_to_board_pose.setOrigin(tf::Vector3(sensor_board_pose_g.position.x, sensor_board_pose_g.position.y, sensor_board_pose_g.position.z));
	car_to_board_pose.setRotation(tf::Quaternion(sensor_board_pose_g.orientation.yaw, sensor_board_pose_g.orientation.pitch, sensor_board_pose_g.orientation.roll)); 				// yaw, pitch, roll
	tf::StampedTransform car_to_board_transform(car_to_board_pose, tf::Time(0), "/car", "/board");
	transformer.setTransform(car_to_board_transform, "car_to_board_transform");

	// camera pose with respect to the board
	board_to_camera_pose_g.setOrigin(tf::Vector3(camera_pose_g.position.x, camera_pose_g.position.y, camera_pose_g.position.z));
	board_to_camera_pose_g.setRotation(tf::Quaternion(camera_pose_g.orientation.yaw, camera_pose_g.orientation.pitch, camera_pose_g.orientation.roll)); 				// yaw, pitch, roll
	tf::StampedTransform board_to_camera_transform(board_to_camera_pose_g, tf::Time(0), "/board", "/camera");
	transformer.setTransform(board_to_camera_transform, "board_to_camera_transform");

	/**
	 * visual odometry pose with respect to the camera
	 * (the rotation comes from parent coordinate system (camera) to
	 * the child coordinate system (visual_odometry)) following yaw,
	 * pitch, roll angles order).
	 */
	camera_to_visual_odometry_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); // x, y, z;
	camera_to_visual_odometry_transform.setRotation(tf::Quaternion(-M_PI / 2.0, 0.0, -M_PI / 2.0));	// yaw, pitch, roll
	tf::StampedTransform camera_to_visual_odometry_stamped_transform(camera_to_visual_odometry_transform, tf::Time(0), "/camera", "/visual_odometry"); 	// create a time stamped transformation that defines the visual odometry position with respect to the camera
	transformer.setTransform(camera_to_visual_odometry_stamped_transform, "camera_to_visual_odometry_transform"); // add a link to the tf tree with the camera_to_visual_odometry_transform stamped transformation

	// get the transformation between the visual odometry coordinate system with respect to the carmen coordinate system.
	transformer.lookupTransform("/car", "/visual_odometry", tf::Time(0), g_car_to_visual_odometry_transform);
}
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");
}
Esempio n. 5
0
void
initialize_transforms()
{//
	tf::Time::init();

	// initial car pose with respect to the world
	world_to_car_pose.setOrigin(tf::Vector3(car_pose.position.x, car_pose.position.y, car_pose.position.z));
	world_to_car_pose.setRotation(tf::Quaternion(car_pose.orientation.yaw, car_pose.orientation.pitch, car_pose.orientation.roll));
	tf::StampedTransform world_to_car_transform(world_to_car_pose, tf::Time(0), "/world", "/car");
	transformer.setTransform(world_to_car_transform, "world_to_car_transform");

	// board pose with respect to the car
	car_to_board_pose.setOrigin(tf::Vector3(sensor_board_1_pose.position.x, sensor_board_1_pose.position.y, sensor_board_1_pose.position.z));
	car_to_board_pose.setRotation(tf::Quaternion(sensor_board_1_pose.orientation.yaw, sensor_board_1_pose.orientation.pitch, sensor_board_1_pose.orientation.roll)); 				// yaw, pitch, roll
	tf::StampedTransform car_to_board_transform(car_to_board_pose, tf::Time(0), "/car", "/board");
	transformer.setTransform(car_to_board_transform, "car_to_board_transform");

	// velodyne pose with respect to the board
	board_to_velodyne_pose.setOrigin(tf::Vector3(velodyne_pose.position.x, velodyne_pose.position.y, velodyne_pose.position.z));
	board_to_velodyne_pose.setRotation(tf::Quaternion(velodyne_pose.orientation.yaw, velodyne_pose.orientation.pitch, velodyne_pose.orientation.roll)); 				// yaw, pitch, roll
	tf::StampedTransform board_to_velodyne_transform(board_to_velodyne_pose, tf::Time(0), "/board", "/velodyne");
	transformer.setTransform(board_to_velodyne_transform, "board_to_velodyne_transform");
}
srs_ui_but::COBStretch calculateStretch(std::vector<std::string> links)
{
  ros::Time time_stamp = ros::Time().now();

  float radius = 0;
  float height = 0;

  for (std::vector<std::string>::iterator i = links.begin(); i != links.end(); i++)
  {
    try
    {
      tfListener->waitForTransform(*i, srs_ui_but::DEFAULT_COB_BASE_LINK, time_stamp, ros::Duration(0.2));
      tfListener->lookupTransform(*i, srs_ui_but::DEFAULT_COB_BASE_LINK, time_stamp, linkToBaseTf);

      transformer.setTransform(linkToBaseTf);

      // Transform link to camera
      tf::Stamped<btVector3> p;
      p.setX(0);
      p.setY(0);
      p.setZ(0);
      p.frame_id_ = *i;
      transformer.transformPoint(srs_ui_but::DEFAULT_COB_BASE_LINK, p, p);

      //std::cout << p.getX() << ", " << p.getY() << ", " << p.getZ() << ", " << std::endl;

      float r = pow(p.getX(), 2) + pow(p.getY(), 2);
      if (r > radius)
      {
        radius = r;
        height = p.getZ();
      }
    }

    catch (tf::TransformException& ex)
    {
      ROS_WARN("Transform ERROR:\n %s", ex.what());
    }
  }

  srs_ui_but::COBStretch cob_stretch;
  cob_stretch.radius = sqrt(radius);
  cob_stretch.height = height;
  cob_stretch.time_stamp = time_stamp;

  return cob_stretch;
}
static void
ultrasonic_sensor_message_handler(carmen_ultrasonic_sonar_sensor_message *message)
{

	tf::Transform world_to_car_pose;
	double yaw, pitch, roll;

	if (!grid_mapping_initialized)
		return;
	
	int i;

	carmen_visual_odometry_pose6d_message odometry_message;
	odometry_message = interpolator.InterpolateMessages(message);

	Xt.x = odometry_message.pose_6d.x;
	Xt.y = odometry_message.pose_6d.y;
	Xt.theta = odometry_message.pose_6d.yaw;

	carmen_update_cells_below_robot(&carmen_map, Xt);

	world_to_car_pose.setOrigin(tf::Vector3(odometry_message.pose_6d.x, odometry_message.pose_6d.y, odometry_message.pose_6d.z));
	world_to_car_pose.setRotation(tf::Quaternion(odometry_message.pose_6d.yaw, odometry_message.pose_6d.pitch, odometry_message.pose_6d.roll));
	tf::StampedTransform world_to_car_transform(world_to_car_pose, tf::Time(0), "/world", "/car");
	tf_transformer.setTransform(world_to_car_transform, "world_to_car_transform");

	//SENSOR R1 - FRONTAL
	tf::StampedTransform world_to_ultrasonic_sensor_r1;
	tf_transformer.lookupTransform("/world", "/ultrasonic_sensor_r1", tf::Time(0), world_to_ultrasonic_sensor_r1);
	Xt_r1.x = world_to_ultrasonic_sensor_r1.getOrigin().x();
	Xt_r1.y = world_to_ultrasonic_sensor_r1.getOrigin().y();
	tf::Matrix3x3(world_to_ultrasonic_sensor_r1.getRotation()).getEulerYPR(yaw, pitch, roll);
	Xt_r1.theta = yaw;

	double range[180];

	for (i=0 ; i<180 ; i++)
		range[i] = (double) message->sensor[3];

	carmen_grid_mapping_update_grid_map(&carmen_map, Xt_r1, range, &laser_params);
	carmen_grid_mapping_publish_message(&carmen_map, message->timestamp);

	//SENSOR R2 - LATERAL FRONTAL
	tf::StampedTransform world_to_ultrasonic_sensor_r2;
	tf_transformer.lookupTransform("/world", "/ultrasonic_sensor_r2", tf::Time(0), world_to_ultrasonic_sensor_r2);
	Xt_r2.x = world_to_ultrasonic_sensor_r2.getOrigin().x();
	Xt_r2.y = world_to_ultrasonic_sensor_r2.getOrigin().y();
	tf::Matrix3x3(world_to_ultrasonic_sensor_r2.getRotation()).getEulerYPR(yaw, pitch, roll);
	Xt_r2.theta = yaw;

	for (i=0 ; i<180 ; i++)
			range[i] = (double) message->sensor[2];

	carmen_grid_mapping_update_grid_map(&carmen_map, Xt_r2, range, &laser_params);
	carmen_grid_mapping_publish_message(&carmen_map, message->timestamp);

	//SENSOR L2 - LATERAL TRASEIRO
	tf::StampedTransform world_to_ultrasonic_sensor_l2;
	tf_transformer.lookupTransform("/world", "/ultrasonic_sensor_l2", tf::Time(0), world_to_ultrasonic_sensor_l2);
	Xt_l2.x = world_to_ultrasonic_sensor_l2.getOrigin().x();
	Xt_l2.y = world_to_ultrasonic_sensor_l2.getOrigin().y();
	tf::Matrix3x3(world_to_ultrasonic_sensor_l2.getRotation()).getEulerYPR(yaw, pitch, roll);
	Xt_l2.theta = yaw;

	for (i=0 ; i<180 ; i++)
			range[i] = (double) message->sensor[1];

	carmen_grid_mapping_update_grid_map(&carmen_map, Xt_l2, range, &laser_params);
	carmen_grid_mapping_publish_message(&carmen_map, message->timestamp);

	//SENSOR L1 - TRASEIRO
	tf::StampedTransform world_to_ultrasonic_sensor_l1;
	tf_transformer.lookupTransform("/world", "/ultrasonic_sensor_l1", tf::Time(0), world_to_ultrasonic_sensor_l1);
	Xt_l1.x = world_to_ultrasonic_sensor_l1.getOrigin().x();
	Xt_l1.y = world_to_ultrasonic_sensor_l1.getOrigin().y();
	tf::Matrix3x3(world_to_ultrasonic_sensor_l1.getRotation()).getEulerYPR(yaw, pitch, roll);
	Xt_l1.theta = yaw;

	for (i=0 ; i<180 ; i++)
			range[i] = (double) message->sensor[0];

	carmen_grid_mapping_update_grid_map(&carmen_map, Xt_l1, range, &laser_params);
	carmen_grid_mapping_publish_message(&carmen_map, message->timestamp);


	/*static int count = 1;

	char mapname[256];
	sprintf(mapname, "map%d.jpg", count);

	printf("saving map...\n");
	carmen_graphics_write_map_as_jpeg(mapname, &carmen_map, 0);
	printf("saved map...\n");

	count++;*/

}