VMObsAvoid.cpp

Go to the documentation of this file.

/*
 * VMObsAvoid.cpp
 *
 *  Created on: Dec 14, 2011
 *      Author: mriedel
 */

#include <trajmodules/VMObsAvoid.hpp>

#include <telekyb_base/ROS.hpp>

// Declare
PLUGINLIB_DECLARE_CLASS(tk_trajprocessor, VMObsAvoid, telekyb_traj::VMObsAvoid, TELEKYB_NAMESPACE::TrajectoryModule);

namespace telekyb_traj {

VMObsAvoidOptions::VMObsAvoidOptions()
        : OptionContainer("VMObsAvoid")
{
        tObsPointsTopicName = addOption<std::string>("tObsPointsTopicName", "Topic Name of Obstacle Points",
                        "undef", true, true);
}


VMObsAvoid::VMObsAvoid()
        : TrajectoryModule("tk_trajprocessor/VMObsAvoid", TrajModulePosType::Velocity, 100),
          obsAvoidPotentialAlg(getName())
{

}

void VMObsAvoid::initialize()
{

}

void VMObsAvoid::destroy()
{

}

// set back to intial conditions
void VMObsAvoid::willTurnActive()
{
        ros::NodeHandle n(ROSModule::Instance().getMainNodeHandle());
        lastObstaclePoints.clear();
        obsPointSub = n.subscribe(options.tObsPointsTopicName->getValue(), 1, &VMObsAvoid::obsPointCB, this);
}

// called after turning inactive
void VMObsAvoid::didTurnInactive()
{
        obsPointSub.shutdown();
}

bool VMObsAvoid::trajectoryStep(const TKState& currentState, TKTrajectory& trajInput)
{
        boost::mutex::scoped_lock lastObstaclePointsLock(lastObstaclePointsMutex);
        Velocity3D obsVelocity = obsAvoidPotentialAlg.getObstacleVelocity(currentState.position, lastObstaclePoints);
//      Acceleration3D obsAcceleration = obsAvoidPotentialAlg.getObstacleAcceleration(currentState.position, lastObstaclePoints);

        trajInput.velocity += obsVelocity;
//      trajInput.acceleration += obsAcceleration;

        return true;
}

void VMObsAvoid::obsPointCB(const telekyb_msgs::StampedPointArray::ConstPtr& obsPointsMsg)
{
        boost::mutex::scoped_lock lastObstaclePointsLock(lastObstaclePointsMutex);
        lastObstaclePoints.resize(obsPointsMsg->points.size());
        for (unsigned int i = 0; i < lastObstaclePoints.size(); ++i) {
                lastObstaclePoints[i](0) = obsPointsMsg->points[i].x;
                lastObstaclePoints[i](1) = obsPointsMsg->points[i].y;
                lastObstaclePoints[i](2) = obsPointsMsg->points[i].z;
        }
}

} /* namespace telekyb_traj */