int main(int argc, char** argv)
{
Aria::init();
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector con(&argParser);
ArRobot robot;
ArSonarDevice sonar;
argParser.loadDefaultArguments();
if(!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
return 1;
}
/* - the action group for teleoperation actions: */
teleop = new ArActionGroup(&robot);
// don't hit any tables (if the robot has IR table sensors)
teleop->addAction(new ArActionLimiterTableSensor, 100);
// limiter for close obstacles
teleop->addAction(new ArActionLimiterForwards("speed limiter near",
300, 600, 250), 95);
// limiter for far away obstacles
teleop->addAction(new ArActionLimiterForwards("speed limiter far",
300, 1100, 400), 90);
// limiter so we don't bump things backwards
teleop->addAction(new ArActionLimiterBackwards, 85);
// the joydrive action (drive from joystick)
ArActionJoydrive joydriveAct("joydrive", 400, 15);
teleop->addAction(&joydriveAct, 50);
// the keydrive action (drive from keyboard)
teleop->addAction(new ArActionKeydrive, 45);
/* - the action group for wander actions: */
wander = new ArActionGroup(&robot);
// if we're stalled we want to back up and recover
wander->addAction(new ArActionStallRecover, 100);
// react to bumpers
wander->addAction(new ArActionBumpers, 75);
// turn to avoid things closer to us
wander->addAction(new ArActionAvoidFront("Avoid Front Near", 225, 0), 50);
// turn avoid things further away
wander->addAction(new ArActionAvoidFront, 45);
// keep moving
wander->addAction(new ArActionConstantVelocity("Constant Velocity", 400), 25);
/* - use key commands to switch modes, and use keyboard
* and joystick as inputs for teleoperation actions. */
// create key handler if Aria does not already have one
ArKeyHandler *keyHandler = Aria::getKeyHandler();
if (keyHandler == NULL)
{
keyHandler = new ArKeyHandler;
Aria::setKeyHandler(keyHandler);
robot.attachKeyHandler(keyHandler);
}
// set the callbacks
ArGlobalFunctor teleopCB(&teleopMode);
ArGlobalFunctor wanderCB(&wanderMode);
keyHandler->addKeyHandler('w', &wanderCB);
keyHandler->addKeyHandler('W', &wanderCB);
keyHandler->addKeyHandler('t', &teleopCB);
keyHandler->addKeyHandler('T', &teleopCB);
// if we don't have a joystick, let 'em know
if (!joydriveAct.joystickInited())
printf("Note: Do not have a joystick, only the arrow keys on the keyboard will work.\n");
// set the joystick so it won't do anything if the button isn't pressed
joydriveAct.setStopIfNoButtonPressed(false);
/* - connect to the robot, then enter teleoperation mode. */
robot.addRangeDevice(&sonar);
if(!con.connectRobot(&robot))
{
ArLog::log(ArLog::Terse, "actionGroupExample: Could not connect to the robot.");
Aria::shutdown();
return 1;
}
robot.enableMotors();
teleopMode();
robot.run(true);
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ros::init(argc, argv, "teleoparm");
ArLog::init(ArLog::StdErr, ArLog::Normal);
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
//ArRobot robot;
ArRobotConnector robotConnector(&parser, &robot);
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "terabotArmDemo: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
//ArTerabotArm arm(&robot);
ArLog::log(ArLog::Normal, "terabotArmDemo: Connected to mobile robot.");
if(!arm.open())
{
ArLog::log(ArLog::Terse, "terabotArmDemo: Error opening serial connection to arm");
Aria::exit(1);
}
robot.runAsync(true);
arm.powerOn();
arm.reset();
arm.enable();
arm.setAllJointSpeeds(defaultJointSpeed);
ArLog::log(ArLog::Terse, "\nWARNING\n\nAbout to move the manipulator arm to the front of the robot in 5 seconds. Press Control-C to cancel and exit the program.\n...");
ArUtil::sleep(5000);
ArLog::log(ArLog::Terse, "Now moving the arm...");
// arm.moveArm(forwardReady);
//arm.moveArm(park);
ArUtil::sleep(500); // need to have read some data from the arm for key handler to work
robot.lock();
ArModeUnguardedTeleop unguardedTeleopMode(&robot, "unguarded teleop", 'u', 'U');
ArModeTeleop teleopMode(&robot, "teleop", 't', 'T');
ArModeLaser laserMode(&robot, "laser", 'l', 'L');
ArModeCommand commandMode(&robot, "direct robot commands", 'd', 'D');
ros::NodeHandle n;
sensor_msgs::JointState joints;
ros::Rate loop_rate(10);
ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("/joint_states", 50);//50 1000
ros::Subscriber arm_sub_ = n.subscribe<sensor_msgs::JointState>("/joint_states", 10, move_arm);//it was 100
float positions[5];
// float park1 [5]= {50, -95,-50, -149, 0};
float park1 [5]= {149, -94, 155, -149, 0};//this is th real park
float initial [5]= {-30.0, 0.0, 0.0, 0.0, 0.0};
// std::cout<<"Gripper Status before:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status before:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status before:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status before:"<<arm.getGripperStatus()<<"\n";
// arm.grip(-100);
// std::cout<<"Gripper Status after:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status after:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status after:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status after:"<<arm.getGripperStatus()<<"\n";
//*********testing**************************
// arm.moveArm(start);
//arm.closeGripper();
// arm.moveArm(park1);
arm.moveArm(forwardReady);// used when the manual control GUI is used
ArUtil::sleep(20000);// time for the non blocking predefined movements to take place
//******************************************
while(ros::ok())
{
joints.name.push_back("j1");
joints.name.push_back("j2");
joints.name.push_back("j3");
joints.name.push_back("j4");
joints.name.push_back("j5");
// why is there no joint
//joints.position.push_back(11);//<<
joints.position.clear();
joints.position.push_back(positions[0]);
joints.position.push_back(positions[1]);
joints.position.push_back(positions[2]);
joints.position.push_back(positions[3]);
joints.position.push_back(positions[4]);
joint_pub.publish(joints);
//arm.getJointStatus(&j1,&j2,&j3,&j4,&j5);
arm.getArmPos(positions);
// std::cout<<"j1:"<<j1<<"j2:"<<j2<<"j3:"<<j3<<"j4:"<<j4<<"j5:"<<j5<<"\n"; fflush(stdout);
std::cout<<"position1:"<<positions[0]<<"position2:"<<positions[1]<<"position3:"<<positions[2]<<"position4:"<<positions[3]<<"position5:"<<positions[4]<<"\n"; fflush(stdout);
//ros::spin();
ros::spinOnce();
loop_rate.sleep();
// std::cout<<"loopend\n";
}
robot.enableMotors();
robot.unlock();
robot.waitForRunExit();
ArLog::log(ArLog::Normal, "terabotArmDemo: Either mobile robot or arm disconnected. Exiting.");
Aria::exit(0);
}
