Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17987] pkg/trunk
|
From: <stu...@us...> - 2009-06-30 18:52:41
|
Revision: 17987
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17987&view=rev
Author: stuglaser
Date: 2009-06-30 18:52:31 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
Moved mechanism-related services from robot_srvs to mechanism_msgs to close ticket #1625
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/velocity.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py
pkg/trunk/controllers/robot_mechanism_controllers/src/dynamic_loader_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_unplug.h
pkg/trunk/pr2/motorconf_gui/src/motorconf_gui/ui.py
pkg/trunk/pr2/qualification/scripts/caster_test_spawner.py
pkg/trunk/pr2/qualification/scripts/counterbalance_spawner.py
pkg/trunk/pr2/qualification/scripts/full_arm_test_spawner.py
pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate.py
pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/action_mechanism_control.h
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/stacks/mechanism/mechanism_control/scripts/spawner.py
pkg/trunk/stacks/mechanism/mechanism_control/src/action_mechanism_control.cpp
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control/mechanism.py
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/stacks/mechanism/mechanism_msgs/manifest.xml
Removed Paths:
-------------
pkg/trunk/stacks/common_msgs/robot_srvs/srv/KillAndSpawnControllers.srv
pkg/trunk/stacks/common_msgs/robot_srvs/srv/KillController.srv
pkg/trunk/stacks/common_msgs/robot_srvs/srv/ListControllerTypes.srv
pkg/trunk/stacks/common_msgs/robot_srvs/srv/ListControllers.srv
pkg/trunk/stacks/common_msgs/robot_srvs/srv/SpawnController.srv
pkg/trunk/stacks/common_msgs/robot_srvs/srv/SwitchController.srv
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -42,7 +42,7 @@
import rospy
from std_msgs.msg import *
from mechanism_control import mechanism
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
def xml_for(joint):
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -41,7 +41,7 @@
import rospy
from std_msgs.msg import *
from mechanism_control import mechanism
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
def xml_for(joint, p, i, d, iClamp):
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/velocity.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/velocity.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/velocity.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -41,7 +41,7 @@
import rospy
from std_msgs.msg import *
from mechanism_control import mechanism
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
def xml_for(joint, p, i, d, iClamp):
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -1,4 +1,30 @@
#!/usr/bin/env python
+# Copyright (c) 2009, 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, Inc. 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.
PKG = 'robot_mechanism_controllers'
import roslib; roslib.load_manifest(PKG)
@@ -12,7 +38,7 @@
import random
from std_msgs.msg import *
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
class wxeffect_panel(wx.Panel):
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/dynamic_loader_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/dynamic_loader_controller.cpp 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/dynamic_loader_controller.cpp 2009-06-30 18:52:31 UTC (rev 17987)
@@ -32,8 +32,8 @@
*/
#include "robot_mechanism_controllers/dynamic_loader_controller.h"
-#include "robot_srvs/SpawnController.h"
-#include "robot_srvs/KillController.h"
+#include "mechanism_msgs/SpawnController.h"
+#include "mechanism_msgs/KillController.h"
#include <iostream>
#include <boost/thread.hpp>
@@ -57,8 +57,8 @@
void DynamicLoaderController::unloadLibrary(std::vector<std::string> names, lt_dlhandle handle)
{
- robot_srvs::KillController::Request req;
- robot_srvs::KillController::Response resp;
+ mechanism_msgs::KillController::Request req;
+ mechanism_msgs::KillController::Response resp;
BOOST_FOREACH(std::string &name, names) {
req.name = name;
@@ -71,8 +71,8 @@
void DynamicLoaderController::loadLibrary(std::string &xml)
{
- robot_srvs::SpawnController::Request req;
- robot_srvs::SpawnController::Response resp;
+ mechanism_msgs::SpawnController::Request req;
+ mechanism_msgs::SpawnController::Response resp;
req.xml_config = xml;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -5,7 +5,7 @@
import rospy, sys
from robot_mechanism_controllers.srv import *
-from robot_srvs.srv import *
+from mechanism_msgs.srv import *
from std_msgs.msg import *
from time import sleep
Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_unplug.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_unplug.h 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_unplug.h 2009-06-30 18:52:31 UTC (rev 17987)
@@ -48,7 +48,7 @@
#include "robot_mechanism_controllers/CartesianHybridState.h"
// Srvs
-#include <robot_srvs/SwitchController.h>
+#include <mechanism_msgs/SwitchController.h>
//TF
#include <tf/transform_listener.h>
Modified: pkg/trunk/pr2/motorconf_gui/src/motorconf_gui/ui.py
===================================================================
--- pkg/trunk/pr2/motorconf_gui/src/motorconf_gui/ui.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/pr2/motorconf_gui/src/motorconf_gui/ui.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -50,7 +50,7 @@
from std_msgs.msg import *
from mechanism_control import mechanism
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
from mechanism_msgs.msg import MechanismState
@@ -129,7 +129,7 @@
self._last_msg_time = 0
self._core_launcher = self.launch_core()
- rospy.init_node("motorconf_GUI", anonymous = True)
+ rospy.init_node("motorconf_GUI", anonymous = True)
self.start_pr2_etherCAT()
rospy.Subscriber("/mechanism_state", MechanismState, self.on_mech_state_msg)
@@ -371,7 +371,7 @@
time_diff = float(str(data.header.stamp - self._last_msg_time)) / 1000000000
if time_diff < 0.25:
return # Only process msgs at 4 Hz
-
+
self._last_msg_time = data.header.stamp
rospy.logout('Displaying mechanism state msg in window')
Modified: pkg/trunk/pr2/qualification/scripts/caster_test_spawner.py
===================================================================
--- pkg/trunk/pr2/qualification/scripts/caster_test_spawner.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/pr2/qualification/scripts/caster_test_spawner.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -38,7 +38,7 @@
roslib.load_manifest('qualification')
import rospy, sys, time
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
from mechanism_control import mechanism
@@ -54,7 +54,7 @@
try:
rospy.wait_for_service('spawn_controller')
spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
- kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+ kill_controller = rospy.ServiceProxy('kill_controller', KillController)
controller_file = open(sys.argv[1])
controller_xml = controller_file.read() % side
@@ -62,7 +62,7 @@
controller_file.close()
resp = spawn_controller(controller_xml,1)
-
+
if len(resp.ok) != 1 or resp.ok[0] != chr(1):
rospy.logerr('Failed to spawn test controller')
rospy.logerr('Controller XML: %s' % controller_xml)
Modified: pkg/trunk/pr2/qualification/scripts/counterbalance_spawner.py
===================================================================
--- pkg/trunk/pr2/qualification/scripts/counterbalance_spawner.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/pr2/qualification/scripts/counterbalance_spawner.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -41,7 +41,7 @@
from optparse import OptionParser
from std_msgs.msg import *
-from robot_srvs.srv import *
+from mechanism_msgs.srv import *
from std_srvs.srv import *
from robot_mechanism_controllers.srv import *
@@ -49,7 +49,7 @@
from mechanism_control import mechanism
spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
-kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+kill_controller = rospy.ServiceProxy('kill_controller', KillController)
class SendMessageOnSubscribe(rospy.SubscribeListener):
def __init__(self, msg):
@@ -80,7 +80,7 @@
except Exception, e:
print "Failed to spawn holding controller %s" % name
print xml_for_hold(name, p, i, d, iClamp)
-
+
return False
def set_controller(controller, command):
@@ -90,10 +90,10 @@
def hold_arm(side, pan_angle, holding):
if hold_joint("%s_gripper" % side, 15, 0, 1, 1, holding):
set_controller("%s_gripper_hold" % side, float(0.0))
-
+
if hold_joint("%s_wrist_roll" % side, 12, 3, 1, 1, holding):
set_controller("%s_wrist_roll_hold" % side, float(0.0))
-
+
if hold_joint("%s_wrist_flex" % side, 12, 3, 1, 1, holding):
set_controller("%s_wrist_flex_hold" % side, float(0.0))
@@ -121,20 +121,20 @@
side = rospy.get_param("full_arm_test/side")
rospy.init_node('cb_test_spawner_' + side, anonymous=True)
-
+
try:
#joint = side + sys.argv[1]
controller_file = open(sys.argv[1])
# Put side in to controller xml string
controller_xml = controller_file.read() % (side, side)
controller_file.close()
-
+
print controller_xml
holding = []
-
+
rospy.wait_for_service('spawn_controller')
-
+
if hold_joint("torso_lift", 2000000, 0, 1000, 1200, holding):
print 'Raising torso'
set_controller("torso_lift_hold", float(0.30))
@@ -143,13 +143,13 @@
# Hold both arms in place
hold_arm('r', -1.2, holding)
hold_arm('l', 1.2, holding)
-
+
print 'Killing joint controllers'
# Kill controller for given joint
kill_controller(side + '_elbow_flex_hold')
kill_controller(side + '_shoulder_lift_hold')
-
+
if side + '_elbow_flex_hold' in holding:
holding.remove(side + '_elbow_flex_hold')
else:
@@ -159,13 +159,13 @@
holding.remove(side + '_shoulder_lift_hold')
else:
print 'Joint %s is not being held' % joint
-
+
time.sleep(1.0)
-
+
print 'Spawning test controller'
# Spawn test controller and run test
resp = spawn_controller(controller_xml,1)
-
+
if len(resp.ok) != 1 or resp.ok[0] != chr(1):
rospy.logerr('Failed to spawn test controller')
rospy.logerr('Controller XML: %s' % controller_xml)
@@ -174,7 +174,7 @@
print 'Test controller spawned'
holding.append(resp.name[0])
-
+
print 'Test controller is up, running test'
while not rospy.is_shutdown():
time.sleep(0.5)
Modified: pkg/trunk/pr2/qualification/scripts/full_arm_test_spawner.py
===================================================================
--- pkg/trunk/pr2/qualification/scripts/full_arm_test_spawner.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/pr2/qualification/scripts/full_arm_test_spawner.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -41,7 +41,7 @@
from optparse import OptionParser
from std_msgs.msg import *
-from robot_srvs.srv import *
+from mechanism_msgs.srv import *
from std_srvs.srv import *
from robot_mechanism_controllers.srv import *
@@ -49,7 +49,7 @@
from mechanism_control import mechanism
spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
-kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+kill_controller = rospy.ServiceProxy('kill_controller', KillController)
class SendMessageOnSubscribe(rospy.SubscribeListener):
def __init__(self, msg):
@@ -80,7 +80,7 @@
except Exception, e:
print "Failed to spawn holding controller %s" % name
print xml_for_hold(name, p, i, d, iClamp)
-
+
return False
def set_controller(controller, command):
@@ -90,10 +90,10 @@
def hold_arm(side, pan_angle, holding):
if hold_joint("%s_gripper" % side, 15, 0, 1, 1, holding):
set_controller("%s_gripper_hold" % side, float(0.0))
-
+
if hold_joint("%s_wrist_roll" % side, 12, 3, 1, 1, holding):
set_controller("%s_wrist_roll_hold" % side, float(0.0))
-
+
if hold_joint("%s_wrist_flex" % side, 12, 3, 1, 1, holding):
set_controller("%s_wrist_flex_hold" % side, float(1.0))
@@ -121,18 +121,18 @@
side = rospy.get_param("full_arm_test/side")
rospy.init_node('arm_test_spawner_' + side, anonymous=True)
-
+
try:
joint = side + sys.argv[1]
controller_file = open(sys.argv[2])
# Put side in to controller xml string
- controller_xml = controller_file.read() % side
+ controller_xml = controller_file.read() % side
controller_file.close()
-
+
holding = []
-
+
rospy.wait_for_service('spawn_controller')
-
+
if hold_joint("torso_lift", 2000000, 0, 1000, 1200, holding):
print 'Raising torso'
set_controller("torso_lift_hold", float(0.30))
@@ -141,7 +141,7 @@
# Hold both arms in place
hold_arm('r', -1.2, holding)
hold_arm('l', 1.2, holding)
-
+
print 'Killing joint controller %s_hold' % joint
# Kill controller for given joint
kill_controller(joint + '_hold')
@@ -150,20 +150,20 @@
holding.remove(joint + '_hold')
else:
print 'Joint %s is not being held' % joint
-
+
time.sleep(1.0)
-
+
print 'Spawning test controller'
# Spawn test controller and run test
resp = spawn_controller(controller_xml,1)
-
+
if len(resp.ok) != 1 or resp.ok[0] != chr(1):
rospy.logerr('Failed to spawn test controller')
rospy.logerr('Controller XML: %s' % controller_xml)
sys.exit(2)
holding.append(resp.name[0])
-
+
print 'Test controller is up, running test'
while not rospy.is_shutdown():
time.sleep(0.5)
Deleted: pkg/trunk/stacks/common_msgs/robot_srvs/srv/KillAndSpawnControllers.srv
===================================================================
--- pkg/trunk/stacks/common_msgs/robot_srvs/srv/KillAndSpawnControllers.srv 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/common_msgs/robot_srvs/srv/KillAndSpawnControllers.srv 2009-06-30 18:52:31 UTC (rev 17987)
@@ -1,6 +0,0 @@
-string add_config
-string[] kill_name
----
-string[] add_name
-uint8[] add_ok
-uint8[] kill_ok
Deleted: pkg/trunk/stacks/common_msgs/robot_srvs/srv/KillController.srv
===================================================================
--- pkg/trunk/stacks/common_msgs/robot_srvs/srv/KillController.srv 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/common_msgs/robot_srvs/srv/KillController.srv 2009-06-30 18:52:31 UTC (rev 17987)
@@ -1,3 +0,0 @@
-string name
----
-byte ok
Deleted: pkg/trunk/stacks/common_msgs/robot_srvs/srv/ListControllerTypes.srv
===================================================================
--- pkg/trunk/stacks/common_msgs/robot_srvs/srv/ListControllerTypes.srv 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/common_msgs/robot_srvs/srv/ListControllerTypes.srv 2009-06-30 18:52:31 UTC (rev 17987)
@@ -1,2 +0,0 @@
----
-string[] types
\ No newline at end of file
Deleted: pkg/trunk/stacks/common_msgs/robot_srvs/srv/ListControllers.srv
===================================================================
--- pkg/trunk/stacks/common_msgs/robot_srvs/srv/ListControllers.srv 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/common_msgs/robot_srvs/srv/ListControllers.srv 2009-06-30 18:52:31 UTC (rev 17987)
@@ -1,2 +0,0 @@
----
-string[] controllers
Deleted: pkg/trunk/stacks/common_msgs/robot_srvs/srv/SpawnController.srv
===================================================================
--- pkg/trunk/stacks/common_msgs/robot_srvs/srv/SpawnController.srv 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/common_msgs/robot_srvs/srv/SpawnController.srv 2009-06-30 18:52:31 UTC (rev 17987)
@@ -1,6 +0,0 @@
-string xml_config
-uint8 autostart
----
-int8[] ok
-string[] name
-string[] error
Deleted: pkg/trunk/stacks/common_msgs/robot_srvs/srv/SwitchController.srv
===================================================================
--- pkg/trunk/stacks/common_msgs/robot_srvs/srv/SwitchController.srv 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/common_msgs/robot_srvs/srv/SwitchController.srv 2009-06-30 18:52:31 UTC (rev 17987)
@@ -1,4 +0,0 @@
-string[] start_controllers
-string[] stop_controllers
----
-byte ok
Modified: pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate.py
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -44,7 +44,7 @@
roslib.load_manifest('mechanism_bringup')
import rospy
from std_msgs.msg import *
-from robot_srvs.srv import *
+from mechanism_msgs.srv import *
from robot_mechanism_controllers.srv import *
def calibrate(config):
@@ -136,7 +136,7 @@
if not imustatus:
print "Mechanism calibration complete, but IMU calibration failed."
sys.exit(2)
-
+
print "Calibration complete"
if __name__ == '__main__':
Modified: pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -46,7 +46,7 @@
roslib.load_manifest('mechanism_bringup')
import rospy
from std_msgs.msg import *
-from robot_srvs.srv import *
+from mechanism_msgs.srv import *
from robot_mechanism_controllers.srv import *
from robot_mechanism_controllers import controllers
@@ -59,8 +59,8 @@
def peer_subscribe(self, topic_name, topic_publish, peer_publish):
peer_publish(self.msg)
- sleep(0.1)
- # TODO: change this line when flushing messages is implemented
+ sleep(0.1)
+ # TODO: change this line when flushing messages is implemented
# rospy.signal_shutdown("Done")
# sys.exit(0)
@@ -111,9 +111,9 @@
break # Go to next controller if no exception
except:
print "Failed to kill controller %s on try %d" % (name, i)
-
+
#
-# Functions make xml code for controllers
+# Functions make xml code for controllers
#
def xml_for_cal(name, velocity, p, i, d, iClamp):
return """
@@ -121,7 +121,7 @@
<calibrate joint="%s_joint" actuator="%s_motor"
transmission="%s_trans" velocity="%d" />
<pid p="%d" i="%d" d="%d" iClamp="%d" />
-</controller>""" % (name, name, name, name, name, velocity, p, i, d, iClamp)
+</controller>""" % (name, name, name, name, name, velocity, p, i, d, iClamp)
def xml_for_hold(name, p, i, d, iClamp):
return """
@@ -134,15 +134,15 @@
return """
<controller name="cal_%s_wrist" type="WristCalibrationControllerNode">
<calibrate transmission="%s_wrist_trans"
-actuator_l="%s_wrist_l_motor" actuator_r="%s_wrist_r_motor"
-flex_joint="%s_wrist_flex_joint" roll_joint="%s_wrist_roll_joint"
+actuator_l="%s_wrist_l_motor" actuator_r="%s_wrist_r_motor"
+flex_joint="%s_wrist_flex_joint" roll_joint="%s_wrist_roll_joint"
velocity="1.5" />
<pid p="4.0" i="0.2" d="0" iClamp="2.0" />
</controller>""" % (side, side, side, side, side, side)
def hold_joint(name, p, i, d, iClamp, holding):
# Try to launch 3x
- # If launched, add to list of holding controllers and return true
+ # If launched, add to list of holding controllers and return true
for i in range(1,4):
try:
resp = spawn_controller(xml_for_hold(name, p, i, d, iClamp), 1)
@@ -182,7 +182,7 @@
rospy.init_node('calibration', anonymous=True)
holding = [] # Tracks which controllers are holding joints by name
-
+
# Calibrate all joints sequentially
# Hold joints after they're calibrated.
# Can set desired joint position by using name + /set_command service
@@ -190,8 +190,8 @@
imustatus = calibrate_imu()
if not imustatus:
- print "IMU Calibration may have failed."
-
+ rospy.logerr("IMU Calibration failed.")
+
# Check sign on torso lift controller
calibrate(xml_for_cal("torso_lift", -10, 2000000, 0, 10000, 12000))
hold_joint("torso_lift", 1000000, 0.0, 10000, 1000000, holding)
@@ -212,7 +212,7 @@
set_controller("r_elbow_flex_controller", float(-2.0))
set_controller("l_elbow_flex_controller", float(-2.0))
-
+
upperarm_roll_name = "r_upper_arm_roll"
calibrate(xml_for_cal(upperarm_roll_name, 1.0, 6, 0.2, 0, 2))
hold_joint(upperarm_roll_name, 25, 2, 1.0, 1.0, holding)
@@ -228,7 +228,7 @@
hold_joint("r_shoulder_lift", 60, 10, 5, 4, holding)
hold_joint("l_shoulder_lift", 60, 10, 5, 4, holding)
-
+
set_controller("r_shoulder_lift_controller", float(1.0))
set_controller("l_shoulder_lift_controller", float(1.0))
@@ -236,10 +236,10 @@
torso_pub = rospy.Publisher('torso_lift_controller/set_command', Float64)
torso_pub.publish(Float64(float(0.0)))
sleep(1.5)
-
+
# Calibrate other controllers given
xml = ''
-
+
if len(sys.argv) > 1:
xacro_cmd = roslib.packages.get_pkg_dir('xacro', True) + '/xacro.py'
@@ -253,7 +253,7 @@
else:
print "Reading from stdin..."
xml = sys.stdin.read()
-
+
try:
calibrate(xml)
finally:
@@ -265,9 +265,9 @@
break # Go to next controller if no exception
except:
rospy.logerr("Failed to kill controller %s on try %d" % (name, i))
-
+
if not imustatus:
print "Mechanism calibration complete, but IMU calibration failed."
sys.exit(2)
-
- print "Calibration complete"
+
+ rospy.logout("Calibration complete")
Modified: pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/action_mechanism_control.h
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/action_mechanism_control.h 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/action_mechanism_control.h 2009-06-30 18:52:31 UTC (rev 17987)
@@ -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
@@ -37,7 +37,7 @@
#include <ros/node.h>
-#include <robot_srvs/SwitchController.h>
+#include <mechanism_msgs/SwitchController.h>
#include <std_msgs/Empty.h>
#include <pr2_robot_actions/SwitchControllers.h>
#include <pr2_robot_actions/SwitchControllersState.h>
@@ -49,10 +49,10 @@
class ActionMechanismControl: public robot_actions::Action<pr2_robot_actions::SwitchControllers, std_msgs::Empty>{
-public:
+public:
ActionMechanismControl(ros::Node& node);
~ActionMechanismControl();
-
+
virtual robot_actions::ResultStatus execute(const pr2_robot_actions::SwitchControllers& c, std_msgs::Empty&);
Modified: pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-06-30 18:52:31 UTC (rev 17987)
@@ -52,12 +52,12 @@
#include <realtime_tools/realtime_publisher.h>
#include <misc_utils/advertised_service_guard.h>
-#include <robot_srvs/ListControllerTypes.h>
-#include <robot_srvs/ListControllers.h>
-#include <robot_srvs/SpawnController.h>
-#include <robot_srvs/KillController.h>
-#include <robot_srvs/KillAndSpawnControllers.h>
-#include <robot_srvs/SwitchController.h>
+#include <mechanism_msgs/ListControllerTypes.h>
+#include <mechanism_msgs/ListControllers.h>
+#include <mechanism_msgs/SpawnController.h>
+#include <mechanism_msgs/KillController.h>
+#include <mechanism_msgs/KillAndSpawnControllers.h>
+#include <mechanism_msgs/SwitchController.h>
#include <mechanism_msgs/MechanismState.h>
#include <mechanism_msgs/JointStates.h>
#include <diagnostic_msgs/DiagnosticMessage.h>
@@ -80,7 +80,7 @@
// Non real-time functions
bool initXml(TiXmlElement* config);
void getControllerNames(std::vector<std::string> &v);
- bool spawnController(const std::string &xml_string,
+ bool spawnController(const std::string &xml_string,
std::vector<int8_t>& ok, std::vector<std::string>& name);
bool killController(const std::string &name);
bool switchController(const std::vector<std::string>& start_controllers,
@@ -175,23 +175,23 @@
void update(); // Must be realtime safe
- bool listControllerTypes(robot_srvs::ListControllerTypes::Request &req,
- robot_srvs::ListControllerTypes::Response &resp);
- bool listControllers(robot_srvs::ListControllers::Request &req,
- robot_srvs::ListControllers::Response &resp);
- bool killAndSpawnControllers(robot_srvs::KillAndSpawnControllers::Request &req,
- robot_srvs::KillAndSpawnControllers::Response &resp);
- bool switchController(robot_srvs::SwitchController::Request &req,
- robot_srvs::SwitchController::Response &resp);
+ bool listControllerTypes(mechanism_msgs::ListControllerTypes::Request &req,
+ mechanism_msgs::ListControllerTypes::Response &resp);
+ bool listControllers(mechanism_msgs::ListControllers::Request &req,
+ mechanism_msgs::ListControllers::Response &resp);
+ bool killAndSpawnControllers(mechanism_msgs::KillAndSpawnControllers::Request &req,
+ mechanism_msgs::KillAndSpawnControllers::Response &resp);
+ bool switchController(mechanism_msgs::SwitchController::Request &req,
+ mechanism_msgs::SwitchController::Response &resp);
private:
ros::Node *node_;
- bool spawnController(robot_srvs::SpawnController::Request &req,
- robot_srvs::SpawnController::Response &resp);
+ bool spawnController(mechanism_msgs::SpawnController::Request &req,
+ mechanism_msgs::SpawnController::Response &resp);
- bool killController(robot_srvs::KillController::Request &req,
- robot_srvs::KillController::Response &resp);
+ bool killController(mechanism_msgs::KillController::Request &req,
+ mechanism_msgs::KillController::Response &resp);
bool getController(const std::string& name, int& id);
Modified: pkg/trunk/stacks/mechanism/mechanism_control/scripts/spawner.py
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/scripts/spawner.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/mechanism/mechanism_control/scripts/spawner.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -1,10 +1,10 @@
#! /usr/bin/env python
# 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
@@ -13,7 +13,7 @@
# * Neither the name of the Willow Garage, Inc. 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
@@ -37,7 +37,7 @@
import rospy, sys
from mechanism_control import mechanism
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
def print_usage(exit_code = 0):
print 'spawner.py [--stopped] <controllers_config>'
Modified: pkg/trunk/stacks/mechanism/mechanism_control/src/action_mechanism_control.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/src/action_mechanism_control.cpp 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/mechanism/mechanism_control/src/action_mechanism_control.cpp 2009-06-30 18:52:31 UTC (rev 17987)
@@ -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
@@ -41,24 +41,24 @@
namespace mechanism_control{
-ActionMechanismControl::ActionMechanismControl(Node& node):
+ActionMechanismControl::ActionMechanismControl(Node& node):
robot_actions::Action<pr2_robot_actions::SwitchControllers, std_msgs::Empty>("switch_controllers")
{};
-
-
+
+
ActionMechanismControl::~ActionMechanismControl(){};
-
-
+
+
robot_actions::ResultStatus ActionMechanismControl::execute(const pr2_robot_actions::SwitchControllers& c, std_msgs::Empty&)
{
- ROS_INFO("ActionMechanismControl: received request to start %i controllers and stop %i controllers",
+ ROS_INFO("ActionMechanismControl: received request to start %i controllers and stop %i controllers",
c.start_controllers.size(), c.stop_controllers.size());
for (unsigned int i=0; i<c.start_controllers.size(); i++)
ROS_INFO("ActionMechanismControl: - starting controller %s", c.start_controllers[i].c_str());
for (unsigned int i=0; i<c.stop_controllers.size(); i++)
ROS_INFO("ActionMechanismControl: - stopping controller %s", c.stop_controllers[i].c_str());
- robot_srvs::SwitchController::Request req;
- robot_srvs::SwitchController::Response resp;
+ mechanism_msgs::SwitchController::Request req;
+ mechanism_msgs::SwitchController::Response resp;
req.start_controllers = c.start_controllers;
req.stop_controllers = c.stop_controllers;
if (!ros::service::call("switch_controller", req, resp)){
Modified: pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control/mechanism.py
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control/mechanism.py 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control/mechanism.py 2009-06-30 18:52:31 UTC (rev 17987)
@@ -6,7 +6,7 @@
import sys
import rospy
-from robot_srvs.srv import *
+from mechanism_msgs.srv import *
import std_srvs.srv
def list_controller_types():
@@ -54,9 +54,9 @@
resp = s.call(SwitchControllerRequest(start, stop))
if resp.ok == 1:
if st:
- print "Started %s successfully" % name
- else:
- print "Stopped %s successfully" % name
+ print "Started %s successfully" % name
+ else:
+ print "Stopped %s successfully" % name
else:
if st:
print "Error when starting ", name
Modified: pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-06-30 18:52:31 UTC (rev 17987)
@@ -644,8 +644,8 @@
}
bool MechanismControlNode::listControllerTypes(
- robot_srvs::ListControllerTypes::Request &req,
- robot_srvs::ListControllerTypes::Response &resp)
+ mechanism_msgs::ListControllerTypes::Request &req,
+ mechanism_msgs::ListControllerTypes::Response &resp)
{
(void) req;
std::vector<std::string> types = controller::ControllerFactory::Instance().RegisteredIds();
@@ -655,8 +655,8 @@
bool MechanismControlNode::listControllers(
- robot_srvs::ListControllers::Request &req,
- robot_srvs::ListControllers::Response &resp)
+ mechanism_msgs::ListControllers::Request &req,
+ mechanism_msgs::ListControllers::Response &resp)
{
// lock services
boost::mutex::scoped_lock guard(services_lock_);
@@ -677,8 +677,8 @@
bool MechanismControlNode::spawnController(
- robot_srvs::SpawnController::Request &req,
- robot_srvs::SpawnController::Response &resp)
+ mechanism_msgs::SpawnController::Request &req,
+ mechanism_msgs::SpawnController::Response &resp)
{
// lock services
boost::mutex::scoped_lock guard(services_lock_);
@@ -699,8 +699,8 @@
}
bool MechanismControlNode::killController(
- robot_srvs::KillController::Request &req,
- robot_srvs::KillController::Response &resp)
+ mechanism_msgs::KillController::Request &req,
+ mechanism_msgs::KillController::Response &resp)
{
// lock services
boost::mutex::scoped_lock guard(services_lock_);
@@ -709,8 +709,8 @@
}
bool MechanismControlNode::switchController(
- robot_srvs::SwitchController::Request &req,
- robot_srvs::SwitchController::Response &resp)
+ mechanism_msgs::SwitchController::Request &req,
+ mechanism_msgs::SwitchController::Response &resp)
{
// lock services
boost::mutex::scoped_lock guard(services_lock_);
@@ -719,8 +719,8 @@
return true;
}
-bool MechanismControlNode::killAndSpawnControllers(robot_srvs::KillAndSpawnControllers::Request &req,
- robot_srvs::KillAndSpawnControllers::Response &res)
+bool MechanismControlNode::killAndSpawnControllers(mechanism_msgs::KillAndSpawnControllers::Request &req,
+ mechanism_msgs::KillAndSpawnControllers::Response &res)
{
// lock services
boost::mutex::scoped_lock guard(services_lock_);
Modified: pkg/trunk/stacks/mechanism/mechanism_msgs/manifest.xml
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_msgs/manifest.xml 2009-06-30 18:34:45 UTC (rev 17986)
+++ pkg/trunk/stacks/mechanism/mechanism_msgs/manifest.xml 2009-06-30 18:52:31 UTC (rev 17987)
@@ -9,7 +9,7 @@
<depend package="robot_msgs"/>
<depend package="std_msgs" />
<export>
- <cpp cflags="-I${prefix}/msg/cpp"/>
+ <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp" />
</export>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
