flightgoggles_ros_bridge/src/GeneralClient/GeneralClient.cpp
/**
 * updateCameraTrajectory function to generate a simple circular trajectory
 */
updateCameraTrajectory function to generate a simple circular trajectory
updateCameraTrajectory function to generate a simple circular trajectory
void GeneralClient::updateCameraTrajectory(){
  double period = 15.0f;
  double r = 2.0f;
  double t = (flightGoggles.getTimestamp()-startTime)/1000000.0f;
  double theta = -((t/period)*2.0f*M_PI);
  Transform3 camera_pose;
  camera_pose.translation() = Vector3(r*cos(theta), r*sin(theta), 1.5f);
  camera_pose.linear() = Eigen::AngleAxisd(theta-M_PI, Eigen::Vector3d(0,0,1)).toRotationMatrix();
  flightGoggles.setCameraPoseUsingROSCoordinates(camera_pose, 0);
  flightGoggles.setCameraPoseUsingROSCoordinates(camera_pose, 1);
}
void GeneralClient :: updateCameraTrajectory ( ) { double period = 15.0f ; double r = 2.0f ; double t = ( flightGoggles . getTimestamp ( ) - startTime ) / 1000000.0f ; double theta = - ( ( t / period ) * 2.0f * M_PI ) ; Transform3 camera_pose ; camera_pose . translation ( ) = Vector3 ( r * cos ( theta ) , r * sin ( theta ) , 1.5f ) ; camera_pose . linear ( ) = Eigen :: AngleAxisd ( theta - M_PI , Eigen :: Vector3d ( 0 , 0 , 1 ) ) . toRotationMatrix ( ) ; flightGoggles . setCameraPoseUsingROSCoordinates ( camera_pose , 0 ) ; flightGoggles . setCameraPoseUsingROSCoordinates ( camera_pose , 1 ) ; }
updateCameraTrajectory function to generate a simple circular trajectory
updateCameraTrajectory function to generate a simple circular trajectory
// Set rotation matrix using pitch, roll, yaw // Populate status message with new pose