ViconSegment.cpp

Go to the documentation of this file.

/*
 * ViconSegment.cpp
 *
 *  Created on: Sep 3, 2012
 *      Author: mriedel
 */

#include <telekyb_vicon/ViconSegment.hpp>

#include <telekyb_base/ROS/ROSModule.hpp>
#include <telekyb_vicon/ViconSubject.hpp>


namespace TELEKYB_NAMESPACE {

ViconSegment::ViconSegment(ViconSubject& parent_, int segmentID_, const std::string segmentName_)
        : parent(parent_), segmentID(segmentID_), segmentName(segmentName_)
{
        ros::NodeHandle n(ROSModule::Instance().getMainNodeHandle(), parent.getName());
        posePublisher = n.advertise<geometry_msgs::PoseStamped>(segmentName,10);

        poseMsg.header.frame_id = parent.getName() + "/" + segmentName;

        ROS_INFO("Created Segment %s/%s",parent.getName().c_str(), segmentName.c_str());

}

ViconSegment& ViconSegment::operator= (const ViconSegment & other)
{
        segmentID = other.segmentID;
        segmentName = other.segmentName;
        posePublisher = other.posePublisher;
        poseMsg = other.poseMsg;
        return *this;
}

ViconSegment::~ViconSegment()
{

}

void ViconSegment::publish(const Eigen::Vector3d& globalTranslation, const Eigen::Quaterniond& globalOrientation)
{

        poseMsg.header.stamp = ros::Time::now();

        poseMsg.pose.position.x = globalTranslation(0);
        poseMsg.pose.position.y = globalTranslation(1);
        poseMsg.pose.position.z = globalTranslation(2);

        poseMsg.pose.orientation.w = globalOrientation.w();
        poseMsg.pose.orientation.x = globalOrientation.x();
        poseMsg.pose.orientation.y = globalOrientation.y();
        poseMsg.pose.orientation.z = globalOrientation.z();

        posePublisher.publish(poseMsg);
}

int ViconSegment::getSegmentID() const
{
        return segmentID;
}

std::string ViconSegment::getSegmentName() const
{
        return segmentName;
}


} /* namespace telekyb */