Commit 2b962d4d authored by Sam Horvath's avatar Sam Horvath

ENH: Scope angulation now functioning correctly

parent 56e2f58e
Pipeline #58921 failed with stage
......@@ -127,7 +127,7 @@ void setup() {
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(57600);
Serial.println("setup");
//Serial.println("setup");
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
......
......@@ -39,18 +39,20 @@ FLSCameraController::getCameraHeadAngleOffset() const
return m_cameraHeadAngleOffset;
}
void
FLSCameraController::setCameraAngulationOffset(const double angle)
FLSCameraController::setCameraAngulation(const double angle)
{
m_cameraAngulationOffset = angle;
m_cameraAngulationRotOffset = Quatd(Eigen::AngleAxisd(angle*PI / 180., Vec3d(0., 1., 0.)));
m_cameraAngulation = angle;
m_cameraAngulationRotOffset = Quatd(Eigen::AngleAxisd(angle*PI / 180., Vec3d(0., 1., 0.)));
}
double
FLSCameraController::getCameraAngulationOffset() const
FLSCameraController::getCameraAngulation() const
{
return m_cameraAngulationOffset;
}
return m_cameraAngulation;
}
void
FLSCameraController::setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> aClient)
......@@ -58,30 +60,26 @@ FLSCameraController::setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> a
arduinoActive = true;
arduinoClient = aClient;
m_rollOffset = arduinoClient->getRoll();
}
void
FLSCameraController::setCameraAngulationTranslationOffset(const double t)
{
m_angulationTranslationOffset = t;
}
double
FLSCameraController::getCameraAngulationTranslationOffset() const
{
return m_angulationTranslationOffset;
}
void
FLSCameraController::runModule()
{
if (arduinoActive)
//Get head angle from Arduino, performing calibration if this is the first valid report
if (arduinoActive & calibrated)
{
this->setCameraHeadAngleOffset(arduinoClient->getRoll() - m_rollOffset);
}
else if (arduinoActive)
{
if (arduinoClient->getRoll() != 0){
std::cout << "FLS Camera Controller: Calibration complete; Safe to move camera" << std::endl;
m_rollOffset = arduinoClient->getRoll();
calibrated = true;
}
}
auto roff = Quatd(Eigen::AngleAxisd(m_cameraHeadAngleOffset*PI / 180., Vec3d(0., 0., 1.)));
roff *= m_cameraAngulationRotOffset;
this->setCameraRotationOffset(roff);
if (!m_trackingDataUptoDate)
......@@ -96,18 +94,22 @@ FLSCameraController::runModule()
Vec3d p = getPosition();
Quatd r = getRotation();
m_cameraTranslationOffset = r*Vec3d(m_angulationTranslationOffset*cos(m_cameraAngulationOffset*PI / 180.),
0.,
m_angulationTranslationOffset*sin(m_cameraAngulationOffset*PI / 180.));
//Adjust the upward angulation position to the center of the ROM
auto angulationDirectionOffset = Quatd(Eigen::AngleAxisd(-90*PI / 180., Vec3d(0., 0., 1.)));
r *= angulationDirectionOffset;
// Apply Offsets over the device pose
p += m_cameraTranslationOffset; // Offset the device position
r *= m_cameraRotationalOffset; // Apply camera head rotation offset
//Apply offset from angulation
r *= m_cameraAngulationRotOffset;
// Set camera pose
m_camera.setPosition(p);
m_camera.setFocalPoint(r*FORWARD_VECTOR + p);
m_camera.setViewUp(r*UP_VECTOR);
m_camera.setPosition(p); //position of camera
m_camera.setFocalPoint(r*FORWARD_VECTOR + p); //direction camera is looking
m_camera.setViewUp(m_cameraRotationalOffset*UP_VECTOR); //Orientation of camera
m_trackingDataUptoDate = false;
}
......
......@@ -55,16 +55,10 @@ public:
const double getCameraHeadAngleOffset() const;
///
/// \brief Get/Set the rotation offset due to angulation
/// \brief Get/Set the angulation
///
void setCameraAngulationOffset(const double angle);
double getCameraAngulationOffset() const;
///
/// \brief Get/Set the translation offset due to angulation
///
void setCameraAngulationTranslationOffset(const double t);
double getCameraAngulationTranslationOffset() const;
void setCameraAngulation(const double angle);
double getCameraAngulation() const ;
void setArduinoDevice(std::shared_ptr<VRPNArduinoDeviceClient> aClient);
......@@ -75,11 +69,12 @@ protected:
///
void runModule() override;
double m_cameraHeadAngleOffset = 0; ///< camera head angle offset (in deg)
Quatd m_cameraAngulationRotOffset = Quatd::Identity();///< Rotation offset for the camera via telescope angulation
double m_angulationTranslationOffset = 0.1;
double m_cameraHeadAngleOffset = 0; ///< camera head angle offset (in deg)
double m_cameraAngulation = 0; ///the actual angulation!!! 0, 30, 45 deg
Quatd m_cameraAngulationRotOffset = Quatd::Identity(); ///< Rotation offset for the camera via telescope angulation
std::shared_ptr<VRPNArduinoDeviceClient> arduinoClient;
bool arduinoActive = false;
bool calibrated = false;
double m_rollOffset = 0;
};
} // imstk
......
......@@ -31,7 +31,7 @@ endif()
include(imstkAddExternalProject)
imstk_add_external_project( VRPN
GIT_REPOSITORY https://github.com/sjh26/vrpn
GIT_TAG e8cfc645c33f4072723e503d7aa85f3a37646de6
GIT_TAG 6721b5ea8972cf6bcbaccdd2d32479dd77b74b53
INSTALL_COMMAND ${SKIP_STEP_COMMAND}
CMAKE_ARGS
-DBUILD_TESTING:BOOL=OFF
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment