void BaseMotionController::run()
{
while (ros::ok())
{
ros::Rate(10).sleep();
ros::spinOnce();
// wait for start trigger and new Twist message
if (!start_relative_movement_ || !new_command_sent_)
{
continue;
}
odom_received_ = false;
if (start_relative_movement_)
{
// rotate first then translate
if (rotate_first_)
{
if (z_rot_ != 0.0)
{
rotateRelative(z_rot_);
}
if (x_trans_ != 0.0 || y_trans_ != 0.0)
{
translateRelative(x_trans_, y_trans_);
}
}
else
{
// translate first then rotate
if (x_trans_ != 0.0 || y_trans_ != 0.0)
{
translateRelative(x_trans_, y_trans_);
}
if (z_rot_ != 0.0)
{
rotateRelative(z_rot_);
}
}
}
if (new_command_sent_)
{
new_command_sent_ = false;
x_trans_ = 0.0;
y_trans_ = 0.0;
z_rot_ = 0.0;
}
done_moving_ = true;
std_msgs::String move_done_str;
move_done_str.data = "e_done";
move_done_pub_.publish(move_done_str);
}
}
//
// 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()
