int main(int argc, char **argv)
{
ros::init (argc, argv, "hsmakata_pick_n_place_demo");
ros::NodeHandle nh;
ros::Publisher display_publisher = nh.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
ros::AsyncSpinner spinner(1);
spinner.start();
pub_co = nh.advertise<moveit_msgs::CollisionObject>("collision_object", 10);
pub_aco = nh.advertise<moveit_msgs::AttachedCollisionObject>("attached_collision_object", 10);
grasps_marker = nh.advertise<visualization_msgs::MarkerArray>("grasps_marker", 10);
sub_point = nh.subscribe<visualization_msgs::Marker>("cup_center",1,cb_points);
moveit::planning_interface::MoveGroup gripper("gripper");
gripper.setNamedTarget("closed");
gripper.move();
moveit::planning_interface::MoveGroup katana("manipulator");
katana.setNamedTarget("home");
katana.move();
ros::spin();
}
int main(int argc, char** argv){
ros::init(argc, argv, "simple_gripper");
ros::NodeHandle nh;
left_Gripper gripper(nh);
gripper.close();
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "gripper_v5_ros_control_node");
std::vector<std::string> actr_names;
std::vector<std::string> jnt_names;
std::vector<std::string> controller_names;
std::vector<double> reducers;
int rate_hz;
if (!(ros::param::get("~actuator_names", actr_names) && ros::param::get("~joint_names", jnt_names) &&
ros::param::get("~controller_names", controller_names) && ros::param::get("~mechanical_reduction", reducers) &&
ros::param::get("~control_rate", rate_hz)))
{
ROS_ERROR("Couldn't get necessary parameters");
return 0;
}
GripperRosControl gripper(actr_names, jnt_names, controller_names, reducers);
controller_manager::ControllerManager cm(&gripper);
// For non-realtime spinner thread
ros::AsyncSpinner spinner(1);
spinner.start();
// Control loop
ros::Rate rate(rate_hz);
ros::Time prev_time = ros::Time::now();
while (ros::ok())
{
const ros::Time now = ros::Time::now();
const ros::Duration elapsed_time = now - prev_time;
gripper.read();
cm.update(now, elapsed_time);
gripper.write();
prev_time = now;
rate.sleep();
}
spinner.stop();
gripper.cleanup();
return 0;
}
int main(int argc, char** argv)
{
// Initialize some global data
Aria::init();
// If you want ArLog to print "Verbose" level messages uncomment this:
//ArLog::init(ArLog::StdOut, ArLog::Verbose);
// This object parses program options from the command line
ArArgumentParser parser(&argc, argv);
// Load some default values for command line arguments from /etc/Aria.args
// (Linux) or the ARIAARGS environment variable.
parser.loadDefaultArguments();
// Central object that is an interface to the robot and its integrated
// devices, and which manages control of the robot by the rest of the program.
ArRobot robot;
// Object that connects to the robot or simulator using program options
ArRobotConnector robotConnector(&parser, &robot);
// If the robot has an Analog Gyro, this object will activate it, and
// if the robot does not automatically use the gyro to correct heading,
// this object reads data from it and corrects the pose in ArRobot
ArAnalogGyro gyro(&robot);
// Connect to the robot, get some initial data from it such as type and name,
// and then load parameter files for this robot.
if (!robotConnector.connectRobot())
{
// Error connecting:
// if the user gave the -help argumentp, then just print out what happened,
// and continue so options can be displayed later.
if (!parser.checkHelpAndWarnUnparsed())
{
ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything");
}
// otherwise abort
else
{
ArLog::log(ArLog::Terse, "Error, could not connect to robot.");
Aria::logOptions();
Aria::exit(1);
}
}
// Connector for laser rangefinders
ArLaserConnector laserConnector(&parser, &robot, &robotConnector);
// Connector for compasses
ArCompassConnector compassConnector(&parser);
// Parse the command line options. Fail and print the help message if the parsing fails
// or if the help was requested with the -help option
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Used to access and process sonar range data
ArSonarDevice sonarDev;
// Used to perform actions when keyboard keys are pressed
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
// ArRobot contains an exit action for the Escape key. It also
// stores a pointer to the keyhandler so that other parts of the program can
// use the same keyhandler.
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Attach sonarDev to the robot so it gets data from it.
robot.addRangeDevice(&sonarDev);
// Start the robot task loop running in a new background thread. The 'true' argument means if it loses
// connection the task loop stops and the thread exits.
robot.runAsync(true);
// Connect to the laser(s) if lasers were configured in this robot's parameter
// file or on the command line, and run laser processing thread if applicable
// for that laser class. (For the purposes of this demo, add all
// possible lasers to ArRobot's list rather than just the ones that were
// specified with the connectLaser option (so when you enter laser mode, you
// can then interactively choose which laser to use from the list which will
// show both connected and unconnected lasers.)
if (!laserConnector.connectLasers(false, false, true))
{
printf("Could not connect to lasers... exiting\n");
Aria::exit(2);
}
// Create and connect to the compass if the robot has one.
ArTCM2 *compass = compassConnector.create(&robot);
if(compass && !compass->blockingConnect()) {
compass = NULL;
}
// Sleep for a second so some messages from the initial responses
// from robots and cameras and such can catch up
ArUtil::sleep(1000);
// We need to lock the robot since we'll be setting up these modes
// while the robot task loop thread is already running, and they
// need to access some shared data in ArRobot.
robot.lock();
// now add all the modes for this demo
// these classes are defined in ArModes.cpp in ARIA's source code.
ArModeLaser laser(&robot, "laser", 'l', 'L');
ArModeTeleop teleop(&robot, "teleop", 't', 'T');
ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U');
ArModeWander wander(&robot, "wander", 'w', 'W');
ArModeGripper gripper(&robot, "gripper", 'g', 'G');
ArModeCamera camera(&robot, "camera", 'c', 'C');
ArModeSonar sonar(&robot, "sonar", 's', 'S');
ArModeBumps bumps(&robot, "bumps", 'b', 'B');
ArModePosition position(&robot, "position", 'p', 'P', &gyro);
ArModeIO io(&robot, "io", 'i', 'I');
ArModeActs actsMode(&robot, "acts", 'a', 'A');
ArModeCommand command(&robot, "command", 'd', 'D');
ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass);
// activate the default mode
teleop.activate();
// turn on the motors
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
// Block execution of the main thread here and wait for the robot's task loop
// thread to exit (e.g. by robot disconnecting, escape key pressed, or OS
// signal)
robot.waitForRunExit();
Aria::exit(0);
}
int main(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 225, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 225, 1100, 400);
ArActionTableSensorLimiter tableLimiter;
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArPTZ *ptz;
ptz = new ArVCC4(&robot, true);
ArGripper gripper(&robot);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, ptz);
DropOff dropOff(&acts, &gripper, ptz);
PickUp pickUp(&acts, &gripper, ptz);
TakeBlockToWall takeBlock(&robot, &gripper, ptz, &acq, &driveTo, &pickUp,
&dropOff, &tableLimiter);
if (!acts.openPort(&robot))
{
printf("Could not connect to acts, exiting\n");
exit(0);
}
Aria::init();
robot.addRangeDevice(&sonar);
//con.setBaud(38400);
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
ptz->init();
ArUtil::sleep(8000);
printf("### 2222\n");
ptz->panTilt(0, -40);
printf("### whee\n");
ArUtil::sleep(8000);
robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&tableLimiter, 100);
robot.addAction(&limiter, 99);
robot.addAction(&limiterFar, 98);
robot.addAction(&backwardsLimiter, 97);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&dropOff, 74);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 0;
}
int main(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArActionConstantVelocity backup("backup", -200);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArSonyPTZ sony(&robot);
ArGripper gripper(&robot, ArGripper::GENIO);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, &sony);
PickUp pickUp(&acts, &gripper, &sony);
TakeBlockToWall takeBlock(&robot, &gripper, &sony, &acq, &driveTo, &pickUp,
&backup);
Aria::init();
if (!acts.openPort(&robot))
{
printf("Could not connect to acts\n");
exit(1);
}
robot.addRangeDevice(&sonar);
//con.setBaud(38400);
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
sony.init();
ArUtil::sleep(1000);
//robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&limiter, 100);
robot.addAction(&limiterFar, 99);
robot.addAction(&backwardsLimiter, 98);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&backup, 50);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArRobot robot;
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector connector(&argParser);
ArGripper gripper(&robot);
ArSonarDevice sonar;
robot.addRangeDevice(&sonar);
argParser.loadDefaultArguments();
if (!Aria::parseArgs() || !argParser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::shutdown();
return 1;
}
if (!connector.connectRobot(&robot))
{
ArLog::log(ArLog::Terse, "gripperExample: Could not connect to robot... exiting");
Aria::shutdown();
return 2;
}
ArLog::log(ArLog::Normal, "gripperExample: Connected to robot.");
ArLog::log(ArLog::Normal, "gripperExample: GripperType=%d", gripper.getType());
gripper.logState();
if(gripper.getType() == ArGripper::NOGRIPPER)
{
ArLog::log(ArLog::Terse, "gripperExample: Error: Robot does not have a gripper. Exiting.");
Aria::shutdown();
return -1;
}
// Teleoperation actions with obstacle-collision avoidance
ArActionLimiterTableSensor tableLimit;
robot.addAction(&tableLimit, 110);
ArActionLimiterForwards limitNearAction("near", 300, 600, 250);
robot.addAction(&limitNearAction, 100);
ArActionLimiterForwards limitFarAction("far", 300, 1100, 400);
robot.addAction(&limitFarAction, 90);
ArActionLimiterBackwards limitBackAction;
robot.addAction(&limitBackAction, 50);
ArActionJoydrive joydriveAction("joydrive", 400, 15);
robot.addAction(&joydriveAction, 40);
joydriveAction.setStopIfNoButtonPressed(false);
ArActionKeydrive keydriveAction;
robot.addAction(&keydriveAction, 30);
// Handlers to control the gripper and print out info (classes defined above)
PrintGripStatus printStatus(&gripper);
GripperControlHandler gripControl(&gripper);
printStatus.addRobotTask(&robot);
gripControl.addKeyHandlers(&robot);
// enable motors and run (if we lose connection to the robot, exit)
ArLog::log(ArLog::Normal, "You may now operate the robot with arrow keys or joystick. Operate the gripper with the u, d, o, c, and page up/page down keys.");
robot.enableMotors();
robot.run(true);
Aria::shutdown();
return 0;
}
