//
// void step()
// Last modified: 27Dec2010
//
// Executes the appropriate active behavior.
//
// Returns: <none>
// Parameters: <none>
//
void Robot::step()
{
GLfloat collDist = collisionRadius();
vector<Vector> rels = getObjectRelationships(collDist);
if (rels.size() > 0)
{
GLfloat minDist = HUGE;
GLint minIndex = 0;
for (GLint i = 0, n = rels.size(); i < n; ++i)
{
GLfloat currDist = rels[i].magnitude();
if (currDist < minDist)
{
minDist = currDist;
minIndex = i;
}
}
//avoid(rels[minIndex], collDist);
}
if (behavior.isActive())
{
translateRelative(getTransVel());
rotateRelative(getAngVel());
}
} // step()
void Robot::step()
{
if (behavior.isActive())
{
translateRelative(getTransVel());
rotateRelative(getAngVel());
}
} // step()
void trackPointCB(const geometry_msgs::Point::ConstPtr& msg)
{
// make sure that the action hasn't been canceled
if (!as_.isActive())
{
twist_.angular.z = 0;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
return;
}
if( !has_goal_)
{
twist_.angular.z = 0;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
return;
}
if( obstacle_detected_ && msg->y < SVTIUS)
{
result_.success = false;
ROS_INFO("%s: Obsticle Detected", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
feedback_.success = false;
twist_.angular.z = 0;
twist_.linear.x = 0;
if ( msg->y >= TOP_T
&& msg->x <= RIGHT_T
&& msg->x >= LEFT_T )
{
twist_pub_.publish(twist_);
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
result_.success = true;
as_.setSucceeded(result_);
}
if(drop_flag) {
twist_pub_.publish(twist_);
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
result_.success = true;
drop_flag = false;
as_.setSucceeded(result_);
}
if (msg->y < TOP_T)
{
ROS_INFO("MOVE FORWARD");
twist_.linear.x = getLinVel(msg->y, TOP_T);
}
if (msg->x < LEFT_T)
{
ROS_INFO("TURN RIGHT");
twist_.angular.z = getAngVel(msg->x, LEFT_T);
}
else if (msg->x > RIGHT_T)
{
ROS_INFO("TURN LEFT");
twist_.angular.z = -1.0 * getAngVel(msg->x, RIGHT_T);
}
twist_pub_.publish(twist_);
as_.publishFeedback(feedback_);
}
