Matrix4d Transformation::getInvTransform() {
if ( mDirty ) {
computeTransform();
mDirty = false;
}
return mTransform.inverse();
}
Matrix4d TrfmRotateExpMap::getInvTransform(){
if(mDirty){
computeTransform();
mDirty=false;
}
return mTransform.transpose();
}
Matrix4d Transformation::getTransform() {
if ( mDirty ) {
computeTransform();
mDirty = false;
}
return mTransform;
}
void NavSatTransform::run()
{
ros::Time::init();
double frequency = 10;
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
// Subscribe to the messages we need
ros::Subscriber odomSub = nh.subscribe("odometry/filtered", 1, &NavSatTransform::odomCallback, this);
ros::Subscriber gpsSub = nh.subscribe("gps/fix", 1, &NavSatTransform::gpsFixCallback, this);
ros::Subscriber imuSub = nh.subscribe("imu/data", 1, &NavSatTransform::imuCallback, this);
// Load the parameters we need
nhPriv.getParam("magnetic_declination_radians", magneticDeclination_);
nhPriv.getParam("yaw_offset", yawOffset_);
nhPriv.param("broadcast_utm_transform", broadcastUtmTransform_, false);
nhPriv.param("zero_altitude", zeroAltitude_, false);
nhPriv.param("publish_filtered_gps", publishGps_, false);
nhPriv.param("frequency", frequency, 10.0);
ros::Publisher gpsOdomPub = nh.advertise<nav_msgs::Odometry>("odometry/gps", 10);
ros::Publisher filteredGpsPub;
if(publishGps_)
{
filteredGpsPub = nh.advertise<sensor_msgs::NavSatFix>("gps/filtered", 10);
}
tf::TransformBroadcaster utmBroadcaster;
tf::StampedTransform utmTransformStamped;
utmTransformStamped.child_frame_id_ = "utm";
ros::Rate rate(frequency);
while(ros::ok())
{
ros::spinOnce();
if(!transformGood_)
{
computeTransform();
if(transformGood_)
{
// Once we have the transform, we don't need the IMU
imuSub.shutdown();
}
}
else
{
nav_msgs::Odometry gpsOdom;
if(prepareGpsOdometry(gpsOdom))
{
gpsOdomPub.publish(gpsOdom);
}
if(publishGps_)
{
sensor_msgs::NavSatFix odomGps;
if(prepareFilteredGps(odomGps))
{
filteredGpsPub.publish(odomGps);
}
}
// Send out the UTM transform in case anyone
// else would like to use it.
if(transformGood_ && broadcastUtmTransform_)
{
utmTransformStamped.setData(utmWorldTransform_);
utmTransformStamped.frame_id_ = worldFrameId_;
utmTransformStamped.stamp_ = ros::Time::now();
utmBroadcaster.sendTransform(utmTransformStamped);
}
}
rate.sleep();
}
}
CPerspectiveCamera::CPerspectiveCamera(const CPoint& position, const CPoint& target, const F& fov, unsigned int width, unsigned int height,
E_IMAGE_TYPE type) :
ICamera(position, target, width, height, type), mFOV(fov)
{
computeTransform();
}
void CPerspectiveCamera::SetTarget(const CPoint& target)
{
mTarget = target;
computeTransform();
}
void CPerspectiveCamera::SetPosition(const CPoint& position)
{
mPosition = position;
computeTransform();
}
