/*
void doneCb(const actionlib::SimpleClientGoalState& state,
const cwru_action::trajResultConstPtr& result) {
ROS_INFO(" doneCb: server responded with state [%s]", state.toString().c_str());
ROS_INFO("got return val = %d", result->return_val);
}
*/
int main(int argc, char** argv) {
ros::init(argc, argv, "baxter_cart_move_as");
ros::NodeHandle nh; //standard ros node handle
ROS_INFO("instantiating an ArmMotionInterface: ");
ArmMotionInterface armMotionInterface(&nh);
// start servicing requests:
ROS_INFO("ready to start servicing cartesian-space goals");
while (ros::ok()) {
ros::spinOnce();
ros::Duration(0.1).sleep(); //don't consume much cpu time if not actively working on a command
}
return 0;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "test_cart_path_planner_lib");
ros::NodeHandle nh; //standard ros node handle
ROS_INFO("instantiating an ArmMotionInterface: ");
ArmMotionInterface armMotionInterface(&nh);
//can do this, if needed: A = cartTrajPlanner.get_fk_Affine_from_qvec(Vectorq7x1 q_vec)
cout << "instantiating a traj streamer" << endl; // enter 1:";
//cin>>ans;
// let the streamer be owned by "main"
Baxter_traj_streamer baxter_traj_streamer(&nh); //instantiate a Baxter_traj_streamer object and pass in pointer to nodehandle for constructor to use
// warm up the joint-state callbacks;
cout << "warming up callbacks..." << endl;
for (int i = 0; i < 100; i++) {
ros::spinOnce();
//cout<<"spin "<<i<<endl;
ros::Duration(0.01).sleep();
}
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
cout << "r_arm state:" << g_q_vec_right_arm.transpose() << endl;
//let's leave this in main:
ROS_INFO("instantiating an action client of trajActionServer");
actionlib::SimpleActionClient<baxter_traj_streamer::trajAction> traj_streamer_action_client("trajActionServer", true);
// attempt to connect to the server:
ROS_INFO("waiting for server trajActionServer: ");
bool server_exists = false;
while ((!server_exists)&&(ros::ok())) {
server_exists = traj_streamer_action_client.waitForServer(ros::Duration(5.0)); // wait for up to 5 seconds
ros::spinOnce();
ros::Duration(0.5).sleep();
ROS_INFO("retrying...");
}
// something odd in above: does not seem to wait for 5 seconds, but returns rapidly if server not running
// if (!server_exists) {
// ROS_WARN("could not connect to server; will wait forever");
// return 0; // bail out; optionally, could print a warning message and retry
//}
//server_exists = action_client.waitForServer(); //wait forever
ROS_INFO("connected to action server"); // if here, then we connected to the server;
bool unpackok;
Eigen::Vector3d delta_p;
bool is_valid;
bool finished_before_timeout;
baxter_traj_streamer::trajGoal goal;
Vectorq7x1 q_vec_goal_vq7; //goal pose in joint space, expressed as a 7x1 vector
moveit_msgs::DisplayTrajectory display_trajectory;
ros::Publisher display_traj_pub = nh.advertise<moveit_msgs::DisplayTrajectory>("/preview_traj", 1, true);
// start servicing requests:
while (ros::ok()) {
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm(); // update the current sensed arm angles
//decide if should start processing a new request:
if (armMotionInterface.newRqst()&&!armMotionInterface.isBusy()) {
armMotionInterface.setNewRqst(false); //g_received_new_request_=false; // reset trigger to receive a new request
armMotionInterface.setIsBusy(true); //g_busy_working_on_a_request_= true; // begin processing new request
}
if (armMotionInterface.isBusy()) {
switch (armMotionInterface.get_cmd_mode()) {
case ARM_TEST_MODE:
ROS_INFO("responding to request TEST_MODE: ");
cout << "unpacking q_vec_start from request message..." << endl;
unpackok = armMotionInterface.unpack_qstart();
if (unpackok) {
cout << "start qvec rqst: " << armMotionInterface.get_start_qvec().transpose() << endl;
}
unpackok = armMotionInterface.unpack_qend();
if (unpackok) {
cout << "end qvec rqst: " << armMotionInterface.get_end_qvec().transpose() << endl;
}
//reply w/ populating q_vec_start in response:
armMotionInterface.setIsBusy(false);
break;
case ARM_PLAN_JSPACE_PATH_CURRENT_TO_QGOAL:
ROS_INFO("case ARM_PLAN_JSPACE_PATH_CURRENT_TO_QGOAL");
//get the start q_vec = current arm pose:
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000; //Vectorq7x1 g_q_vec_right_arm;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
//get the goal q_vec, per request message:
armMotionInterface.unpack_qend();
q_vec_goal_vq7= armMotionInterface.get_end_qvec(); //need to convert from VectorXd to Vectorq7x1
is_valid = armMotionInterface.plan_jspace_path_qstart_to_qend(g_q_vec_right_arm,q_vec_goal_vq7);
if (is_valid) ROS_INFO("computed valid path");
else ROS_INFO("no valid path found");
armMotionInterface.setIsBusy(false);
break;
case ARM_PLAN_PATH_CURRENT_TO_PRE_POSE:
ROS_INFO("responding to request PLAN_PATH_CURRENT_TO_PRE_POSE");
armMotionInterface.plan_path_to_predefined_pre_pose();
armMotionInterface.setIsBusy(false);
break;
case ARM_PLAN_PATH_CURRENT_TO_GOAL_POSE:
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
//armMotionInterface.unpack_qstart(); // fills in q_vec_start_rqst_--no...use current arm pose
armMotionInterface.unpack_goal_pose(); //fills in a_flange_end_; should check return status
// bool plan_path_qstart_to_Agoal(Vectorq7x1 q_start); // provide q_start; assumes use of a_flange_end_ and fills in g_optimal_path
is_valid = armMotionInterface.plan_path_qstart_to_Agoal(g_q_vec_right_arm);
if (is_valid) ROS_INFO("computed valid path");
else ROS_INFO("no valid path found");
armMotionInterface.setIsBusy(false);
break;
case ARM_DISPLAY_TRAJECTORY:
cout << "getting current joint states:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
//if here, then joint_states_ is valid
//populate the moveit message to preview the trajectory plan;
//display_trajectory.model_id = "robot_description"; // not sure what goes here; "baxter"??
//declare joint_states_ as start state of display
display_trajectory.trajectory_start.joint_state = baxter_traj_streamer.get_joint_states(); // get the joint states from Baxter;
baxter_traj_streamer.stuff_trajectory(g_optimal_path, g_des_trajectory);//convert planned path to trajectory
display_trajectory.trajectory.resize(1); //display only 1 trajectory
display_trajectory.trajectory[0].joint_trajectory = g_des_trajectory; //the one we care about
display_traj_pub.publish(display_trajectory); // and publish this to topic "/preview_traj"
armMotionInterface.setIsBusy(false);
break;
case ARM_DESCEND_20CM:
ROS_INFO("responding to request DESCEND_20CM");
delta_p << 0, 0, -0.2; //specify delta-p for a 20cm descent
// find a joint-space path to do this, holding R fixed
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
is_valid = armMotionInterface.plan_cartesian_delta_p(g_q_vec_right_arm, delta_p);
armMotionInterface.setPathValid(is_valid);
armMotionInterface.incrementPathID();
armMotionInterface.setIsBusy(false);
break;
case ARM_DEPART_20CM:
ROS_INFO("responding to request ARM_DEPART_20CM");
delta_p << 0, 0, 0.2; //specify delta-p for a 20cm descent
// find a joint-space path to do this, holding R fixed
cout << "getting current right-arm pose:" << endl;
g_q_vec_right_arm[0] = 1000;
while (fabs(g_q_vec_right_arm[0]) > 3) { // keep trying until see viable value populated by fnc
g_q_vec_right_arm = baxter_traj_streamer.get_qvec_right_arm();
ros::spinOnce();
ros::Duration(0.01).sleep();
}
is_valid = armMotionInterface.plan_cartesian_delta_p(g_q_vec_right_arm, delta_p);
armMotionInterface.setPathValid(is_valid);
armMotionInterface.incrementPathID();
armMotionInterface.setIsBusy(false);
break;
//case DEPART_20CM:
//case CART_MOVE_DELTA_P: unpack_delta_p()...
//case PLAN_PATH_QSTART_TO_ADES:
//case PLAN_PATH_QSTART_TO_QGOAL:
//case PLAN_PATH_ASTART_TO_QGOAL:
case ARM_EXECUTE_PLANNED_PATH: //assumes there is a valid planned path in g_optimal_path
ROS_INFO("responding to request EXECUTE_PLANNED_PATH");
if (armMotionInterface.getPathValid()) {
// convert path to a trajectory:
baxter_traj_streamer.stuff_trajectory(g_optimal_path, g_des_trajectory); //convert from vector of 7dof poses to trajectory message
goal.trajectory = g_des_trajectory;
ROS_INFO("sending action request to traj streamer node");
traj_streamer_action_client.sendGoal(goal, &doneCb); // we could also name additional callback functions here, if desired
// action_client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb); //e.g., like this
finished_before_timeout = traj_streamer_action_client.waitForResult(ros::Duration(20.0));
if (!finished_before_timeout) {
ROS_WARN("EXECUTE_PLANNED_PATH: giving up waiting on result");
// should set status in service request here...
} else {
ROS_INFO("finished before timeout");
// should set status in service request here...
}
} else {
ROS_WARN("requested move without valid path! doing nothing");
}
armMotionInterface.setIsBusy(false);
break;
default:
ROS_WARN("this command mode is not defined: %d", armMotionInterface.get_cmd_mode());
//clean up/terminate:
armMotionInterface.setIsBusy(false);
}
}
ros::spinOnce();
//cout<<"main loop..."<<endl;
ros::Duration(0.1).sleep(); //don't consume much cpu time if not actively working on a command
}
return 0;
}
