poRect poObject::getFrame() {
poRect rect = getBounds();
poPoint topLeft = getTransformedPoint( rect.getTopLeft() );
poPoint topRight = getTransformedPoint( rect.getTopRight() );
poPoint bottomRight = getTransformedPoint( rect.getBottomRight() );
poPoint bottomLeft = getTransformedPoint( rect.getBottomLeft() );
poRect frame;
frame.setPosition( bottomRight );
frame.include( topLeft );
frame.include( topRight );
frame.include( bottomLeft );
return frame;
}
bool BaseMotionController::translateRelative(double x_trans, double y_trans)
{
geometry_msgs::Twist zero_velocity;
while (!odom_received_)
{
ros::spinOnce();
if (!start_relative_movement_)
{
base_velocities_pub_.publish(zero_velocity);
return false;
}
}
odom_received_ = false;
tf::TransformListener tf_listener;
tf::StampedTransform robot_transform;
bool tf_received = false;
// wait for transform
// this is required to convert odom values (x and y position) to the base_footprint frame
while (!tf_received)
{
try
{
tf_listener.lookupTransform(world_frame_tf_, base_frame_tf_, ros::Time(0), robot_transform);
tf_received = true;
}
catch(tf::TransformException &ex)
{
}
}
// current orientation of the robot w.r.t starting orientation
double robot_yaw = tf::getYaw(robot_transform.getRotation());
// transform point to base frame
geometry_msgs::Point point;
point.x = x_current_;
point.y = y_current_;
geometry_msgs::Point newPoint = getTransformedPoint(point, robot_yaw);
// calculate desired x and y positions
double x_goal = newPoint.x + x_trans;
double y_goal = newPoint.y + y_trans;
ROS_DEBUG("Current: x: %f, y: %f ", x_current_, y_current_);
ROS_DEBUG("Goal: x: %f, y: %f ", x_goal, y_goal);
// if already at goal positions, set velocity to zero and return
if (isMovementDone(x_goal, newPoint.x) && isMovementDone(y_goal, newPoint.y))
{
base_velocities_pub_.publish(zero_velocity);
return true;
}
// while either x or y translation needs to be done
while (!isMovementDone(x_goal, newPoint.x) || !isMovementDone(y_goal, newPoint.y))
{
geometry_msgs::Twist base_velocity;
// if x translation is required
if (!isMovementDone(x_goal, newPoint.x))
{
if (x_goal - newPoint.x < 0.0)
{
base_velocity.linear.x = -x_vel_;
}
else
{
base_velocity.linear.x = x_vel_;
}
}
// if y translation is required
if (!isMovementDone(y_goal, newPoint.y))
{
if (y_goal - newPoint.y < 0.0)
{
base_velocity.linear.y = -y_vel_;
}
else
{
base_velocity.linear.y = y_vel_;
}
}
// set velocity
base_velocities_pub_.publish(base_velocity);
// wait till we receive another odom reading
while (!odom_received_)
{
ros::spinOnce();
if (!start_relative_movement_)
{
base_velocities_pub_.publish(zero_velocity);
return false;
}
}
odom_received_ = false;
// transform point to base frame
point.x = x_current_;
point.y = y_current_;
newPoint = getTransformedPoint(point, robot_yaw);
}
// set velocity to zero
base_velocities_pub_.publish(zero_velocity);
return true;
}
