/**
* @function Load
* @brief
*/
void drchubo_Walk1::Load( physics::ModelPtr _parent,
sdf::ElementPtr _sdf ) {
printf("Calling Load Function \n");
this->mModel = _parent;
this->mWorld = this->mModel->GetWorld();
// Fill the actuated joints
mActuatedJoints.resize( mNumActuatedJoints );
for( int i = 0; i < mNumActuatedJoints; ++i ) {
mActuatedJoints[i] = mModel->GetJoint( mActuatedJointNames[i].c_str() );
}
// Read Trajectory file
ReadTrajectoryFile();
// Set initial hard-coded pose
SetHardCodedInitialPose();
// Initialize control values
SetInitControl();
// Save last update time
mLastUpdatedTime = mModel->GetWorld()->GetSimTime();
// Set to update every world cycle. Listen to update event
this->mUpdateConnection = event::Events::ConnectWorldUpdateBegin( boost::bind(&drchubo_Walk1::UpdateStates, this));
}
int main( int argc, char** argv )
{
/*********** Parse the Trajectory file ************/
// parse the trajectory file
FILE* fCfg = fopen(argv[1], "r");
if(fCfg == NULL)
{
printf("ERROR: unable to open %s\n", argv[1]);
exit(1);
}
std::vector<std::vector<float> > traj;
traj = ReadTrajectoryFile(fCfg);
printf("There are %i steps to this trajectory\n",traj.size());
/*********** Initialize ROS ****************/
ros::init(argc,argv);
ros::node *node = new ros::node("test_arm_traj");
set_arm_traj sat;
node->subscribe("mechanism_state",sat.in, &set_arm_traj::doNothing,10);
signal(SIGINT, finalize);
signal(SIGQUIT, finalize);
signal(SIGTERM, finalize);
/*********** Start moving the robot ************/
double run_time = 25.0;
ros::Time iteration_time;
bool run_time_set = true;
int m,n,g;
node->advertise<pr2_mechanism_controllers::JointPosCmd>("left_arm_commands",10);
sleep(1);
node->publish("left_arm_commands", sat.LeftArmCmd);
sleep(1);
ros::Time start_time = ros::Time::now();
ros::Duration sleep_time(0.01);
//subtract one for now because the goal is all zeros
for (m = 0; m < traj.size(); m++)
{
iteration_time = ros::Time::now();
done = 0;
for(n = 0; n < NUMOFLINKS; n++)
{
sat.LeftArmCmd.positions[n] = double(traj[m].at(n));
}
// publish output
node->publish("left_arm_commands", sat.LeftArmCmd);
while(!done)
{
// ros::Duration delta_time = ros::Time::now() - start_time;
ros::Duration delta_time = ros::Time::now() - iteration_time;
if(run_time_set && delta_time.toSec() > run_time)
{
printf("It's taking too long to get to waypoint #%i...\n\n",m+1);
g = 6;
for (int i = GRIPPER_ROLL_LEFT_JOINT; i <= SHOULDER_PAN_LEFT_JOINT; i++)
{
printf("%s: %2.3f (%2.3f)\n", sat.in.joint_states[i].name.c_str(),sat.in.joint_states[i].position, traj[m].at(g));
g--;
}
printf("\n");
break;
}
if ((sat.in.joint_states[GRIPPER_ROLL_LEFT_JOINT].position + MOE >= traj[m].at(6) &&
sat.in.joint_states[GRIPPER_ROLL_LEFT_JOINT].position - MOE <= traj[m].at(6)) &&
(sat.in.joint_states[WRIST_FLEX_LEFT_JOINT].position + MOE >= traj[m].at(5) &&
sat.in.joint_states[WRIST_FLEX_LEFT_JOINT].position - MOE <= traj[m].at(5)) &&
(sat.in.joint_states[FOREARM_ROLL_LEFT_JOINT].position + MOE >= traj[m].at(4) &&
sat.in.joint_states[FOREARM_ROLL_LEFT_JOINT].position - MOE <= traj[m].at(4)) &&
(sat.in.joint_states[ELBOW_FLEX_LEFT_JOINT].position + MOE >= traj[m].at(3) &&
sat.in.joint_states[ELBOW_FLEX_LEFT_JOINT].position - MOE <= traj[m].at(3)) &&
(sat.in.joint_states[UPPERARM_ROLL_LEFT_JOINT].position + MOE >= traj[m].at(2) &&
sat.in.joint_states[UPPERARM_ROLL_LEFT_JOINT].position - MOE <= traj[m].at(2)) &&
(sat.in.joint_states[SHOULDER_PITCH_LEFT_JOINT].position + MOE >= traj[m].at(1) &&
sat.in.joint_states[SHOULDER_PITCH_LEFT_JOINT].position - MOE <= traj[m].at(1)) &&
(sat.in.joint_states[SHOULDER_PAN_LEFT_JOINT].position + MOE >= traj[m].at(0) &&
sat.in.joint_states[SHOULDER_PAN_LEFT_JOINT].position - MOE <= traj[m].at(0)))
{
printf("\nArm is at waypoint #%i of %i\n\n", m+1, traj.size());
g = 6;
for (int i = GRIPPER_ROLL_LEFT_JOINT; i <= SHOULDER_PAN_LEFT_JOINT; i++)
{
printf("%s: %2.3f (%2.3f)\n", sat.in.joint_states[i].name.c_str(),sat.in.joint_states[i].position, traj[m].at(g));
g--;
}
printf("\n");
done = 1;
}
sleep_time.sleep();
}
}
node->unsubscribe("mechanism_state");
node->unadvertise("left_arm_commands");
ros::fini();
return 0;
}
