joyosephk / pr2lib Goto Github PK

This package contains two servers for interacting with the pr2. The first is the 'reciever' executable, which takes in low- level commands to the pr2 and drives the pr2 accordingly. The second server, 'high_level_reciever', can be run on top on the first. This takes in higher level motion commands, parses them, and passes them down to the low-level 'reciever' server.

##Folders: pr2lib -- this folder
src -- source code for nodes
srv -- source code for services
scripts -- python scripts
launch -- contains launch files
params -- parameter files
bin -- executables location, auto-gen
build --CMake auto-gen
srv_gen -- srv auto-gen

##SETUP: To build and use this package, you need to have ROS Fuerte installed on your pc or vm, which can be done by following the instructions in http://wiki.ros.org/fuerte/Installation/Ubuntu. Things would be easier if you have Ubuntu 12.04.5. You will also need to have PR2 installed within ROS. This can be done by following instructions: http://wiki.ros.org/Robots/PR2/fuerte.

Check out the package and add the following environment variables to your .bashrc:

export PR2LIB_PATH=[path_to_pr2lib]
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:[path_to_pr2lib]

For example, my [path_to_pr2lib] is /home/joe/nsf_nri/workspace/pr2lib

Then you can build the package with:

rosmake pr2lib

The package should build and you should be good to go. There are no strictly runtime dependencies. <br >

##RUNNING SIMULATOR:

For simulation, make sure the following variables are on your .bashrc

export ROS_MASTER_URI=http://localhost:11311 //this is where the ros core will be located
export ROBOT=sim //since we are running the simulator
export ROS_HOSTNAME=localhost

Then run the following:

roslaunch pr2_gazebo pr2_empty_world.launch // Spawn pr2 in simulation
rosrun rviz rviz // interface to see all the internal states

####Simple control commands (.py files may need to be chmod) roslaunch pr2_teleop teleop_keyboard.launch // keyboard teleop
rosrun pr2lib movebase.py // move the base of the robot with velocity vector and duration
rosrun pr2lib head.py // change static position of the head
rosrun pr2lib gripper.py // change static position of the head
rosrun pr2lib torso.py // change height of the robot

There are two ways to use this package, one to parse only low level commands, and one to parse high level commands as well. Run the following commands in sequence on separate terminals.

roslaunch pr2lib pr2scripts.launch //runs some needed pr2 scripts for IK
roslaunch pr2lib nodes.launch //use this option if you want to have high level parsing available. Once you do this, you should the left arm retract.

OR instead of those two, run:

roslaunch pr2lib low_level_nodes.launch //use this option if you only want low level parsing

export ROS_MASTER_URI=http://10.68.0.1:11311 // roscore is now located in the robot
export ROBOT_IP=10.68.0.1 // robot's ip
export ROS_IP=[remote_ip] // ip robot gave you
export ROBOT=pr2

-Turn on the robot (red switch)
-After four blip sequences, you are ready to ssh.
ssh [email protected] (get passwd from Joe)
-Run the following in robot terminal
robot claim
robot start
-Run in your terminal:
rosrun pr2_dashboard pr2_dashboard // right click the gear button in 'Diagnostic' and click 'Reset'. The robot may start moving around to calibrate itself. Make sure nothing is in collision with the robot.

