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; }