int main(int argc, char** argv)
{
ros::init(argc, argv, "wam_teleop"); // Start our wam_node and name it "wam_teleop"
WamTeleop wam_teleop; // Declare a member of WamTeleop "wam_teleop"
wam_teleop.init(); // Initialize our teleoperation
ros::Rate pub_rate(CNTRL_FREQ); // Setting the publishing rate to CNTRL_FREQ (50Hz by default)
while (wam_teleop.n_.ok()) // Looping at the specified frequency while our ros node is ok
{
ros::spinOnce();
wam_teleop.update(); // Update and send commands to the WAM
pub_rate.sleep();
}
return 0;
}
int wam_main(int argc, char** argv, ::barrett::ProductManager& pm, ::barrett::systems::Wam<DOF>& wam)
{
//BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
ros::init(argc, argv, "wam_node");
WamNode<DOF> wam_node(wam);
wam_node.init(pm);
ros::Rate pub_rate(WamNode<DOF>::PUBLISH_FREQ);
//pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
while (ros::ok() && pm.getSafetyModule()->getMode() == ::barrett::SafetyModule::ACTIVE)
{
ros::spinOnce();
wam_node.publishWam(pm);
//wam_node.updateRT(pm);
pub_rate.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;
}