Run the following (IK) in the SSH (robot's) terminal: roslaunch pr2lib pr2scripts.launch & // added & so that you could type 'Enter' and put more commands
rosrun pr2lib ik_l &
rosrun pr2lib ik_r &

Run the following on the remote terminal rosrun pr2lib reciever // low-level receiver
rosrun pr2lib high_level_reciever // at this point, you should see the pr2's left arm move to a default position.

Now the robot is ready to receive sequences from the authoring environment:
http://nsf-nri.herokuapp.com/

Make sure you also have Ethernet connection through the statacenter.

Note that these tests currently are basic demonstrations that do not adequately cover all test cases. These programs are designed to be easliy changable, giving the user freedom to edit these files to test specific cases.

rosrun pr2lib test_input //test low level controller
rosrun pr2lib high_level_test_input //test high level controller

l_gripper_motor_accelerometer_link

Port 8888. Each message is a 'header + (gripper | arm | twist | end_msg | rotate | driveForward)'. Note that the message type must align with the type of message being sent. Note that some of these structs are defined in the high level ICD below.

At the moment, twist messages are not implemented due to difficulty using the arm with the navigation stack. Instead, use the naviation free rotate and driveForward messages.

header:
-short check
-int seq_number
-int timestamp
-bool isBocking
-int messageType (1 = gripper, 2 = twist, 3 = arm, 4 = end_msg, 5 = driveForward, 6 = rotate)

gripper:
-int seq_number
-bool arm (0=right)
-float position (0 closed, 0.065 fully open)
-float max_effort (0->100)

arm:
-int seq_number
-bool arm
-float posX;
-float posY;
-float posZ;
-float QX;
-float QY;
-float QZ;
-float QW;

twist:
-int seq_number;
-float linearX;
-float linearY;
-float linearZ;
-float angularX;
-float angularY;
-float angularZ;

##HIGH LEVEL ICD: Port 9999. This server expects a sequence of command messages followed by an end command. After the end command is recieved and the simulation finishes, it will send a reply elapsedTime message indicating the time between the first command starting and the last command finishing. Note that the simulator will start executing commands as soon as the first command is passed. Each command message is of the form 'command_header + (transport_empty | transport_loaded | grasp | position | release_load | movebase | rotate | driveForward)'. Those definitions can be found in the structs below, and have little endianess (most common).

The sequence number of a header and the following message must align. However, it is not checked that sequential messages have sequential sequence numbers.

Orientation and Angle ints are enums for different hand positions. Orientation has value either 0 or 1. This corresponds to the gripper being flat to the ground, or vertical (rotated 90 deg either way). Angle has values 0,1,2. 0 means gripper pointed at the ground, 1 means pointer flat, 2 means pointed up.

The movebase command is not currently implemented, For driving the robot, use the rotate and driveForward messages. These are blocking and do not use navigation stack, hence no issues while using arms.

Error reporting: If an action is invalid for some reason, bad request or invalid IK, the server will send a elasped time respose with a negative value indicating an error. An enum for these values can be found in src/enums.cpp. The server will then terminate its connection to this client, and wait to accept a new client.

If the client crashes or disconnects, the server will kill that connection and wait for a new one. It will not reset the robot position.

command_header:
-short check;
-int seq_number;
-int timestamp;
-int messageType; (0 = transport_empty, 1 = transport_loaded, 2 = grasp, 3 = position, 4 = release_load, 5 = movebase, 6 = retract, 7 = use_breaker, 8 = end_msg, 9 = rotate, 10 = driveForward)

transport_empty:
-int seq_number;
-int object;
-int orientation;
-int angle;
-int arm;
-float max_joint_vel;
s

transport_loaded:
-int seq_number;
-int position;
-int arm;
-float max_joint_vel;

grasp:
-int seq_number;
-int effort;
-int arm;

position:
-int seq_number;
-int orientation;
-int angle;
-int arm;
-float max_joint_vel;

release_load:
-int seq_number;
-int arm;

retract:
-int seq_number;
-int arm;

use_breaker:
-int arm;
-int size;

movebase:
-int seq_number;
-double relativeX;
-double relativeY;
-double relativeZ;

elapsedTime:
-double time;

rotate:
-int seq_number;
-bool clockwise;
-double radians;

driveForward:
-int seq_number;
-double distance;

##Navigation Stack: This section is not complete and not currently used in the package.

Installation: -go to https://github.com/PR2/pr2_navigation_apps and download and extract the zip
-add the extracted folder to the ROS package path
rosmake pr2_2dnav_local

##Random Stuff:

rostopic pub r_gripper_controller/command pr2_controllers_msgs/Pr2GripperCommand "{position: 0.06, max_effort: 100.0}" http://www.pythonprasanna.com/Papers%20and%20Articles/Sockets/udpserver.c http://www.pythonprasanna.com/Papers%20and%20Articles/Sockets/udpclient.c

rosservice call /execute_cartesian_ik_trajectory -- "{header: { frame_id: /base_link}, poses: [{position: [0.76, -0.19, 0.83], orientation:[0.02, 0.09, 0.0, 1.0]}]}"

Navigation stack
rosmake pr2_navigation_perception pr2_navigation_teleop pr2_navigation_global fake_localization amcl map_server

Good tutorial on sample movements
https://pr2s.clearpathrobotics.com/wiki/Tutorials/MovingMechanism