int main (int argc, char **argv)
{
ros::init(argc, argv, "test_averaging");
// create the action client
actionlib::SimpleActionClient<learning_actionlib::AveragingAction> ac("averaging");
boost::thread spin_thread(&spinThread);
ROS_INFO("Waiting for action server to start.");
ac.waitForServer();
ROS_INFO("Action server started, sending goal.");
// send a goal to the action
learning_actionlib::AveragingGoal goal;
goal.samples = 100;
ac.sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
if (finished_before_timeout)
{
actionlib::SimpleClientGoalState state = ac.getState();
ROS_INFO("Action finished: %s",state.toString().c_str());
}
else
ROS_INFO("Action did not finish before the time out.");
// shutdown the node and join the thread back before exiting
ros::shutdown();
spin_thread.join();
//exit
return 0;
}
TEST(MoveArm, goToJointGoal)
{
ros::NodeHandle nh;
ros::NodeHandle private_handle("~");
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
boost::thread spin_thread(&spinThread);
move_arm.waitForServer();
ROS_INFO("Connected to server");
arm_navigation_msgs::MoveArmGoal goalB;
std::vector<std::string> names(7);
names[0] = "r_shoulder_pan_joint";
names[1] = "r_shoulder_lift_joint";
names[2] = "r_upper_arm_roll_joint";
names[3] = "r_elbow_flex_joint";
names[4] = "r_forearm_roll_joint";
names[5] = "r_wrist_flex_joint";
names[6] = "r_wrist_roll_joint";
goalB.motion_plan_request.group_name = "right_arm";
private_handle.param<std::string>("planner_id",goalB.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
private_handle.param<std::string>("planner_service_name",goalB.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
{
// goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.stamp = ros::Time::now();
// goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.frame_id = "base_link";
goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
}
goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = 0.15;
if (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goalB);
finished_within_time = move_arm.waitForResult(ros::Duration(10.0));
actionlib::SimpleClientGoalState state = move_arm.getState();
bool success = (state == actionlib::SimpleClientGoalState::ABORTED);
EXPECT_TRUE(success);
ROS_INFO("Action finished: %s",state.toString().c_str());
}
ros::shutdown();
spin_thread.join();
}
TEST(CollisionMapTest, collisionMapGroundPlaneTest)
{
boost::thread spin_thread(&spinThread);
collision_map::CollisionMapTest cmap;
ros::Duration dd(1.0);
ros::NodeHandle nh;
while (nh.ok() && !cmap.done_)
{
dd.sleep();
}
ros::shutdown();
spin_thread.join();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "TheMasterNode");
ros::NodeHandle nh;
// here task_commonsserver is the name of the node of the actionserver.
ros::Subscriber subOrange =
nh.subscribe<task_commons::orangeActionFeedback>("/task_commonsserver/feedback", 1000, &forwardCb);
ros::Subscriber subalign = nh.subscribe<task_commons::alignActionFeedback>("/align/feedback", 1000, &alignCb);
orange orangeClient("task_commonsserver");
align alignClient("align");
ROS_INFO("Waiting for task_commons server to start.");
orangeClient.waitForServer();
ROS_INFO("task_commons server started");
ROS_INFO("Waiting for align server to start.");
alignClient.waitForServer();
ROS_INFO("align server started.");
boost::thread spin_thread(&spinThread);
orangegoal.order = true;
orangeClient.sendGoal(orangegoal);
ROS_INFO("Orange detection started");
orangeClient.waitForResult();
orangeSuccess = (*(orangeClient.getResult())).MotionCompleted;
if (orangeSuccess)
{
ROS_INFO("orange colour detected");
}
else
ROS_INFO("orange not detected");
aligngoal.StartDetection = true;
alignClient.sendGoal(aligngoal);
ROS_INFO("alignment started");
alignClient.waitForResult();
alignSuccess = (*(orangeClient.getResult())).MotionCompleted;
if (alignSuccess)
{
ROS_INFO("alignment successful");
}
else
ROS_INFO("alignment failed");
return 0;
}
int main (int argc, char **argv)
{
ros::init(argc, argv, "test_hoverHeight");
//create the action client
actionlib::SimpleActionClient<my_hector::hoverHeightAction> ac("hoverHeight");
boost::thread spin_thread(&spinThread);
ROS_INFO("Waiting for action server to start.");
ac.waitForServer();
ROS_INFO("Action server started, sending goal.");
//send a goal to the action
my_hector::hoverHeightGoal goal;
//move to altitude equal to 1m
goal.height = 1.0;
ac.sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = ac.waitForResult(ros::Duration(60.0));
if (finished_before_timeout)
{
actionlib::SimpleClientGoalState state = ac.getState();
ROS_INFO("Action finished: %s",state.toString().c_str());
}
else
ROS_INFO("Action did not finish before the time out.");
//shutdown the node and join the thread back before exiting
ros::shutdown();
spin_thread.join();
//exit
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "test_controller");
boost::thread spin_thread(&spinThread);
JointExecutorActionClient *traj_action_client_ = new JointExecutorActionClient("collision_free_arm_trajectory_action_right_arm");
while(!traj_action_client_->waitForServer(ros::Duration(1.0))){
ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
if(!ros::ok()) {
exit(0);
}
}
pr2_controllers_msgs::JointTrajectoryGoal goal;
goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
goal.trajectory.points.resize(1);
for(unsigned int i=0; i < 7; i++)
goal.trajectory.points[0].positions.push_back(0.0);
goal.trajectory.points[0].positions[0] = -1.57/2.0;
goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
traj_action_client_->sendGoal(goal);
ROS_INFO("Sent goal");
while(!traj_action_client_->getState().isDone() && ros::ok())
{
ros::Duration(0.1).sleep();
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pr2_telep_general_joystick", ros::init_options::NoSigintHandler);
//signal(SIGINT,quit);
boost::thread spin_thread(boost::bind(&spin_function));
Pr2TeleopGeneralJoystick generaljoy;
generaljoy.init();
ros::Rate pub_rate(FastHz);
unsigned int counter_limit = (unsigned int)(FastHz/SlowHz);
unsigned int counter = 0;
ros::Time beforeCall = ros::Time::now();
ros::Time afterCall = ros::Time::now();
while (ros::ok())
{
//ROS_INFO_STREAM("Time since last " << (ros::Time::now()-beforeCall).toSec());
beforeCall = ros::Time::now();
if(!generaljoy.gc->isWalkAlongOk() && !generaljoy.set_walk_along_mode_ && !generaljoy.walk_along_init_waiting_) {
if(generaljoy.convertCurrentVelToDesiredHeadPos(FastHz)) {
generaljoy.gc->sendHeadCommand(generaljoy.des_pan_pos_, generaljoy.des_tilt_pos_);
}
generaljoy.gc->sendHeadTrackCommand();
generaljoy.gc->sendBaseCommand(generaljoy.des_vx_, generaljoy.des_vy_, generaljoy.des_vw_);
}
if((counter % counter_limit) == 0) {
counter = 0;
if(generaljoy.set_walk_along_mode_) {
bool ok = generaljoy.gc->moveToWalkAlongArmPose();
//if we didn't get a select while moving the arms
if(ok && generaljoy.set_walk_along_mode_) {
ROS_INFO("Arms in walk position");
generaljoy.walk_along_init_waiting_ = true;
} else {
ROS_INFO("Arms not in walk position");
}
generaljoy.set_walk_along_mode_ = false;
}
if(generaljoy.gc->isWalkAlongOk()) {
generaljoy.gc->sendWalkAlongCommand(generaljoy.walk_along_thresh_,
generaljoy.walk_along_x_dist_max_,
generaljoy.walk_along_x_speed_scale_,
generaljoy.walk_along_y_dist_max_,
generaljoy.walk_along_y_speed_scale_,
generaljoy.walk_along_w_speed_scale_);
} else {
if(generaljoy.convertCurrentVelToDesiredTorsoPos(SlowHz)) {
generaljoy.gc->sendTorsoCommand(generaljoy.des_torso_pos_, generaljoy.des_torso_vel_);
}
//generaljoy.gc->updateCurrentWristPositions();
generaljoy.gc->sendWristVelCommands(generaljoy.des_right_wrist_vel_, generaljoy.des_left_wrist_vel_, SlowHz);
generaljoy.gc->sendArmVelCommands(generaljoy.right_arm_vx_, generaljoy.right_arm_vy_, generaljoy.right_arm_vz_, generaljoy.right_arm_vel_roll_, generaljoy.right_arm_vel_pitch_, generaljoy.right_arm_vel_yaw_,
generaljoy.left_arm_vx_, generaljoy.left_arm_vy_, generaljoy.left_arm_vz_, generaljoy.left_arm_vel_roll_, generaljoy.left_arm_vel_pitch_, generaljoy.left_arm_vel_yaw_,
SlowHz);
}
}
//ROS_INFO_STREAM("Everything took " << (afterCall-beforeCall).toSec());
counter++;
pub_rate.sleep();
}
ros::shutdown();
spin_thread.join();
return 0;
}
TEST(MoveArm, goToPoseGoal)
{
ros::NodeHandle nh;
actionlib::SimpleActionClient<move_arm::MoveArmAction> move_arm(nh, "move_right_arm");
boost::thread spin_thread(&spinThread);
move_arm.waitForServer();
ROS_INFO("Connected to server");
move_arm::MoveArmGoal goalA;
//getting a monitor so that we can track the configuration of the arm
planning_environment::RobotModels rm("robot_description");
EXPECT_TRUE(rm.loadedModels());
tf::TransformListener tf;
planning_environment::KinematicModelStateMonitor km(&rm, &tf);
km.waitForState();
//should have the state at this point
goalA.goal_constraints.set_pose_constraint_size(1);
goalA.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
goalA.goal_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
//starting configuration
//-position [x, y, z] = [0.77025, -.18, 0.73]
//-rotation [x, y, z, w] = [0, -0.05, 0, 0]
goalA.goal_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X
+ arm_navigation_msgs::PoseConstraint::POSITION_Y
+ arm_navigation_msgs::PoseConstraint::POSITION_Z
+ arm_navigation_msgs::PoseConstraint::ORIENTATION_R
+ arm_navigation_msgs::PoseConstraint::ORIENTATION_P
+ arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
goalA.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
goalA.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.60;
goalA.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.25;
goalA.goal_constraints.pose_constraint[0].pose.pose.position.z = 0.84;
goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
goalA.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.01;
goalA.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.01;
goalA.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.01;
goalA.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.01;
goalA.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
goalA.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.01;
goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.01;
goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.01;
goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.01;
goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.01;
goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.01;
goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.01;
goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
goalA.contacts.resize(2);
goalA.contacts[0].links.push_back("r_gripper_l_finger_link");
goalA.contacts[0].links.push_back("r_gripper_r_finger_link");
goalA.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
goalA.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
goalA.contacts[0].depth = 0.04;
goalA.contacts[0].bound.type = arm_navigation_msgs::Object::SPHERE;
goalA.contacts[0].bound.dimensions.push_back(0.3);
goalA.contacts[0].pose = goalA.goal_constraints.pose_constraint[0].pose;
goalA.contacts[1].links.push_back("r_gripper_palm_link");
goalA.contacts[1].links.push_back("r_wrist_roll_link");
goalA.contacts[1].depth = 0.02;
goalA.contacts[1].bound.type = arm_navigation_msgs::Object::SPHERE;
goalA.contacts[1].bound.dimensions.push_back(0.2);
goalA.contacts[1].pose = goalA.goal_constraints.pose_constraint[0].pose;
if (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goalA);
finished_within_time = move_arm.waitForResult(ros::Duration(10.0));
EXPECT_TRUE(finished_within_time);
if (!finished_within_time)
{
move_arm.cancelGoal();
ROS_INFO("Timed out achieving goal A");
}
else
{
actionlib::SimpleClientGoalState state = move_arm.getState();
ROS_INFO("Action finished: %s",state.toString().c_str());
EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
double dist_pose;
double dist_angle;
diffConfig(km,goalA,dist_pose,dist_angle);
//close enough - summed tolerances
EXPECT_TRUE(dist_pose < .005+.005+.01);
EXPECT_TRUE(dist_angle < .005*3);
EXPECT_TRUE(finalStateMatchesGoal(km,goalA));
}
}
nh.setParam( "/move_right_arm/long_range_only", true);
move_arm::MoveArmGoal goalB;
//starting configuration
//-position [x, y, z] = [0.77025, -.18, 0.73]
//-rotation [x, y, z, w] = [0, -0.05, 0, 0]
goalB.goal_constraints.set_pose_constraint_size(1);
goalB.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
goalB.goal_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
goalB.goal_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X
+ arm_navigation_msgs::PoseConstraint::POSITION_Y
+ arm_navigation_msgs::PoseConstraint::POSITION_Z
+ arm_navigation_msgs::PoseConstraint::ORIENTATION_R
+ arm_navigation_msgs::PoseConstraint::ORIENTATION_P
+ arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
goalB.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
goalB.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.60;
goalB.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.25;
goalB.goal_constraints.pose_constraint[0].pose.pose.position.z = 0.5;
goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
goalB.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.05;
goalB.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.05;
goalB.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.05;
goalB.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.05;
goalB.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.05;
goalB.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.05;
goalB.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.05;
goalB.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.05;
goalB.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.05;
goalB.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.05;
goalB.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.05;
goalB.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.05;
goalB.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
goalB.path_constraints.set_pose_constraint_size(1);
goalB.path_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
goalB.path_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
goalB.path_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X
+ arm_navigation_msgs::PoseConstraint::POSITION_Y
+ arm_navigation_msgs::PoseConstraint::POSITION_Z
+ arm_navigation_msgs::PoseConstraint::ORIENTATION_R
+ arm_navigation_msgs::PoseConstraint::ORIENTATION_P;
// + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
goalB.path_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
goalB.path_constraints.pose_constraint[0].pose.pose.position.x = 0.60;
goalB.path_constraints.pose_constraint[0].pose.pose.position.y = -0.25;
goalB.path_constraints.pose_constraint[0].pose.pose.position.z = .45;
goalB.path_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
goalB.path_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
goalB.path_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
goalB.path_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
goalB.path_constraints.pose_constraint[0].position_tolerance_above.x = 0.1;
goalB.path_constraints.pose_constraint[0].position_tolerance_above.y = 0.1;
goalB.path_constraints.pose_constraint[0].position_tolerance_below.x = 0.1;
goalB.path_constraints.pose_constraint[0].position_tolerance_below.y = 0.1;
goalB.path_constraints.pose_constraint[0].position_tolerance_above.z = 10.0;
goalB.path_constraints.pose_constraint[0].position_tolerance_below.z = 0.1;
goalB.path_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.1;
goalB.path_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.1;
// goalB.path_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.05;
goalB.path_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.1;
goalB.path_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.1;
// goalB.path_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.05;
goalB.path_constraints.pose_constraint[0].orientation_importance = 0.4;
goalB.contacts.resize(2);
goalB.contacts[0].links.push_back("r_gripper_l_finger_link");
goalB.contacts[0].links.push_back("r_gripper_r_finger_link");
goalB.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
goalB.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
goalB.contacts[0].depth = 0.04;
goalB.contacts[0].bound.type = arm_navigation_msgs::Object::SPHERE;
goalB.contacts[0].bound.dimensions.push_back(0.3);
goalB.contacts[0].pose = goalB.goal_constraints.pose_constraint[0].pose;
goalB.contacts[1].links.push_back("r_gripper_palm_link");
goalB.contacts[1].links.push_back("r_wrist_roll_link");
goalB.contacts[1].depth = 0.02;
goalB.contacts[1].bound.type = arm_navigation_msgs::Object::SPHERE;
goalB.contacts[1].bound.dimensions.push_back(0.2);
goalB.contacts[1].pose = goalB.goal_constraints.pose_constraint[0].pose;
if (nh.ok())
{
move_arm.sendGoal(goalB);
ros::Time start_time = ros::Time::now();
ros::Duration elapsed(0.0);
//trying ticks in case time gets wonky
unsigned int ticks=0;
while(1) {
bool result_during_cycle = move_arm.waitForResult(ros::Duration(.2));
//got some result before time out
if(result_during_cycle) {
break;
}
elapsed = ros::Time::now()-start_time;
std::cout << "Time " << elapsed.toSec() << std::endl;
//checking if we've gone over max time - if so bail
if(elapsed.toSec() > 10.0 || ticks++ > 50) break;
goalB.path_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
goalB.path_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
//check that the path obeys constraints
EXPECT_TRUE(currentStateSatisfiesPathConstraints(km,goalB));
}
if (elapsed.toSec() > 10.0)
{
move_arm.cancelGoal();
ROS_INFO("Timed out achieving goal A");
EXPECT_TRUE(elapsed.toSec() < 10.0);
}
else
{
actionlib::SimpleClientGoalState state = move_arm.getState();
ROS_INFO("Action finished: %s",state.toString().c_str());
EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
double dist_pose;
double dist_angle;
diffConfig(km,goalA,dist_pose,dist_angle);
//close enough - summed tolerances
EXPECT_TRUE(dist_pose < .01+.01+.002);
EXPECT_TRUE(dist_angle < .05*3);
EXPECT_TRUE(finalStateMatchesGoal(km,goalB));
}
}
ros::shutdown();
spin_thread.join();
}
TEST(MoveArm, goToPoseGoal)
{
ros::NodeHandle nh;
ros::NodeHandle private_handle("~");
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
boost::thread spin_thread(&spinThread);
move_arm.waitForServer();
ROS_INFO("Connected to server");
arm_navigation_msgs::MoveArmGoal goalA;
goalA.motion_plan_request.group_name = "right_arm";
goalA.motion_plan_request.num_planning_attempts = 1;
private_handle.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
private_handle.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
goalA.motion_plan_request.allowed_planning_time = ros::Duration(10.0);
goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.15;
goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.95;
goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices.resize(1);
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].x = 0.15;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].y = -0.95;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].z = 0.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.x = 0.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.y = 0.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.z = 0.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = -0.7071;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 0.7071;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.2;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.2;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.2;
goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
/* move arm should send the pose constraint straight to the planner */
goalA.disable_ik = true;
std::vector<std::string> names(7);
names[0] = "r_shoulder_pan_joint";
names[1] = "r_shoulder_lift_joint";
names[2] = "r_upper_arm_roll_joint";
names[3] = "r_elbow_flex_joint";
names[4] = "r_forearm_roll_joint";
names[5] = "r_wrist_flex_joint";
names[6] = "r_wrist_roll_joint";
int num_test_attempts = 0;
int max_attempts = 5;
bool success = false;
while (nh.ok())
{
bool finished_within_time = false;
move_arm.sendGoal(goalA);
finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
actionlib::SimpleClientGoalState state = move_arm.getState();
success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if ((!finished_within_time || !success) && num_test_attempts < max_attempts)
{
move_arm.cancelAllGoals();
ROS_INFO("Timed out achieving goal A, trying again. Trying again, attempt: %d",num_test_attempts);
num_test_attempts++;
}
else
{
if(!success)
{
ROS_INFO("Action unsuccessful");
move_arm.cancelAllGoals();
}
ROS_INFO("Action finished: %s",state.toString().c_str());
break;
}
}
EXPECT_TRUE(success);
ros::shutdown();
spin_thread.join();
}
