bool AriaRobotControl::wait_until_stopped(ros::Duration wait, int max_iterations)
{
wait.sleep();
int count = 0;
//get the state of the robot
while(!get_state_client.call(get_state_srv));
//std::cout << "IS STOPPED: " << get_state_srv.response.isStopped << std::endl;
//wait until the robot has stopped moving to issue a move command
while((!(get_state_srv.response.isStopped) || !(get_state_srv.response.isHeadingDone) \
|| !(get_state_srv.response.isMoveDone)) && count < max_iterations)
{
ROS_INFO("ROBOT STILL moving");
wait.sleep();
while(!get_state_client.call(get_state_srv)){
ROS_INFO("failed service call!");
}
count++;
}
if(get_state_srv.response.isStopped)
{
return true;
}
else
{
return false;
}
}//end is robot stopped
void testForPose(moveit::planning_interface::MoveGroup &group,
geometry_msgs::Pose initial_pose) {
geometry_msgs::Pose destination;
BottlePosition result;
if (!moveTo(group, initial_pose)) {
ROS_ERROR("No plan was found!");
return;
}
stabilisation_duration.sleep();
bool found_bottle = getBottlePositionFromTf(result);
geometry_msgs::Point bottleCapPosition = result.bottleCapPose.position;
ROS_INFO("Found bottle cap at: %f %f %f", bottleCapPosition.x,
bottleCapPosition.y, bottleCapPosition.z);
destination.position.x = bottleCapPosition.x;
destination.position.y = bottleCapPosition.y;
destination.position.z = bottleCapPosition.z + 0.03;
destination.orientation = initial_pose.orientation;
if (!moveTo(group, destination)) {
ROS_ERROR("No plan was found!");
return;
}
stabilisation_duration.sleep();
if (!moveTo(group, initial_pose)) {
ROS_ERROR("No plan was found!");
return;
}
stabilisation_duration.sleep();
}
int Navigator::navigate_to_table() {
std::vector<geometry_msgs::PoseStamped> points;
mobot_pub_des_state::path path_srv;
points.push_back(trajBuilder_.xyPsi2PoseStamped(3.0,0.0,0.0));
path_srv.request.path.poses = points;
append_client_.call(path_srv);
t_ = 0;
looptimer.sleep();
mobot_pub_des_state::emptyQueue emptyQueueMsg;
empty_client_.call(emptyQueueMsg);
while (!emptyQueueMsg.response.empty) {
if (t_ > 25) {
return navigator::navigatorResult::FAILED_CANNOT_REACH_DES_POSE;
}
t_ += 1.0;
looptimer.sleep();
empty_client_.call(emptyQueueMsg);
}
return navigator::navigatorResult::DESIRED_POSE_ACHIEVED; //just say we were successful
}
/** This is a workaround for the case that we're running inside of
rospy and ros::Time is not initialized inside the c++ instance.
This makes the system fall back to Wall time if not initialized.
https://github.com/ros/geometry/issues/30
*/
void sleep_fallback_to_wall(const ros::Duration& d)
{
try
{
d.sleep();
}
catch (ros::TimeNotInitializedException ex)
{
ros::WallDuration wd = ros::WallDuration(d.sec, d.nsec);
wd.sleep();
}
}
BottlePosition findBottlePose(moveit::planning_interface::MoveGroup &group) {
BottlePosition result;
vector<Pose> arm_poses = computeArmPosesForDetection();
vector<BottlePosition> measurements;
for (vector<Pose>::iterator pose = arm_poses.begin();
pose != arm_poses.end(); pose++) {
if (!moveTo(group, *pose)) {
throw std::runtime_error(
"Error in determining bottle pose: The arm could not move to the given pose.");
}
stabilisation_duration.sleep();
BottlePosition bottle_position;
bool found_bottle = getBottlePositionFromTf(bottle_position);
if (found_bottle) {
measurements.push_back(bottle_position);
ROS_INFO("Found bottle at:");
printPose(measurements.back().bottlePose);
ROS_INFO("And cap at:");
printPose(measurements.back().bottleCapPose);
} else {
ROS_WARN("Measurement is ignored");
}
}
if (measurements.size() == 0) {
ROS_ERROR("The bottle was not found from any pose. The program will shut down!");
exit(-1);
}
result = computeBottlePoseFromMeasurements(measurements);
ROS_INFO("Average pose:");
printPose(result.bottlePose);
ROS_INFO("Average cap pose:");
printPose(result.bottleCapPose);
return result;
}
int main(int argc, char **argv)
{
const double radius = 0.5; // 0.5
const double pitch_step = 0.2;
const int circle_points = 6; // 6
const int circle_rings = 3; // 3
const double plan_time = 5;
const int plan_retry = 1;
//const int pose_id_max = circle_points * 3;
const ros::Duration wait_settle(1.0);
const ros::Duration wait_camera(0.5);
const ros::Duration wait_notreached(3.0);
const ros::Duration wait_reset(30.0);
const std::string planning_group = "bumblebee"; //robot, bumblebee
const std::string end_effector = "tool_flange"; // bumblebee_cam1, tool_flange
const std::string plannerId = "PRMkConfigDefault";
const std::string frame_id = "base_link";
ros::init (argc, argv, "simple_pose_mission");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle;
ros::Publisher cameratate_pub = node_handle.advertise<group4_msgs::PointCloudPose>("/robot_rx60b/camerapose", 5);
ros::Publisher markerPublisher = node_handle.advertise<visualization_msgs::MarkerArray>("/simple_pose_mission/poses", 10);
tf::TransformListener listener;
// Marker
MarkerPose marker("object_center");
addObjectCenterMarker(marker);
// Robot and scene
moveit::planning_interface::MoveGroup group(planning_group);
group.setPlannerId(plannerId);
group.setPoseReferenceFrame(frame_id);
//group.setWorkspace(-1.5,-1.5,1.5,1.5,0,1);
group.setGoalTolerance(0.01f);
group.setPlanningTime(plan_time);
group4_msgs::PointCloudPose msg;
msg.header.frame_id = frame_id;
tf::Pose object_center = marker.getPose("object_center");
NumberedPoses poses = generatePoses(object_center, circle_points, circle_rings, pitch_step, radius);
NumberedPoses::iterator poses_itt;
bool poses_updated = true;
while (ros::ok())
{
bool success;
if (poses_updated || poses_itt == poses.end())
{
poses_itt = poses.begin();
poses_updated = false;
}
NumberedPose numbered_pose = *poses_itt;
visualizePoses(poses, frame_id,markerPublisher, numbered_pose.pose_id);
ROS_INFO("Moving to pose %i of %i", numbered_pose.pose_id, numbered_pose.pose_id_max);
tf::StampedTransform transform;
try{
listener.lookupTransform("/bumblebee_cam1", "/tool_flange",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
tf::Pose p = numbered_pose.pose_tf * transform;
geometry_msgs::Pose desired_pose = tfPoseToGeometryPose(p);
group.setPoseTarget(desired_pose, end_effector);
success = false;
for (int itry = 0; itry < plan_retry; itry++)
{
group.setStartStateToCurrentState();
wait_notreached.sleep();
success = group.move();
if (success)
break;
else
ROS_INFO("Plan unsuccessfull.. Retrying! (%i)", itry);
}
if (true)
{
wait_settle.sleep();
geometry_msgs::PoseStamped carmine_pose = group.getCurrentPose("camera_link");
geometry_msgs::PoseStamped bumblebee_pose_left = group.getCurrentPose("bumblebee_cam1");
geometry_msgs::PoseStamped bumblebee_pose_right = group.getCurrentPose("bumblebee_cam2");
msg.header.stamp = ros::Time::now();
msg.pose_id.data = numbered_pose.pose_id;
msg.pose_id_max.data = numbered_pose.pose_id_max;
msg.spin_center_pose = tfPoseToGeometryPose(object_center);
msg.carmine_pose = carmine_pose.pose;
msg.bumblebee_pose_left = bumblebee_pose_left.pose;
msg.bumblebee_pose_right = bumblebee_pose_right.pose;
cameratate_pub.publish(msg);
ROS_INFO("Waiting %f seconds for camera.", wait_camera.toSec());
wait_camera.sleep();
}
if (poses_itt == poses.end())
{
ROS_INFO("Last pose reached. Waiting %f seconds before resetting", wait_reset.toSec());
poses_itt = poses.begin();
wait_reset.sleep();
}
else
poses_itt++;
}
group.stop();
return 0;
}
