// Rotate as specified in rad (based on base odometry information) with given speed in rad/s
bool Base::turn(bool clockwise, double radians, double speed)
{
while (radians < 0) radians += 2 * M_PI;
while (radians > 2 * M_PI) radians -= 2 * M_PI;
tf::StampedTransform start_transform;
tf::StampedTransform current_transform;
// Wait and get transform
tf_listener_odom_.waitForTransform("base_footprint", "odom_combined", ros::Time(0), ros::Duration(5.0));
tf_listener_odom_.lookupTransform("base_footprint", "odom_combined", ros::Time(0), start_transform);
// Set cmd_
cmd_.linear.x = cmd_.linear.y = cmd_.linear.z = 0.0;
cmd_.angular.z = speed;
if (clockwise)
cmd_.angular.z = -speed ;
// The axis we want to be rotating by
tf::Vector3 desired_turn_axis(0, 0, 1);
if (!clockwise)
desired_turn_axis = -desired_turn_axis;
// Loop until finished turn
ros::Rate rate(PUBLISH_RATE_);
bool done = false;
while (!done && nh_.ok()) {
// Send the drive command
pub_base_vel_.publish(cmd_);
rate.sleep();
// Get the current transform
try {
tf_listener_odom_.lookupTransform("base_footprint", "odom_combined", ros::Time(0), current_transform);
} catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
break;
}
tf::Transform relative_transform = start_transform.inverse() * current_transform;
tf::Vector3 actual_turn_axis = relative_transform.getRotation().getAxis();
double angle_turned = relative_transform.getRotation().getAngle();
if (fabs(angle_turned) < 1.0e-2)
continue;
if (actual_turn_axis.dot(desired_turn_axis) < 0)
angle_turned = 2 * M_PI - angle_turned;
if (angle_turned > radians)
done = true;
}
return done;
}
bool turnOdom(double radians)
{
// If the distance to travel is negligble, don't even try.
if (fabs(radians) < 0.01)
return true;
while(radians < -M_PI) radians += 2*M_PI;
while(radians > M_PI) radians -= 2*M_PI;
//we will record transforms here
tf::StampedTransform start_transform;
tf::StampedTransform current_transform;
try
{
//wait for the listener to get the first message
listener_.waitForTransform(base_frame, odom_frame,
ros::Time::now(), ros::Duration(1.0));
//record the starting transform from the odometry to the base frame
listener_.lookupTransform(base_frame, odom_frame,
ros::Time(0), start_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
return false;
}
//we will be sending commands of type "twist"
geometry_msgs::Twist base_cmd;
//the command will be to turn at 0.75 rad/s
base_cmd.linear.x = base_cmd.linear.y = 0.0;
base_cmd.angular.z = turn_rate;
if (radians < 0)
base_cmd.angular.z = -turn_rate;
//the axis we want to be rotating by
tf::Vector3 desired_turn_axis(0,0,1);
ros::Rate rate(25.0);
bool done = false;
while (!done && nh_.ok() && as_.isActive())
{
//send the drive command
cmd_vel_pub_.publish(base_cmd);
rate.sleep();
//get the current transform
try
{
listener_.lookupTransform(base_frame, odom_frame,
ros::Time(0), current_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
break;
}
tf::Transform relative_transform =
start_transform.inverse() * current_transform;
tf::Vector3 actual_turn_axis =
relative_transform.getRotation().getAxis();
double angle_turned = relative_transform.getRotation().getAngle();
// Update feedback and result.
feedback_.turn_distance = angle_turned;
result_.turn_distance = angle_turned;
as_.publishFeedback(feedback_);
if ( fabs(angle_turned) < 1.0e-2) continue;
//if ( actual_turn_axis.dot( desired_turn_axis ) < 0 )
// angle_turned = 2 * M_PI - angle_turned;
if (fabs(angle_turned) > fabs(radians)) done = true;
}
if (done) return true;
return false;
}
