Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22493] pkg/trunk
|
From: <stu...@us...> - 2009-08-21 00:07:48
|
Revision: 22493
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22493&view=rev
Author: stuglaser
Date: 2009-08-21 00:07:36 +0000 (Fri, 21 Aug 2009)
Log Message:
-----------
Moving messages and services out of pr2_mechanism_controllers
Modified Paths:
--------------
pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/arm_commander.py
pkg/trunk/calibration/kinematic_calibration/scripts/arm_commander_node.py
pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py
pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py
pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/arm_commander.py
pkg/trunk/calibration/optical_flag_calibration/scripts/flag_tester.py
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/demos/plug_in/move_to_pickup.py
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm_setup.h
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_setup.cpp
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_untuck_arms.h
pkg/trunk/highlevel/plugs/plugs_core/src/action_untuck_arms.cpp
pkg/trunk/highlevel/safety/safety_core/include/safety_core/action_tuck_arms.h
pkg/trunk/highlevel/safety/safety_core/src/action_tuck_arms.cpp
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/sandbox/joint_waypoint_controller/include/joint_waypoint_controller/joint_waypoint_controller.h
pkg/trunk/sandbox/joint_waypoint_controller/src/joint_waypoint_controller.cpp
Added Paths:
-----------
pkg/trunk/sandbox/experimental_controllers/msg/ControllerState.msg
pkg/trunk/sandbox/experimental_controllers/msg/JointPosCmd.msg
pkg/trunk/sandbox/experimental_controllers/srv/GetCartesianPosCmd.srv
pkg/trunk/sandbox/experimental_controllers/srv/GetJointPosCmd.srv
pkg/trunk/sandbox/experimental_controllers/srv/TrajectoryCancel.srv
pkg/trunk/sandbox/experimental_controllers/srv/TrajectoryQuery.srv
pkg/trunk/sandbox/experimental_controllers/srv/TrajectoryStart.srv
pkg/trunk/sandbox/experimental_controllers/srv/WheelRadiusMultiplier.srv
pkg/trunk/sandbox/experimental_controllers/test/test_arm_trajectory_service.cpp
pkg/trunk/sandbox/experimental_controllers/test/test_joint_trajectory_controller_srv.cpp
Removed Paths:
-------------
pkg/trunk/controllers/pr2_mechanism_controllers/msg/ControllerState.msg
pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointPosCmd.msg
pkg/trunk/controllers/pr2_mechanism_controllers/msg/OdometryResiduals.msg
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetBaseCommand.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetCartesianPosCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetCartesianVelCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointGains.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointPosCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointVelCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetBaseCommand.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetCartesianPosCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetCartesianVelCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointGains.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointPathCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointPosCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointTarget.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointVelCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryCancel.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryWait.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/WheelRadiusMultiplier.srv
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_joint_trajectory_controller_srv.cpp
Modified: pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/arm_commander.py
===================================================================
--- pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/arm_commander.py 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/arm_commander.py 2009-08-21 00:07:36 UTC (rev 22493)
@@ -38,8 +38,8 @@
import rospy
import sys
#from std_msgs.msg import Empty
-from pr2_mechanism_controllers.srv import *
-#from pr2_mechanism_controllers.msg import
+from experimental_controllers.srv import *
+#from experimental_controllers.msg import
from manipulation_msgs.msg import *
class ArmCommander() :
@@ -53,7 +53,7 @@
self.init_joint_headers(joint_headers_string)
self.init_joint_data(joint_data_string)
-
+
rospy.wait_for_service(arm_controller_name + '/TrajectoryQuery')
# Build service proxies for arm controller
@@ -67,7 +67,7 @@
def init_joint_headers(self, joint_headers) :
self.joint_headers = joint_headers.split()
-
+
def init_joint_data(self, joint_data) :
joint_data_all = [[float(x) for x in cur_line.split()] for cur_line in joint_data.split("\n")]
self.joint_data = [x for x in joint_data_all if len(x)==len(self.joint_headers)]
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/arm_commander_node.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/arm_commander_node.py 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/arm_commander_node.py 2009-08-21 00:07:36 UTC (rev 22493)
@@ -5,8 +5,8 @@
import rospy
import sys
from std_msgs.msg import Empty
-from pr2_mechanism_controllers.srv import *
-from pr2_mechanism_controllers.msg import *
+from experimental_controllers.srv import *
+from experimental_controllers.msg import *
from mechanism_msgs import JointState, JointStates
cmd_count = 0
@@ -16,7 +16,7 @@
start_topic = callback_args[1]
head_cmd = callback_args[2]
head_pub = callback_args[3]
-
+
global cmd_count
print "Callback Called"
print " Waiting for Service:\n (%s)..." % start_topic
@@ -34,7 +34,7 @@
print " Timestamps:"
print start_resp.timestamps
- print " *** Head Command ***"
+ print " *** Head Command ***"
print head_cmd[cmd_count]
ps = JointState()
ps.name = 'head_pan_joint'
@@ -54,8 +54,8 @@
print "************"
cmd_count = cmd_count + 1
-
+
if __name__ == '__main__':
print "Running python code"
rospy.init_node('arm_commander', sys.argv, anonymous=False)
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py 2009-08-21 00:07:36 UTC (rev 22493)
@@ -6,8 +6,8 @@
import rospy
import sys
from std_msgs.msg import Empty
-from pr2_mechanism_controllers.srv import *
-from pr2_mechanism_controllers.msg import *
+from experimental_controllers.srv import *
+from experimental_controllers.msg import *
from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
from manipulation_msgs.msg import *
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py 2009-08-21 00:07:36 UTC (rev 22493)
@@ -6,8 +6,8 @@
import rospy
import sys
from std_msgs.msg import Empty
-from pr2_mechanism_controllers.srv import *
-from pr2_mechanism_controllers.msg import *
+from experimental_controllers.srv import *
+from experimental_controllers.msg import *
from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
from manipulation_msgs.msg import *
Modified: pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/arm_commander.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/arm_commander.py 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/arm_commander.py 2009-08-21 00:07:36 UTC (rev 22493)
@@ -38,8 +38,8 @@
import rospy
import sys
#from std_msgs.msg import Empty
-from pr2_mechanism_controllers.srv import *
-#from pr2_mechanism_controllers.msg import
+from experimental_controllers.srv import *
+#from experimental_controllers.msg import
from manipulation_msgs.msg import *
@@ -54,7 +54,7 @@
self.init_joint_headers(joint_headers_string)
self.init_joint_data(joint_data_string)
-
+
rospy.wait_for_service(arm_controller_name + '/TrajectoryQuery')
# Build service proxies for arm controller
@@ -68,7 +68,7 @@
def init_joint_headers(self, joint_headers) :
self.joint_headers = joint_headers.split()
-
+
def init_joint_data(self, joint_data) :
joint_data_all = [[float(x) for x in cur_line.split()] for cur_line in joint_data.split("\n")]
self.joint_data = [x for x in joint_data_all if len(x)==len(self.joint_headers)]
Modified: pkg/trunk/calibration/optical_flag_calibration/scripts/flag_tester.py
===================================================================
--- pkg/trunk/calibration/optical_flag_calibration/scripts/flag_tester.py 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/calibration/optical_flag_calibration/scripts/flag_tester.py 2009-08-21 00:07:36 UTC (rev 22493)
@@ -6,8 +6,8 @@
import rospy
import sys
from std_msgs.msg import Empty
-from pr2_mechanism_controllers.srv import *
-from pr2_mechanism_controllers.msg import *
+from experimental_controllers.srv import *
+from experimental_controllers.msg import *
from manipulation_msgs.msg import *
cmd_count = 0
@@ -71,7 +71,7 @@
print " Response:"
print " Traj ID: %u" % start_resp.trajectoryid
-
+
prev_id = start_resp.trajectoryid
while (not rospy.is_shutdown()) :
@@ -93,10 +93,10 @@
print "*** Arm Command 0 ***"
joint_traj = JointTraj([JointTrajPoint(arm_cmd[0],0)])
start_resp = start_srv.call(TrajectoryStartRequest(joint_traj,0,1))
-
+
print " Response:"
print " Traj ID: %u" % start_resp.trajectoryid
-
+
prev_id = start_resp.trajectoryid
#rospy.spin()
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-08-21 00:07:36 UTC (rev 22493)
@@ -38,7 +38,7 @@
#include <ros/node.h>
#include <boost/thread/mutex.hpp>
#include <tf/tf.h>
-#include <pr2_mechanism_controllers/WheelRadiusMultiplier.h>
+#include <experimental_controllers/WheelRadiusMultiplier.h>
// messages
@@ -87,7 +87,7 @@
geometry_msgs::Twist _vel;
// service messages
- pr2_mechanism_controllers::WheelRadiusMultiplier::Request _srv_snd, _srv_rsp;
+ experimental_controllers::WheelRadiusMultiplier::Request _srv_snd, _srv_rsp;
// active sensors
bool _odom_active, _imu_active, _mech_active, _completed;
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/msg/ControllerState.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/ControllerState.msg 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/ControllerState.msg 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,3 +0,0 @@
-string name
-float64 update_time
-float64 max_update_time
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointPosCmd.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointPosCmd.msg 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointPosCmd.msg 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,4 +0,0 @@
-string[] names
-float64[] positions
-float64[] margins
-float64 timeout
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/msg/OdometryResiduals.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/OdometryResiduals.msg 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/OdometryResiduals.msg 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,3 +0,0 @@
-float64[] residuals
-string[] names
-float64 time
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetBaseCommand.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetBaseCommand.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetBaseCommand.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,4 +0,0 @@
----
-float64 vx
-float64 vy
-float64 vw
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetCartesianPosCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetCartesianPosCmd.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetCartesianPosCmd.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,7 +0,0 @@
----
-float64 x
-float64 y
-float64 z
-float64 roll
-float64 pitch
-float64 yaw
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetCartesianVelCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetCartesianVelCmd.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetCartesianVelCmd.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,7 +0,0 @@
----
-float64 vx
-float64 vy
-float64 vz
-float64 wx
-float64 wy
-float64 wz
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointGains.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointGains.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointGains.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,8 +0,0 @@
-string name
----
-string name
-float64 p
-float64 i
-float64 d
-float64 i_min
-float64 i_max
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointPosCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointPosCmd.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointPosCmd.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,2 +0,0 @@
----
-JointPosCmd command
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointVelCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointVelCmd.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointVelCmd.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,3 +0,0 @@
-float64[] velocity
----
-float64[] velocity
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,3 +0,0 @@
-geometry_msgs/PoseStamped transform
----
-manipulation_msgs/JointTraj traj
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetBaseCommand.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetBaseCommand.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetBaseCommand.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,7 +0,0 @@
-float64 vx
-float64 vy
-float64 vw
----
-float64 vx
-float64 vy
-float64 vw
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetCartesianPosCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetCartesianPosCmd.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetCartesianPosCmd.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,7 +0,0 @@
-float64 x
-float64 y
-float64 z
-float64 roll
-float64 pitch
-float64 yaw
----
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetCartesianVelCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetCartesianVelCmd.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetCartesianVelCmd.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,7 +0,0 @@
-float64 vx
-float64 vy
-float64 vz
-float64 wx
-float64 wy
-float64 wz
----
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointGains.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointGains.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointGains.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,7 +0,0 @@
-string name
-float64 p
-float64 i
-float64 d
-float64 i_min
-float64 i_max
----
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointPathCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointPathCmd.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointPathCmd.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,3 +0,0 @@
-JointPosCmd[] path
----
-float64[] end_positions
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointPosCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointPosCmd.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointPosCmd.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,3 +0,0 @@
-float64[] positions
----
-float64[] positions
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointTarget.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointTarget.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointTarget.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,3 +0,0 @@
-JointPosCmd[] positions
----
-float64[] end_positions
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointVelCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointVelCmd.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointVelCmd.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,3 +0,0 @@
-float64[] velocity
----
-float64[] velocity
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryCancel.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryCancel.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryCancel.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,6 +0,0 @@
-## cancels a trajectory(ies)
-
-# if 0, cancels all trajectories, otherwise cancels the trajectory with the specified id
-# if no such trajectory exists return fail
-uint32 trajectoryid
----
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,20 +0,0 @@
-## queries where the current trajectory is along its path
-
-# if trajectoryid is 0, returns the timestamp of the current trajectory
-# and done is filled whenever all the trajectories have been finished
-int32 trajectoryid
-int32 Query_Joint_Names=-1
----
-uint8 State_Active=0
-uint8 State_Done=1
-uint8 State_Queued=2
-uint8 State_Deleted=3
-uint8 State_Failed=4
-uint8 State_Canceled=5
-
-uint8 done # 1 if trajectory is done, 0 if ongoing, 2 if queued, 3 if deleted, 4 if failed, 5 if canceled
-
-float32 trajectorytime # the current timestamp the trajectory is at. If the trajectory is finished, specifies the total time of the trajectory
-
-string[] jointnames # names of the joints that the controller expects
-float64[] jointpositions # joint values
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,7 +0,0 @@
-## Starts a trajectory
-manipulation_msgs/JointTraj traj
-uint8 hastiming # if 1, use timestamps of trajectory points, otherwise do internal retiming
-uint8 requesttiming # if 1, send back the timestamps for every trajectory point (first point starts at 0)
----
-uint32 trajectoryid # unique trajectory id to be used for later referencing
-float32[] timestamps # trajectory timestamps if requested
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryWait.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryWait.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryWait.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,8 +0,0 @@
-## waits for a trajectory to finish or times out
-
-# if 0, waits for all trajectories, otherwise waits for the trajectory with the specified id
-# if no such trajectory exists return fail, if such a trajectory exists and is done already return immediately
-uint32 trajectoryid
-float32 timeout # if 0, waits indefinitely
----
-uint8 timedout # 1 if timed out, 0 if any other error occurs
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/WheelRadiusMultiplier.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/WheelRadiusMultiplier.srv 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/WheelRadiusMultiplier.srv 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,3 +0,0 @@
-float64 radius_multiplier
----
-float64 radius_multiplier
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,127 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-#include <ros/node.h>
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
-#include <pr2_mechanism_controllers/TrajectoryQuery.h>
-
-static int done = 0;
-
-void finalize(int donecare)
-{
- done = 1;
-}
-
-int main( int argc, char** argv )
-{
-
- /*********** Initialize ROS ****************/
- ros::init(argc,argv);
- ros::Node *node = new ros::Node("arm_trajectory_controller_client");
-
- signal(SIGINT, finalize);
- signal(SIGQUIT, finalize);
- signal(SIGTERM, finalize);
-
- pr2_mechanism_controllers::TrajectoryStart::Request req;
- pr2_mechanism_controllers::TrajectoryStart::Response res;
-
- pr2_mechanism_controllers::TrajectoryQuery::Request req_q;
- pr2_mechanism_controllers::TrajectoryQuery::Response res_q;
-
- int num_points = 3;
- int num_joints = 7;
-
- req.traj.set_points_size(num_points);
- req.requesttiming = 1;
-
- for(int i=0; i<num_points; i++)
- req.traj.points[i].set_positions_size(num_joints);
-
-
- req.traj.points[0].positions[0] = 0.5;
- req.traj.points[0].positions[1] = 0.5;
- req.traj.points[0].positions[2] = 0.2;
- req.traj.points[0].positions[3] = -0.5;
- req.traj.points[0].positions[4] = 0.4;
- req.traj.points[0].positions[5] = 0.0;
- req.traj.points[0].positions[6] = 0.0;
- req.traj.points[0].time = 0.0;
-
- req.traj.points[1].positions[0] = 0.0;
- req.traj.points[1].positions[1] = 0.0;
- req.traj.points[1].positions[2] = 0.0;
- req.traj.points[1].positions[3] = 0.0;
- req.traj.points[1].positions[4] = 0.0;
- req.traj.points[1].positions[5] = 0.0;
- req.traj.points[1].positions[6] = 0.0;
- req.traj.points[1].time = 0.0;
-
- req.traj.points[2].positions[0] = -0.5;
- req.traj.points[2].positions[1] = 0.3;
- req.traj.points[2].positions[2] = 0.2;
- req.traj.points[2].positions[3] = -1.0;
- req.traj.points[2].positions[4] = -0.4;
- req.traj.points[2].positions[5] = 0.0;
- req.traj.points[2].positions[6] = 0.0;
- req.traj.points[2].time = 0.0;
-
-
- if (ros::service::call("right_arm_trajectory_controller/TrajectoryStart", req, res))
- {
- ROS_INFO("Done");
- }
- sleep(10);
- req_q.trajectoryid = atoi(argv[1]);
-
- if(ros::service::call("right_arm_trajectory_controller/TrajectoryQuery", req_q, res_q))
- {
- ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
- }
- else
- {
- ROS_INFO("service call failed");
- }
- sleep(4);
- if(ros::service::call("right_arm_trajectory_controller/TrajectoryQuery", req_q, res_q))
- {
- ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
- }
- else
- {
- ROS_INFO("service call failed");
- }
-
-
-}
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,122 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-#include <ros/node.h>
-#include <pr2_mechanism_controllers/GraspPointSrv.h>
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
-
-static int done = 0;
-
-void finalize(int donecare)
-{
- done = 1;
-}
-
-int main( int argc, char** argv )
-{
-
- /*********** Initialize ROS ****************/
- ros::init(argc,argv);
- ros::Node *node = new ros::Node("grasp_point_client");
-
- signal(SIGINT, finalize);
- signal(SIGQUIT, finalize);
- signal(SIGTERM, finalize);
-
- pr2_mechanism_controllers::GraspPointSrv::Request req;
- pr2_mechanism_controllers::GraspPointSrv::Response res;
-
- req.transform.header.stamp = ros::Time::now();
- req.transform.header.frame_id = "r_shoulder_pan_link";
- req.transform.pose.position.x = 0.5;
- req.transform.pose.position.y = 0.0;
- req.transform.pose.position.z = +0.2;
-
- req.transform.pose.orientation.x = 0.0;
- req.transform.pose.orientation.y = 0.0;
- req.transform.pose.orientation.z = 0.0;
- req.transform.pose.orientation.w = 1.0;
-
- if (ros::service::call("/grasp_point_node/SetGraspPoint", req, res))
- {
- ROS_INFO("Done");
-
- for(int i=0; i<2;i++)
- {
- for(int j=0; j< (int) res.traj.points[0].get_positions_size(); j++)
- {
- std::cout << res.traj.points[i].positions[j] << " ";
- }
- std::cout << std::endl;
- }
- }
- else
- {
- ROS_INFO("IK failed");
- return 0;
- }
-
- sleep(1);
-
- pr2_mechanism_controllers::TrajectoryStart::Request reqt;
- pr2_mechanism_controllers::TrajectoryStart::Response rest;
-
- int num_points = (int) res.traj.get_points_size();
- int num_joints = (int) res.traj.points[0].get_positions_size();
-
- reqt.traj.set_points_size(num_points);
- reqt.requesttiming = 1;
-
- for(int i=0; i<num_points; i++)
- {
- reqt.traj.points[i].set_positions_size(num_joints);
- for(int j=0; j<num_joints; j++)
- {
- reqt.traj.points[i].positions[j] = res.traj.points[i].positions[j];
- }
- }
- reqt.traj.points[1].time = 4.0;
-
- if (ros::service::call("/right_arm_trajectory_controller/TrajectoryStart", reqt, rest))
- {
- ROS_INFO("Done");
- }
- else
- {
- ROS_INFO("Trajectory failed");
- }
-// sleep(20);
-
- return 0;
-}
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_joint_trajectory_controller_srv.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_joint_trajectory_controller_srv.cpp 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_joint_trajectory_controller_srv.cpp 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,188 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-#include <ros/node.h>
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
-#include <pr2_mechanism_controllers/TrajectoryQuery.h>
-#include <pr2_mechanism_controllers/TrajectoryCancel.h>
-
-static int done = 0;
-
-void finalize(int donecare)
-{
- done = 1;
-}
-
-int main( int argc, char** argv )
-{
-
- /*********** Initialize ROS ****************/
- ros::init(argc,argv);
- ros::Node *node = new ros::Node("arm_trajectory_controller_client");
-
- signal(SIGINT, finalize);
- signal(SIGQUIT, finalize);
- signal(SIGTERM, finalize);
-
- pr2_mechanism_controllers::TrajectoryStart::Request req;
- pr2_mechanism_controllers::TrajectoryStart::Response res;
-
- pr2_mechanism_controllers::TrajectoryQuery::Request req_q;
- pr2_mechanism_controllers::TrajectoryQuery::Response res_q;
-
- pr2_mechanism_controllers::TrajectoryCancel::Request rec_req;
- pr2_mechanism_controllers::TrajectoryCancel::Response rec_res;
-
- int num_points = 3;
- int num_joints = 7;
-
- req.traj.set_points_size(num_points);
- req.requesttiming = 1;
-
- for(int i=0; i<num_points; i++)
- req.traj.points[i].set_positions_size(num_joints);
-
-
- req.traj.points[0].positions[0] = 0.5;
- req.traj.points[0].positions[1] = 0.5;
- req.traj.points[0].positions[2] = 0.2;
- req.traj.points[0].positions[3] = -0.5;
- req.traj.points[0].positions[4] = 0.4;
- req.traj.points[0].positions[5] = 0.0;
- req.traj.points[0].positions[6] = 0.0;
- req.traj.points[0].time = 0.0;
-
- req.traj.points[1].positions[0] = 0.0;
- req.traj.points[1].positions[1] = 0.0;
- req.traj.points[1].positions[2] = 0.0;
- req.traj.points[1].positions[3] = 0.0;
- req.traj.points[1].positions[4] = 0.0;
- req.traj.points[1].positions[5] = 0.0;
- req.traj.points[1].positions[6] = 0.0;
- req.traj.points[1].time = 0.0;
-
- req.traj.points[2].positions[0] = -0.5;
- req.traj.points[2].positions[1] = 0.3;
- req.traj.points[2].positions[2] = 0.2;
- req.traj.points[2].positions[3] = -1.0;
- req.traj.points[2].positions[4] = -0.4;
- req.traj.points[2].positions[5] = 0.0;
- req.traj.points[2].positions[6] = 0.0;
- req.traj.points[2].time = 0.0;
-
-
- int num_tries = 0;
-
- if (ros::service::call("r_arm_joint_trajectory_controller/TrajectoryStart", req, res))
- {
- ROS_INFO("Done");
- }
-// sleep(0.1);
-
- req_q.trajectoryid = res.trajectoryid;
- rec_req.trajectoryid = res.trajectoryid;
-
- if(ros::service::call("r_arm_joint_trajectory_controller/TrajectoryCancel", rec_req, rec_res))
- {
- }
- else
- {
- ROS_INFO("service call failed");
- }
-
-
- usleep(1e4);
- if(ros::service::call("r_arm_joint_trajectory_controller/TrajectoryQuery", req_q, res_q))
- {
- while(res_q.done != res_q.State_Deleted && res_q.done != res_q.State_Canceled)
- {
- ros::service::call("r_arm_joint_trajectory_controller/TrajectoryQuery", req_q, res_q);
- usleep(1e5);
- }
-
- ROS_INFO("response 1:: id:%d, time:%f, response:%d",req_q.trajectoryid,res_q.trajectorytime,res_q.done);
- }
- else
- {
- ROS_INFO("service call failed");
- }
-
- usleep(1e4);
- req_q.trajectoryid = pr2_mechanism_controllers::TrajectoryQuery::Request::Query_Joint_Names;
-
- if(ros::service::call("r_arm_joint_trajectory_controller/TrajectoryQuery", req_q, res_q))
- {
- ROS_INFO("response 2:: %f, %d",res_q.trajectorytime,res_q.done);
- }
- else
- {
- ROS_INFO("service call failed");
- for(int i=0; i < (int) res_q.jointnames.size(); i++)
- {
- ROS_INFO("Joint name: %s", res_q.jointnames[i].c_str());
- }
- }
-
- if (ros::service::call("r_arm_joint_trajectory_controller/TrajectoryStart", req, res))
- {
- ROS_INFO("Done");
- }
-
- req_q.trajectoryid = res.trajectoryid;
- rec_req.trajectoryid = res.trajectoryid;
-
- sleep(4.0);
-
- if(ros::service::call("r_arm_joint_trajectory_controller/TrajectoryCancel", rec_req, rec_res))
- {
- }
- else
- {
- ROS_INFO("service call failed");
- }
-
- if(ros::service::call("r_arm_joint_trajectory_controller/TrajectoryQuery", req_q, res_q))
- {
- while(res_q.done != res_q.State_Deleted && res_q.done != res_q.State_Canceled)
- {
- ros::service::call("r_arm_joint_trajectory_controller/TrajectoryQuery", req_q, res_q);
- usleep(1e5);
- }
- ROS_INFO("response 3:: id:%d, time:%f, response:%d",req_q.trajectoryid,res_q.trajectorytime,res_q.done);
- }
- else
- {
- ROS_INFO("service call failed");
- }
-}
Modified: pkg/trunk/demos/plug_in/move_to_pickup.py
===================================================================
--- pkg/trunk/demos/plug_in/move_to_pickup.py 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/demos/plug_in/move_to_pickup.py 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,7 +1,7 @@
#!/usr/bin/env python
import roslib
-roslib.load_manifest('pr2_mechanism_controllers')
+roslib.load_manifest('experimental_controllers')
roslib.load_manifest('mechanism_control')
roslib.load_manifest('mechanism_bringup')
import numpy
@@ -12,7 +12,7 @@
from mechanism_msgs.msg import JointTraj, JointTrajPoint
from mechanism_control import mechanism
from robot_mechanism_controllers.srv import *
-from pr2_mechanism_controllers.srv import *
+from experimental_controllers.srv import *
from std_msgs.msg import *
from robot_srvs.srv import *
from deprecated_srvs.srv import *
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-08-21 00:07:36 UTC (rev 22493)
@@ -1,13 +1,13 @@
/*********************************************************************
* Software License Agreement (BSD License)
- *
+ *
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
- *
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
@@ -17,7 +17,7 @@
* * Neither the name of Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
@@ -39,7 +39,6 @@
#include <ros/ros.h>
#include <mapping_msgs/ObjectInMap.h>
#include <mapping_msgs/AttachedObject.h>
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
#include <actionlib/client/simple_action_client.h>
#include <move_arm/MoveArmAction.h>
@@ -54,19 +53,19 @@
{
public:
-
+
CleanTable(void) : move_arm(nh, "move_right_arm"), gripper(nh, "actuate_gripper_right_arm")
- {
+ {
pubObj = nh.advertise<mapping_msgs::ObjectInMap>("object_in_map", 1);
pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_object", 1);
-
- vmPub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
+
+ vmPub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
}
-
+
void sendPoint(const roslib::Header &header, double x, double y, double z)
{
visualization_msgs::Marker mk;
-
+
mk.header = header;
mk.ns = "test_recognition";
mk.id = 1;
@@ -76,41 +75,41 @@
mk.pose.position.y = y;
mk.pose.position.z = z;
mk.pose.orientation.w = 1.0;
-
+
mk.scale.x = mk.scale.y = mk.scale.z = 0.03;
-
+
mk.color.a = 1.0;
mk.color.r = 0.5;
mk.color.g = 0.4;
mk.color.b = 0.04;
-
+
vmPub.publish(mk);
}
-
+
bool findObject(tabletop_msgs::TableTopObject &obj)
{
tabletop_objects::FindObjectPoses::Request req;
tabletop_objects::FindObjectPoses::Response res;
-
+
ros::ServiceClient client = nh.serviceClient<tabletop_objects::FindObjectPoses>("table_top/find_object_poses");
if (client.call(req, res))
{
ROS_INFO("Found %d objects", (int)res.objects.size());
if (res.objects.empty())
return false;
-
+
obj = res.objects[0];
for (unsigned int i = 1 ; i < res.objects.size() ; ++i)
{
if (res.objects[i].grasp_pose.pose.position.y < obj.grasp_pose.pose.position.y)
obj = res.objects[i];
}
-
+
// hack for stereo error
double dx = 0.055;
double dy = 0.02;
double dz = -0.0;
-
+
obj.object_pose.pose.position.x += dx;
obj.object_pose.pose.position.y += dy;
obj.object_pose.pose.position.z += dz;
@@ -118,8 +117,8 @@
obj.object_pose.pose.orientation.y = 0;
obj.object_pose.pose.orientation.z = 0;
obj.object_pose.pose.orientation.w = 1;
-
-
+
+
obj.grasp_pose.pose.position.x += dx;
obj.grasp_pose.pose.position.y += dy;
obj.grasp_pose.pose.position.z += dz;
@@ -127,9 +126,9 @@
obj.grasp_pose.pose.orientation.y = 0;
obj.grasp_pose.pose.orientation.z = 0;
obj.grasp_pose.pose.orientation.w = 1;
-
+
obj.grasp_pose.pose.position.x -= 0.16;
-
+
ROS_INFO("pose of object: %f %f %f", obj.object_pose.pose.position.x, obj.object_pose.pose.position.y, obj.object_pose.pose.position.z);
ROS_INFO("point to grasp: %f %f %f", obj.grasp_pose.pose.position.x, obj.grasp_pose.pose.position.y, obj.grasp_pose.pose.position.z);
@@ -140,52 +139,52 @@
o.object = obj.object;
o.pose = obj.object_pose.pose;
pubObj.publish(o);
-
- return true;
+
+ return true;
}
else
{
- ROS_ERROR("Unable to call service for finding object");
+ ROS_ERROR("Unable to call service for finding object");
return false;
}
}
-
+
bool moveTo(tabletop_msgs::TableTopObject &obj)
{
move_arm::MoveArmGoal goal;
-
+
goal.goal_constraints.set_pose_constraint_size(1);
goal.goal_constraints.pose_constraint[0].pose = obj.grasp_pose;
goal.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
-
+
goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.005;
-
+
goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.02;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.02;
-
+
goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.03;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.03;
-
+
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.1;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.1;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.1;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.1;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.1;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.1;
-
+
goal.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
goal.goal_constraints.pose_constraint[0].type =
- motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
+ motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
motion_planning_msgs::PoseConstraint::ORIENTATION_R + motion_planning_msgs::PoseConstraint::ORIENTATION_P + motion_planning_msgs::PoseConstraint::ORIENTATION_Y;
-
+
sendPoint(obj.grasp_pose.header, obj.grasp_pose.pose.position.x, obj.grasp_pose.pose.position.y, obj.grasp_pose.pose.position.z);
-
-
+
+
bool finished_within_time;
move_arm.sendGoal(goal);
finished_within_time = move_arm.waitForGoalToFinish(ros::Duration(60.0));
-
+
if (!finished_within_time)
{
move_arm.cancelGoal();
@@ -200,10 +199,10 @@
}
bool gripClose(void)
- {
+ {
move_arm::ActuateGripperGoal g;
g.data = -80;
-
+
gripper.sendGoal(g);
bool finished_before_timeout = gripper.waitForGoalToFinish(ros::Duration(10));
if (finished_before_timeout)
@@ -218,12 +217,12 @@
return false;
}
}
-
+
bool gripOpen(void)
{
move_arm::ActuateGripperGoal g;
g.data = 80;
-
+
gripper.sendGoal(g);
bool finished_before_timeout = gripper.waitForGoalToFinish(ros::Duration(10));
if (finished_before_timeout)
@@ -249,7 +248,7 @@
o.id = "ID";
o.action = mapping_msgs::ObjectInMap::REMOVE;
pubObj.publish(o);
-
+
mapping_msgs::AttachedObject ao;
ao.header.frame_id = obj.object_pose.header.frame_id;
ao.header.stamp = ros::Time::now();
@@ -257,12 +256,12 @@
ao.objects.push_back(obj.object);
ao.poses.push_back(obj.object_pose.pose);
pubAttach.publish(ao);
-
+
return true;
}
else return false;
}
-
+
bool releaseObject(void)
{
if (gripOpen())
@@ -276,21 +275,21 @@
}
else return false;
}
-
+
private:
-
+
actionlib::SimpleActionClient<move_arm::MoveArmAction> move_arm;
actionlib::SimpleActionClient<move_arm::ActuateGripperAction> gripper;
-
+
ros::NodeHandle nh;
-
+
ros::Publisher pubObj;
ros::Publisher pubAttach;
ros::Publisher vmPub;
-
+
};
-
+
void spinThread(void)
{
ros::spin();
@@ -300,14 +299,14 @@
{
ros::init(argc, argv, "test_recognition_planning");
boost::thread th(&spinThread);
-
+
CleanTable ct;
-
+
tabletop_msgs::TableTopObject obj;
-
+
if (ct.findObject(obj))
{
- sleep(10);
+ sleep(10);
if (ct.moveTo(obj))
{
ct.graspObject(obj);
@@ -319,9 +318,9 @@
}
}
}
-
-
-
+
+
+
/*
mapping_msgs::ObjectInMap o1;
o1.header.frame_id = "/base_link";
@@ -336,12 +335,12 @@
o1.pose.position.x = 0.8;
o1.pose.position.y = -0.4;
o1.pose.position.z = 0.6;
-
+
o1.pose.orientation.x = 0.0;
o1.pose.orientation.y = 0.0;
o1.pose.orientation.z = 0.0;
o1.pose.orientation.w = 1.0;
-
+
o1.object.vertices.resize(3);
o1.object.vertices[0].x = 0;
o1.object.vertices[0].y = 0;
@@ -361,19 +360,19 @@
o1.object.triangles[2] = 2;
sleep(1);
-
+
// while(1){
- pub.publish(o1);
+ pub.publish(o1);
// }
th.join();
-
+
return 0;
*/
-
+
ROS_INFO("Done");
-
+
th.join();
-
+
return 0;
}
Modified: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm_setup.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm_setup.h 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm_setup.h 2009-08-21 00:07:36 UTC (rev 22493)
@@ -41,51 +41,51 @@
#include <ros/ros.h>
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
-#include <pr2_mechanism_controllers/TrajectoryQuery.h>
-#include <pr2_mechanism_controllers/TrajectoryCancel.h>
+#include <experimental_controllers/TrajectoryStart.h>
+#include <experimental_controllers/TrajectoryQuery.h>
+#include <experimental_controllers/TrajectoryCancel.h>
#include <planning_environment/monitors/planning_monitor.h>
namespace move_arm
{
-
+
/// the string used internally to access control starting service; this should be remaped in the launch file
static const std::string CONTROL_START_NAME = "controller_start";
-
+
/// the string used internally to access control querying service; this should be remaped in the launch file
static const std::string CONTROL_QUERY_NAME = "controller_query";
-
+
/// the string used internally to access control canceling service; this should be remaped in the launch file
static const std::string CONTROL_CANCEL_NAME = "controller_cancel";
-
+
/// the string used internally to access the long range motion planning service; this should be remaped in the launch file
static const std::string LR_MOTION_PLAN_NAME = "get_motion_plan_lr";
-
+
/// the string used internally to access the short range motion planning service; this should be remaped in the launch file
static const std::string SR_MOTION_PLAN_NAME = "get_motion_plan_sr";
-
+
/// the string used internally to access valid state searching service; this should be remaped in the launch file
static const std::string SEARCH_VALID_STATE_NAME = "get_valid_state";
-
+
/// the string used internally to access inverse kinematics service; this should be remaped in the launch file
static const std::string ARM_IK_NAME = "arm_ik";
-
-
+
+
/** \brief Configuration of actions that need to actuate parts of the robot */
class MoveArmSetup
{
friend class MoveArm;
friend class TeleopArm;
-
+
public:
-
+
MoveArmSetup(void)
{
collisionModels_ = NULL;
planningMonitor_ = NULL;
}
-
+
virtual ~MoveArmSetup(void)
{
if (planningMonitor_)
@@ -93,24 +93,24 @@
if (collisionModels_)
delete collisionModels_;
}
-
-
+
+
bool configure(void);
-
+
protected:
-
+
bool getControlJointNames(std::vector<std::string> &joint_names);
-
+
ros::NodeHandle nodeHandle_;
tf::TransformListener tf_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::PlanningMonitor *planningMonitor_;
-
+
std::string group_;
std::vector<std::string> groupJointNames_;
-
+
};
-
+
}
#endif
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-20 23:52:19 UTC (rev 22492)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-21 00:07:36 UTC (rev 22493)
@@ -47,7 +47,7 @@
#include <motion_planning_msgs/KinematicPath.h>
#include <manipulation_msgs/JointTraj.h>
#include <manipulation_srvs/IKService.h>
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
+#include <experimental_controllers/TrajectoryStart.h>
#include <boost/thread/thread.hpp>
#include <boost/algorithm/string.hpp>
@@ -88,7 +88,7 @@
void printJoints(const planning_environment::KinematicModelStateMonitor &km, const std::vector<std::string> &names)
{
const planning_models::KinematicModel::ModelInfo &mi = km.getKinematicModel()->getModelInfo();
-
+
for (unsigned int i = 0 ; i < names.size(); ++i)
{
int idx = km.getKinematicModel()->getJointIndex(names[i]);
@@ -147,7 +147,7 @@
goal.goal_constraints.joint_constraint[i].tolerance_below[0] = 0.0;
goal.goal_constraints.joint_constraint[i].tolerance_above[0] = 0.0;
}
-
+
goal.contacts.resize(1);
goal.contacts[0].links.push_back("r_gripper_l_finger_link");
goal.contacts[0].links.push_back("r_gripper_r_finger_link");
@@ -157,13 +157,13 @@
goal.contacts[0].depth = 0.04;
goal.contacts[0].bound.type = mapping_msgs::Object::SPHERE;
goal.contacts[0].bound.dimensions.push_back(0.5);
-
+
goal.contacts[0].pose.header.stamp = ros::Time::now();
goal.contacts[0].pose.header.frame_id = "/base_link";
goal.contacts[0].pose.pose.position.x = 1;
goal.contacts[0].pose.pose.position.y = 0;
goal.contacts[0].pose.pose.position.z = 0.5;
-
+
goal.contacts[0].pose.pose.orientation.x = 0;
goal.contacts[0].pose.pose.orientation.y = 0;
goal.contacts[0].pose.pose.orientation.z = 0;
@@ -181,28 +181,28 @@
goal.goal_constraints.pose_constraint[0].pose.pose.position.x = pz[0];
goal.goal_constraints.pose_constraint[0].pose.pose.position.y = pz[1];
goal.goal_constraints.pose_constraint[0].pose.pose.position.z = pz[2];
-
+
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.x = pz[3];
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.y = pz[4];
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = pz[5];
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = pz[6];
-
+
goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.01;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.005;
-
+
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
-
+
goal.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
-
+
goal.contacts.resize(2);
goal.contacts[0].links.push_back("r_gripper_l_finger_link");
goal.contacts[0].links.push_back("r_gripper_r_finger_link");
@@ -249,11 +249,11 @@
request.data.pose_stamped.pose.orientation.z = 0;
request.data.pose_stamped.pose.orientation.w = 1;
request.data.joint_names = names;
-
+
planning_models::StateParams rs(*km.getRobotState());
if (r)
rs.randomState();
-
+
for(unsigned int i = 0; i < names.size() ; ++i)
{
const double *params = rs.getParamsJoint(names[i]);
@@ -283,7 +283,7 @@
std::cerr << "IK Failed" << std::endl;
}
-
+
void diffConfig(const planning_environment::KinematicModelStateMonitor &km, move_arm::MoveArmGoal &goal)
{
std::vector<std::string> names;
@@ -294,7 +294,7 @@
<< std::endl;
names.push_back(goal.goal_constraints.joint_constraint[i].joint_name);
}
-
+
btTransform pose1 = effPosition(km, goal);
move_arm::MoveArmGoal temp;
setConfig(km.getRobotState(), names, temp);
@@ -309,14 +309,14 @@
void viewState(ros::Publisher &view, const planning_environment::KinematicModelStateMonitor &km, const planning_models::StateParams &st)
{
motion_planning_msgs::KinematicPath kp;
-
+
kp.header.frame_id = km.getFrameId();
kp.header.stamp = km.lastJointStateUpdate();
-
+
// fill in start state with current one
std::vector<planning_models::KinematicModel::Joint*> joints;
km.getKinematicModel()->getJoints(joints);
-
+
kp.start_state.resize(joints.size());
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
@@ -341,24 +341,24 @@
int main(int argc, char **argv)
{
ros::init(argc, argv, "cmd_line_move_arm", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
-
+
std::string arm = "r";
if (argc >= 2)
if (argv[1][0] == 'l')
arm = "l";
-
+
ros::NodeHandle nh;
-
+
std::string group = arm == "r" ? "right_arm" : "left_arm";
actionlib::SimpleActionClient<move_arm::MoveArmAction> move_arm(nh, "move_" + group);
actionlib::SimpleActionClient<move_arm::ActuateGripperAction> gripper(nh, "actuate_gripper_" + group);
ros::Publisher view = nh.advertise<motion_planning_msgs::KinematicPath>("executing_kinematic_path", 1);
-
+
ros::Publisher pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_object", 1)...
[truncated message content] |
