Menu
▾
▴
personalrobots-commit
|
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
...
[truncated message content] |
|
From: <tf...@us...> - 2009-06-30 19:43:02
|
Revision: 17991
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17991&view=rev
Author: tfoote
Date: 2009-06-30 19:43:00 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
ColoredLine(s) going to visualization_markers as per robot_msgs API review
Modified Paths:
--------------
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/pedestrian_detector_HOG.cpp
pkg/trunk/vision/people/src/stereo_color_tracker.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_view/manifest.xml
pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
Added Paths:
-----------
pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLine.msg
pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLines.msg
Removed Paths:
-------------
pkg/trunk/stacks/common/image_msgs/msg/ColoredLine.msg
pkg/trunk/stacks/common/image_msgs/msg/ColoredLines.msg
Deleted: pkg/trunk/stacks/common/image_msgs/msg/ColoredLine.msg
===================================================================
--- pkg/trunk/stacks/common/image_msgs/msg/ColoredLine.msg 2009-06-30 19:39:48 UTC (rev 17990)
+++ pkg/trunk/stacks/common/image_msgs/msg/ColoredLine.msg 2009-06-30 19:43:00 UTC (rev 17991)
@@ -1,9 +0,0 @@
-Header header
-string label
-uint8 r
-uint8 g
-uint8 b
-int16 x0
-int16 y0
-int16 x1
-int16 y1
Deleted: pkg/trunk/stacks/common/image_msgs/msg/ColoredLines.msg
===================================================================
--- pkg/trunk/stacks/common/image_msgs/msg/ColoredLines.msg 2009-06-30 19:39:48 UTC (rev 17990)
+++ pkg/trunk/stacks/common/image_msgs/msg/ColoredLines.msg 2009-06-30 19:43:00 UTC (rev 17991)
@@ -1,3 +0,0 @@
-Header header
-string label
-ColoredLine[] lines
Copied: pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLine.msg (from rev 17963, pkg/trunk/stacks/common/image_msgs/msg/ColoredLine.msg)
===================================================================
--- pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLine.msg (rev 0)
+++ pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLine.msg 2009-06-30 19:43:00 UTC (rev 17991)
@@ -0,0 +1,9 @@
+Header header
+string label
+uint8 r
+uint8 g
+uint8 b
+int16 x0
+int16 y0
+int16 x1
+int16 y1
Property changes on: pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLine.msg
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/common/image_msgs/msg/ColoredLine.msg:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLines.msg (from rev 17963, pkg/trunk/stacks/common/image_msgs/msg/ColoredLines.msg)
===================================================================
--- pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLines.msg (rev 0)
+++ pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLines.msg 2009-06-30 19:43:00 UTC (rev 17991)
@@ -0,0 +1,3 @@
+Header header
+string label
+ColoredLine[] lines
Property changes on: pkg/trunk/stacks/visualization_core/visualization_msgs/msg/ColoredLines.msg
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/common/image_msgs/msg/ColoredLines.msg:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/vision/people/src/face_detection.cpp
===================================================================
--- pkg/trunk/vision/people/src/face_detection.cpp 2009-06-30 19:39:48 UTC (rev 17990)
+++ pkg/trunk/vision/people/src/face_detection.cpp 2009-06-30 19:43:00 UTC (rev 17991)
@@ -49,8 +49,8 @@
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
#include "opencv_latest/CvBridge.h"
-#include "image_msgs/ColoredLine.h"
-#include "image_msgs/ColoredLines.h"
+#include "visualization_msgs/ColoredLine.h"
+#include "visualization_msgs/ColoredLines.h"
#include "topic_synchronizer2/topic_synchronizer.h"
#include "tf/transform_listener.h"
#include "visualization_msgs/Marker.h"
@@ -201,7 +201,7 @@
// Advertise the rectangles to draw if stereo_view is running.
if (do_display_ == "remote") {
- clines_pub_ = nh_.advertise<image_msgs::ColoredLines>("lines_to_draw",1);
+ clines_pub_ = nh_.advertise<visualization_msgs::ColoredLines>("lines_to_draw",1);
ROS_INFO_STREAM_NAMED("face_detector","Advertising colored lines to draw remotely.");
}
// Subscribe to filter measurements.
@@ -352,8 +352,8 @@
bool published = false;
- image_msgs::ColoredLines all_cls;
- vector<image_msgs::ColoredLine> lines;
+ visualization_msgs::ColoredLines all_cls;
+ vector<visualization_msgs::ColoredLine> lines;
// Clear out the old visualization markers.
markers_sub_.markers.clear();
markers_sub_.markers = markers_add_.markers;
Modified: pkg/trunk/vision/people/src/pedestrian_detector_HOG.cpp
===================================================================
--- pkg/trunk/vision/people/src/pedestrian_detector_HOG.cpp 2009-06-30 19:39:48 UTC (rev 17990)
+++ pkg/trunk/vision/people/src/pedestrian_detector_HOG.cpp 2009-06-30 19:43:00 UTC (rev 17991)
@@ -46,7 +46,7 @@
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
#include "opencv_latest/CvBridge.h"
-#include "image_msgs/ColoredLines.h"
+#include "visualization_msgs/ColoredLines.h"
#include "topic_synchronizer/topic_synchronizer.h"
#include "tf/transform_listener.h"
#include <tf/message_notifier.h>
@@ -120,7 +120,7 @@
// Advertise the display boxes.
if (do_display_) {
- node_->advertise<image_msgs::ColoredLines>("lines_to_draw",1);
+ node_->advertise<visualization_msgs::ColoredLines>("lines_to_draw",1);
ROS_INFO_STREAM_NAMED("pedestrian_detector_HOG","Advertising colored lines to draw remotely.");
//cv::namedWindow("people detector", 1);
}
Modified: pkg/trunk/vision/people/src/stereo_color_tracker.cpp
===================================================================
--- pkg/trunk/vision/people/src/stereo_color_tracker.cpp 2009-06-30 19:39:48 UTC (rev 17990)
+++ pkg/trunk/vision/people/src/stereo_color_tracker.cpp 2009-06-30 19:43:00 UTC (rev 17991)
@@ -46,7 +46,7 @@
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
#include "opencv_latest/CvBridge.h"
-#include "image_msgs/ColoredLines.h"
+#include "visualization_msgs/ColoredLines.h"
#include "topic_synchronizer/topic_synchronizer.h"
#include "tf/transform_listener.h"
#include <tf/message_notifier.h>
@@ -160,7 +160,7 @@
node_->advertise<people::PositionMeasurement>("people_tracker_measurements",1);
if (do_display_) {
- node_->advertise<image_msgs::ColoredLines>("lines_to_draw",1);
+ node_->advertise<visualization_msgs::ColoredLines>("lines_to_draw",1);
ROS_INFO_STREAM_NAMED("color_tracker","Advertising colored lines to draw remotely.");
}
@@ -740,8 +740,8 @@
if (do_display_) {
- image_msgs::ColoredLines all_cls;
- vector<image_msgs::ColoredLine> lines;
+ visualization_msgs::ColoredLines all_cls;
+ vector<visualization_msgs::ColoredLine> lines;
lines.resize(4);
lines[0].r = 255; lines[0].g = 0; lines[0].b = 0;
lines[1].r = 255; lines[1].g = 0; lines[1].b = 0;
Modified: pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
===================================================================
--- pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-06-30 19:39:48 UTC (rev 17990)
+++ pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-06-30 19:43:00 UTC (rev 17991)
@@ -49,7 +49,6 @@
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
#include "opencv_latest/CvBridge.h"
-#include "image_msgs/ColoredLines.h"
#include "topic_synchronizer/topic_synchronizer.h"
#include "tf/transform_listener.h"
Modified: pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
===================================================================
--- pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-06-30 19:39:48 UTC (rev 17990)
+++ pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-06-30 19:43:00 UTC (rev 17991)
@@ -46,19 +46,12 @@
#include "opencv/cv.h"
#include "opencv/highgui.h"
-//#include "ros/node.h"
-//#include "image_msgs/StereoInfo.h"
-//#include "image_msgs/DisparityInfo.h"
-//#include "image_msgs/Image.h"
-
#include "ros/node.h"
#include "image_msgs/DisparityInfo.h"
#include "image_msgs/StereoInfo.h"
#include "image_msgs/Image.h"
#include "image_msgs/CamInfo.h"
-#include "image_msgs/ColoredLines.h"
-#include "image_msgs/ColoredLine.h"
//#include "CvStereoCamModel.h"
Modified: pkg/trunk/vision/stereo_view/manifest.xml
===================================================================
--- pkg/trunk/vision/stereo_view/manifest.xml 2009-06-30 19:39:48 UTC (rev 17990)
+++ pkg/trunk/vision/stereo_view/manifest.xml 2009-06-30 19:43:00 UTC (rev 17991)
@@ -9,6 +9,7 @@
<depend package="std_msgs"/>
<depend package="std_srvs"/>
<depend package="image_msgs"/>
+ <depend package="visualization_msgs"/>
<depend package="roscpp"/>
<depend package="topic_synchronizer"/>
<depend package="color_calib"/>
Modified: pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-06-30 19:39:48 UTC (rev 17990)
+++ pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-06-30 19:43:00 UTC (rev 17991)
@@ -54,8 +54,8 @@
#include "image_msgs/StereoInfo.h"
#include "image_msgs/Image.h"
#include "image_msgs/CamInfo.h"
-#include "image_msgs/ColoredLines.h"
-#include "image_msgs/ColoredLine.h"
+#include "visualization_msgs/ColoredLines.h"
+#include "visualization_msgs/ColoredLine.h"
#include "CvStereoCamModel.h"
@@ -159,8 +159,8 @@
image_msgs::CvBridge lbridge; /**< CvBridge for the left camera. */
image_msgs::CvBridge rbridge; /**< CvBridge for the right camera. */
image_msgs::CvBridge dbridge; /**< CvBridge for the disparity image. */
- image_msgs::ColoredLines cls;
- map<string,image_msgs::ColoredLines> map_cls;
+ visualization_msgs::ColoredLines cls;
+ map<string,visualization_msgs::ColoredLines> map_cls;
color_calib::Calibration lcal; /**< Color calibration for the left image. */
color_calib::Calibration rcal; /**< Color calibration for the right image. */
@@ -213,7 +213,7 @@
sync.subscribe("stereo/right/cam_info", rcaminfo, 1);
sync.ready();
- subscribe<image_msgs::ColoredLines>("lines_to_draw",cls,&StereoView::line_cb,1);
+ subscribe<visualization_msgs::ColoredLines>("lines_to_draw",cls,&StereoView::line_cb,1);
}
~StereoView()
@@ -234,10 +234,10 @@
boost::mutex::scoped_lock linelock(lines_mutex_);
// Find if there's already a message with this label.
- map<string,image_msgs::ColoredLines>::iterator it = map_cls.find(cls.label);
+ map<string,visualization_msgs::ColoredLines>::iterator it = map_cls.find(cls.label);
if (it == map_cls.end()) {
// If not, inset this new message into the map.
- map_cls.insert(pair<string,image_msgs::ColoredLines>(cls.label,cls));
+ map_cls.insert(pair<string,visualization_msgs::ColoredLines>(cls.label,cls));
}
else {
// If so, overwrite the old list of lines.
@@ -275,7 +275,7 @@
boost::mutex::scoped_lock linelock(lines_mutex_);
// Draw the lines on the image.
- map<string,image_msgs::ColoredLines>::iterator it;
+ map<string,visualization_msgs::ColoredLines>::iterator it;
for (it = map_cls.begin(); it != map_cls.end(); it++) {
for (uint il=0; il<it->second.lines.size(); il++) {
cvLine(lcalimage,
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-06-30 21:52:31
|
Revision: 18006
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18006&view=rev
Author: vijaypradeep
Date: 2009-06-30 21:52:25 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
Moving LaserScannerSignal to pr2_msgs. PointCloud assembler no longer depends on pr2_mechanism_controllers
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp
pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/manifest.xml
pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/src/point_cloud_srv.cpp
Added Paths:
-----------
pkg/trunk/stacks/pr2_drivers/pr2_msgs/msg/LaserScannerSignal.msg
Removed Paths:
-------------
pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-06-30 21:52:25 UTC (rev 18006)
@@ -42,7 +42,7 @@
#include <realtime_tools/realtime_publisher.h>
// Messages
-#include <pr2_mechanism_controllers/LaserScannerSignal.h>
+#include <pr2_msgs/LaserScannerSignal.h>
// Services
#include <robot_mechanism_controllers/SetCommand.h>
@@ -258,9 +258,9 @@
bool first_time_ ;
LaserScannerController::ProfileExecutionState prev_profile_exec_state_ ; //!< Store the previous profileExecutionState. Need this to compare to the current state to detect transitions
- pr2_mechanism_controllers::LaserScannerSignal m_scanner_signal_ ; //!< Stores the message that we want to send at the end of each sweep, and halfway through each sweep
+ pr2_msgs::LaserScannerSignal m_scanner_signal_ ; //!< Stores the message that we want to send at the end of each sweep, and halfway through each sweep
bool need_to_send_msg_ ; //!< Tracks whether we still need to send out the m_scanner_signal_ message.
- realtime_tools::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal>* publisher_ ; //!< Publishes the m_scanner_signal msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <pr2_msgs::LaserScannerSignal>* publisher_ ; //!< Publishes the m_scanner_signal msg from the update() realtime loop
};
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-06-30 21:52:25 UTC (rev 18006)
@@ -45,7 +45,7 @@
#include "filters/transfer_function.h"
// Messages
-#include <pr2_mechanism_controllers/LaserScannerSignal.h>
+#include <pr2_msgs/LaserScannerSignal.h>
#include <pr2_msgs/PeriodicCmd.h>
#include <pr2_mechanism_controllers/TrackLinkCmd.h>
#include <pr2_msgs/LaserTrajCmd.h>
@@ -155,9 +155,9 @@
pr2_msgs::LaserTrajCmd traj_cmd_ ;
pr2_mechanism_controllers::TrackLinkCmd track_link_cmd_ ;
- pr2_mechanism_controllers::LaserScannerSignal m_scanner_signal_ ; //!< Stores the message that we want to send at the end of each sweep, and halfway through each sweep
+ pr2_msgs::LaserScannerSignal m_scanner_signal_ ; //!< Stores the message that we want to send at the end of each sweep, and halfway through each sweep
bool need_to_send_msg_ ; //!< Tracks whether we still need to send out the m_scanner_signal_ message.
- realtime_tools::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal>* publisher_ ; //!< Publishes the m_scanner_signal msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <pr2_msgs::LaserScannerSignal>* publisher_ ; //!< Publishes the m_scanner_signal msg from the update() realtime loop
};
}
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg 2009-06-30 21:52:25 UTC (rev 18006)
@@ -1,7 +0,0 @@
-
-
-Header header
-
-# signal == 0 => Half profile complete
-# signal == 1 => Full Profile Complete
-int32 signal
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-06-30 21:52:25 UTC (rev 18006)
@@ -573,7 +573,7 @@
if (publisher_ != NULL) // Make sure that we don't memory leak if initXml gets called twice
delete publisher_ ;
- publisher_ = new realtime_tools::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
+ publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
return true;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-06-30 21:52:25 UTC (rev 18006)
@@ -489,7 +489,7 @@
if (publisher_ != NULL) // Make sure that we don't memory leak if initXml gets called twice
delete publisher_ ;
- publisher_ = new realtime_tools::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
+ publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
prev_profile_time_ = 0.0 ;
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml 2009-06-30 21:52:25 UTC (rev 18006)
@@ -15,7 +15,7 @@
<depend package="robot_msgs"/>
<depend package="mechanism_msgs" />
<depend package="sensor_msgs"/>
- <depend package="pr2_mechanism_controllers"/>
+ <depend package="pr2_msgs"/>
<depend package="image_msgs" />
<depend package="mechanism_msgs" />
<depend package="message_filters" />
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp 2009-06-30 21:52:25 UTC (rev 18006)
@@ -39,7 +39,7 @@
// Messages
#include "calibration_msgs/DenseLaserSnapshot.h"
-#include "pr2_mechanism_controllers/LaserScannerSignal.h"
+#include "pr2_msgs/LaserScannerSignal.h"
using namespace dense_laser_assembler ;
@@ -64,7 +64,7 @@
}
- void scannerSignalCallback(const pr2_mechanism_controllers::LaserScannerSignalConstPtr& cur_signal)
+ void scannerSignalCallback(const pr2_msgs::LaserScannerSignalConstPtr& cur_signal)
{
if (cur_signal->signal == 128 || cur_signal->signal == 129) // These codes imply that this is the first signal during a given profile type
first_time_ = true ;
@@ -117,7 +117,7 @@
ros::Subscriber signal_sub_ ;
ros::Publisher snapshot_pub_ ;
- pr2_mechanism_controllers::LaserScannerSignal prev_signal_;
+ pr2_msgs::LaserScannerSignal prev_signal_;
bool first_time_ ;
} ;
Modified: pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/manifest.xml
===================================================================
--- pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/manifest.xml 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/manifest.xml 2009-06-30 21:52:25 UTC (rev 18006)
@@ -13,7 +13,7 @@
<depend package="sensor_msgs"/>
<depend package="tf"/>
<depend package="pr2_srvs"/>
- <depend package="pr2_mechanism_controllers"/>
+ <depend package="pr2_msgs"/>
# For Testing
<depend package="rostest"/>
<depend package="gtest"/>
Modified: pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2009-06-30 21:52:25 UTC (rev 18006)
@@ -39,7 +39,7 @@
// Messages
#include "robot_msgs/PointCloud.h"
-#include "pr2_mechanism_controllers/LaserScannerSignal.h"
+#include "pr2_msgs/LaserScannerSignal.h"
using namespace robot_msgs ;
@@ -58,8 +58,8 @@
public:
- pr2_mechanism_controllers::LaserScannerSignal prev_signal_;
- pr2_mechanism_controllers::LaserScannerSignal cur_signal_;
+ pr2_msgs::LaserScannerSignal prev_signal_;
+ pr2_msgs::LaserScannerSignal cur_signal_;
bool first_time_ ;
Modified: pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/src/point_cloud_srv.cpp
===================================================================
--- pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/src/point_cloud_srv.cpp 2009-06-30 21:51:46 UTC (rev 18005)
+++ pkg/trunk/stacks/pr2_drivers/point_cloud_assembler/src/point_cloud_srv.cpp 2009-06-30 21:52:25 UTC (rev 18006)
@@ -40,7 +40,7 @@
// Messages
#include "robot_msgs/PointCloud.h"
-#include "pr2_mechanism_controllers/LaserScannerSignal.h"
+#include "pr2_msgs/LaserScannerSignal.h"
#include <boost/thread/mutex.hpp>
@@ -63,7 +63,7 @@
class PointCloudSrv : public ros::Node
{
private:
- pr2_mechanism_controllers::LaserScannerSignal laser_scanner_signal_;
+ pr2_msgs::LaserScannerSignal laser_scanner_signal_;
ros::Time laser_time_;
boost::mutex laser_mutex_;
Copied: pkg/trunk/stacks/pr2_drivers/pr2_msgs/msg/LaserScannerSignal.msg (from rev 18005, pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg)
===================================================================
--- pkg/trunk/stacks/pr2_drivers/pr2_msgs/msg/LaserScannerSignal.msg (rev 0)
+++ pkg/trunk/stacks/pr2_drivers/pr2_msgs/msg/LaserScannerSignal.msg 2009-06-30 21:52:25 UTC (rev 18006)
@@ -0,0 +1,7 @@
+
+
+Header header
+
+# signal == 0 => Half profile complete
+# signal == 1 => Full Profile Complete
+int32 signal
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-06-30 21:58:28
|
Revision: 18010
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18010&view=rev
Author: tfoote
Date: 2009-06-30 21:58:20 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
moving tabletop messages out of common_msgs stack
Added Paths:
-----------
pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/
pkg/trunk/demos/tabletop_manipulation/tabletop_srvs/
Removed Paths:
-------------
pkg/trunk/stacks/common_msgs/tabletop_msgs/
pkg/trunk/stacks/common_msgs/tabletop_srvs/
Property changes on: pkg/trunk/demos/tabletop_manipulation/tabletop_msgs
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/stacks/common_msgs/tabletop_msgs:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Property changes on: pkg/trunk/demos/tabletop_manipulation/tabletop_srvs
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/stacks/common_msgs/tabletop_srvs:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-06-30 22:03:44
|
Revision: 18011
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18011&view=rev
Author: tfoote
Date: 2009-06-30 22:03:33 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
Image from image_msgs -> sensor_msgs #1661
Modified Paths:
--------------
pkg/trunk/calibration/calibration_msgs/manifest.xml
pkg/trunk/calibration/kinematic_calibration/deprecated/camera_cal_sampler.h
pkg/trunk/calibration/kinematic_calibration/deprecated/image_point_cache.h
pkg/trunk/calibration/kinematic_calibration/deprecated/state_history.h
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/image_point_stream.py
pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/msg_cache.py
pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/calibration/optical_flag_calibration/manifest.xml
pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
pkg/trunk/drivers/cam/camera_trigger_test/manifest.xml
pkg/trunk/drivers/cam/camera_trigger_test/src/timestamp_test.cpp
pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp
pkg/trunk/drivers/cam/forearm_cam/manifest.xml
pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
pkg/trunk/drivers/cam/forearm_cam/src/utilities/focus.cpp
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/laser_image_node.py
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_prosilica.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_prosilica.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py
pkg/trunk/drivers/swiss_ranger/sr3000/manifest.xml
pkg/trunk/drivers/swiss_ranger/sr3000/src/sr3000.cpp
pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/projection.h
pkg/trunk/mapping/annotated_planar_patch_map/manifest.xml
pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_info.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_pcd_via_service.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_planar_patch_map.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map_via_service.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/empty_annotated_map.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/image_value_node.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/projection.cpp
pkg/trunk/mapping/annotated_planar_patch_map/test/projection_test.cpp
pkg/trunk/mapping/annotated_planar_patch_map/test/send_annotation.py
pkg/trunk/mapping/door_stereo_detector/manifest.xml
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/openrave_planning/openraveros/manifest.xml
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSensorGetData.m
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorgetdata.srv
pkg/trunk/pr2/qualification/tests/forearm_cam_test/led_flash_test.cpp
pkg/trunk/sandbox/camera_calibration/src/nodes/calibration_client.cpp
pkg/trunk/sandbox/camera_calibration/src/nodes/calibration_server.cpp
pkg/trunk/sandbox/image_publisher/include/image_publisher/image_publisher.h
pkg/trunk/sandbox/image_publisher/mainpage.dox
pkg/trunk/sandbox/image_publisher/manifest.xml
pkg/trunk/sandbox/image_publisher/src/decoder.cpp
pkg/trunk/sandbox/image_publisher/src/image_publisher.cpp
pkg/trunk/sandbox/image_publisher/src/test_publisher.cpp
pkg/trunk/sandbox/message_filters/include/message_filters/consumer.h
pkg/trunk/sandbox/message_filters/manifest.xml
pkg/trunk/sandbox/message_filters/src/filter_example.cpp
pkg/trunk/sandbox/pf_object_detector/mainpage.dox
pkg/trunk/sandbox/pf_object_detector/src/pf_detector_node.m
pkg/trunk/sandbox/stanleyi/stanleyi.cpp
pkg/trunk/stacks/camera_drivers/dcam/manifest.xml
pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp
pkg/trunk/stacks/camera_drivers/dcam/src/nodes/stereodcam.cpp
pkg/trunk/stacks/camera_drivers/prosilica_cam/manifest.xml
pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/fake_poll_node.cpp
pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/prosilica_grab.cpp
pkg/trunk/stacks/camera_drivers/prosilica_cam/src/nodes/prosilica_node.cpp
pkg/trunk/stacks/camera_drivers/prosilica_cam/srv/CamInfo.srv
pkg/trunk/stacks/camera_drivers/prosilica_cam/srv/PolledImage.srv
pkg/trunk/stacks/opencv/opencv_latest/include/opencv_latest/CvBridge.h
pkg/trunk/stacks/opencv/opencv_latest/manifest.xml
pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/manifest.xml
pkg/trunk/stacks/semantic_mapping/door_handle_detector/manifest.xml
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h
pkg/trunk/stacks/stereo/stereo_image_proc/manifest.xml
pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp
pkg/trunk/stacks/stereo/stereo_image_proc/src/stereoproc.cpp
pkg/trunk/stacks/visual_feature_detectors/checkerboard_detector/checkerboard_calibration.cpp
pkg/trunk/stacks/visual_feature_detectors/checkerboard_detector/checkerboard_detector.cpp
pkg/trunk/stacks/visual_feature_detectors/checkerboard_detector/manifest.xml
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/tracker_base.h
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/manifest.xml
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/tracker_base.cpp
pkg/trunk/stacks/visualization/image_view/image_view.cpp
pkg/trunk/stacks/visualization/image_view/manifest.xml
pkg/trunk/stacks/visualization/rviz/manifest.xml
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/camera_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/camera_display.h
pkg/trunk/stacks/visualization/rviz/src/rviz/image/ros_image_texture.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/image/ros_image_texture.h
pkg/trunk/util/prosilica_capture_rectified/manifest.xml
pkg/trunk/util/prosilica_capture_rectified/src/prosilica_capture.cpp
pkg/trunk/vision/cmvision/manifest.xml
pkg/trunk/vision/cmvision/src/CMVisionBF.h
pkg/trunk/vision/color_calib/calib_node_dcam.cpp
pkg/trunk/vision/color_calib/manifest.xml
pkg/trunk/vision/cv_mech_turk/manifest.xml
pkg/trunk/vision/cv_mech_turk/src/annotation_to_data_sync.py
pkg/trunk/vision/cv_mech_turk/src/cv_mturk.cpp
pkg/trunk/vision/cv_mech_turk/src/mt_annotate_simple.py
pkg/trunk/vision/cv_mech_turk/src/mt_manual_select.cpp
pkg/trunk/vision/cv_mech_turk/src/mt_manual_select_cv.cpp
pkg/trunk/vision/cv_mech_turk/src/snapper.py
pkg/trunk/vision/image_segmentation/src/msfhseg_node.cpp
pkg/trunk/vision/jpeg/mainpage.dox
pkg/trunk/vision/jpeg/manifest.xml
pkg/trunk/vision/jpeg/src/decoder.cpp
pkg/trunk/vision/jpeg/src/encoder.cpp
pkg/trunk/vision/led_detection/include/led_detection/led_detector.h
pkg/trunk/vision/led_detection/include/led_detection/led_detector_node.h
pkg/trunk/vision/led_detection/manifest.xml
pkg/trunk/vision/led_detection/src/led_detector.cpp
pkg/trunk/vision/led_detection/src/led_detector_node.cpp
pkg/trunk/vision/neven_face_detector/nevenFD.cpp
pkg/trunk/vision/people/manifest.xml
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/pedestrian_detector_HOG.cpp
pkg/trunk/vision/people/src/stereo_color_tracker.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/people/src/track_starter_gui.cpp
pkg/trunk/vision/recognition_lambertian/manifest.xml
pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp
pkg/trunk/vision/recognition_lambertian/src/rec_lam_normal_features.cpp
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/stereo_capture/manifest.xml
pkg/trunk/vision/stereo_capture/src/periodic_capture.cpp
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_checkerboard_detector/include/stereo_checkerboard_detector/mono_checkerboard_helper.h
pkg/trunk/vision/stereo_checkerboard_detector/include/stereo_checkerboard_detector/reprojection_helper.h
pkg/trunk/vision/stereo_checkerboard_detector/include/stereo_checkerboard_detector/stereo_checkerboard_helper.h
pkg/trunk/vision/stereo_checkerboard_detector/manifest.xml
pkg/trunk/vision/stereo_checkerboard_detector/src/checkerboard_corners_node.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/image_pc_debugger.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/mono_checkerboard_helper.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/reprojection_helper.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_helper.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
pkg/trunk/vision/stereo_checkerboard_detector/test/reprojection_unittest.cpp
pkg/trunk/vision/stereo_view/manifest.xml
pkg/trunk/vision/stereo_view/stereo_view.cpp
pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
pkg/trunk/vision/vision_tests/manifest.xml
pkg/trunk/vision/vision_tests/nodes/sink.py
pkg/trunk/vision/vision_tests/nodes/source.py
pkg/trunk/vision/visual_odometry/manifest.xml
pkg/trunk/vision/visual_odometry/nodes/diff.py
pkg/trunk/vision/visual_odometry/nodes/vo.py
pkg/trunk/vision/visual_odometry/scripts/mkgeo.py
pkg/trunk/vision/visual_odometry/scripts/mkrun.py
pkg/trunk/vision/vslam/manifest.xml
pkg/trunk/vision/vslam/nodes/fixplay.py
pkg/trunk/vision/vslam/nodes/picmap.py
pkg/trunk/vision/vslam/nodes/roadmap.py
pkg/trunk/vision/vslam/nodes/watchmap.py
Added Paths:
-----------
pkg/trunk/stacks/common_msgs/sensor_msgs/include/
pkg/trunk/stacks/common_msgs/sensor_msgs/include/sensor_msgs/
pkg/trunk/stacks/common_msgs/sensor_msgs/include/sensor_msgs/FillImage.h
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/CamInfo.msg
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/DisparityInfo.msg
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/Image.msg
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/RawStereo.msg
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/StereoInfo.msg
pkg/trunk/stacks/common_msgs/sensor_msgs/octave/
pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_processImage.m
pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_writeImageMsg.m
Removed Paths:
-------------
pkg/trunk/stacks/common/image_msgs/
Modified: pkg/trunk/calibration/calibration_msgs/manifest.xml
===================================================================
--- pkg/trunk/calibration/calibration_msgs/manifest.xml 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/calibration_msgs/manifest.xml 2009-06-30 22:03:33 UTC (rev 18011)
@@ -9,7 +9,7 @@
<depend package="std_msgs" />
<depend package="robot_msgs" />
- <depend package="image_msgs" />
+ <depend package="sensor_msgs" />
<export>
<cpp cflags="-I${prefix}/msg/cpp" />
Modified: pkg/trunk/calibration/kinematic_calibration/deprecated/camera_cal_sampler.h
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/deprecated/camera_cal_sampler.h 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/deprecated/camera_cal_sampler.h 2009-06-30 22:03:33 UTC (rev 18011)
@@ -45,8 +45,8 @@
#include "kinematic_calibration/Capture.h"
#include "robot_msgs/MechanismState.h"
-#include "image_msgs/Image.h"
-#include "image_msgs/CamInfo.h"
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/CamInfo.h"
#include "topic_synchronizer/topic_synchronizer.h"
#include "tinyxml/tinyxml.h"
@@ -78,7 +78,7 @@
lock_.unlock() ;
}
- image_msgs::ImagePointStamped& getMsg()
+ sensor_msgs::ImagePointStamped& getMsg()
{
return msg_ ;
}
@@ -100,7 +100,7 @@
return latest_time ;
}
- void findClosestBefore(const ros::Time& time, image_msgs::ImagePointStamped& data_out)
+ void findClosestBefore(const ros::Time& time, sensor_msgs::ImagePointStamped& data_out)
{
lock_.lock() ;
cache_.findClosestBefore(time, data_out) ;
@@ -116,7 +116,7 @@
ImagePointCache cache_ ;
boost::mutex lock_ ;
- image_msgs::ImagePointStamped msg_ ;
+ sensor_msgs::ImagePointStamped msg_ ;
} ;
class CameraCalSampler
Modified: pkg/trunk/calibration/kinematic_calibration/deprecated/image_point_cache.h
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/deprecated/image_point_cache.h 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/deprecated/image_point_cache.h 2009-06-30 22:03:33 UTC (rev 18011)
@@ -40,7 +40,7 @@
#include "kinematic_calibration/msg_cache.h"
-#include "image_msgs/ImagePointStamped.h"
+#include "sensor_msgs/ImagePointStamped.h"
#include "kinematic_calibration/Interval.h"
#include "tinyxml/tinyxml.h"
@@ -49,10 +49,10 @@
{
-class ImagePointCache : public MsgCache<image_msgs::ImagePointStamped>
+class ImagePointCache : public MsgCache<sensor_msgs::ImagePointStamped>
{
public:
- ImagePointCache(unsigned int N=1) : MsgCache<image_msgs::ImagePointStamped>(N)
+ ImagePointCache(unsigned int N=1) : MsgCache<sensor_msgs::ImagePointStamped>(N)
{
}
@@ -65,7 +65,7 @@
bool isStable(Interval interval, unsigned int min_samples, double pos_tolerance)
{
- std::deque<image_msgs::ImagePointStamped>::iterator it ;
+ std::deque<sensor_msgs::ImagePointStamped>::iterator it ;
it = storage_.begin() ;
double led_min_x, led_min_y ;
Modified: pkg/trunk/calibration/kinematic_calibration/deprecated/state_history.h
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/deprecated/state_history.h 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/deprecated/state_history.h 2009-06-30 22:03:33 UTC (rev 18011)
@@ -40,7 +40,7 @@
#include "robot_msgs/MechanismState.h"
#include "robot_msgs/PointStamped.h"
-#include "image_msgs/Image.h"
+#include "sensor_msgs/Image.h"
#include "kinematic_calibration/CameraCalSample.h"
#include <limits>
@@ -80,8 +80,8 @@
std::vector<double> joint_min(joint_map.size(), numeric_limits<double>::max()) ;
std::vector<double> joint_max(joint_map.size(), -numeric_limits<double>::max()) ;
- std::vector<image_msgs::ImagePoint> led_min(storage_.begin()->get_points_size()) ;
- std::vector<image_msgs::ImagePoint> led_max(storage_.begin()->get_points_size()) ;
+ std::vector<sensor_msgs::ImagePoint> led_min(storage_.begin()->get_points_size()) ;
+ std::vector<sensor_msgs::ImagePoint> led_max(storage_.begin()->get_points_size()) ;
for (unsigned int i=0; i<storage_.begin()->get_points_size(); i++)
{
Modified: pkg/trunk/calibration/kinematic_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-06-30 22:03:33 UTC (rev 18011)
@@ -10,7 +10,7 @@
<depend package="mocap_msgs" />
<depend package="std_msgs" />
<depend package="topic_synchronizer" />
- <depend package="image_msgs" />
+ <depend package="sensor_msgs" />
<depend package="newmat10"/>
<depend package="pr2_mechanism_controllers" />
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-06-30 22:03:33 UTC (rev 18011)
@@ -5,7 +5,7 @@
Header header
# Raw Stereocam Sensor Data
-image_msgs/RawStereo raw_stereo
+sensor_msgs/RawStereo raw_stereo
# Mechanism State - Stores the robots joint angles
mechanism_msgs/MechanismState mechanism_state
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-06-30 22:03:33 UTC (rev 18011)
@@ -5,7 +5,7 @@
Header header
# Raw Stereocam Sensor Data
-image_msgs/RawStereo raw_stereo
+sensor_msgs/RawStereo raw_stereo
# Mechanism State - Stores the robots joint angles
mechanism_msgs/MechanismState mechanism_state
@@ -17,5 +17,5 @@
mocap_msgs/MocapSnapshot mocap_snapshot
# Data from multiple monocular cameras
-image_msgs/Image[] image
-image_msgs/CamInfo[] cam_info
+sensor_msgs/Image[] image
+sensor_msgs/CamInfo[] cam_info
Modified: pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-06-30 22:03:33 UTC (rev 18011)
@@ -5,12 +5,12 @@
Header header
# Stereocam Sensor Data
-image_msgs/Image left_image
-image_msgs/Image right_image
-image_msgs/Image disparity_image
-image_msgs/StereoInfo stereo_info
-image_msgs/CamInfo left_info
-image_msgs/CamInfo right_info
+sensor_msgs/Image left_image
+sensor_msgs/Image right_image
+sensor_msgs/Image disparity_image
+sensor_msgs/StereoInfo stereo_info
+sensor_msgs/CamInfo left_info
+sensor_msgs/CamInfo right_info
# Mechanism State - Stores the robots joint angles
mechanism_msgs/MechanismState mechanism_state
Modified: pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/image_point_stream.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/image_point_stream.py 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/image_point_stream.py 2009-06-30 22:03:33 UTC (rev 18011)
@@ -40,7 +40,7 @@
from kinematic_calibration.msg_cache import MsgCache
from kinematic_calibration.msg import *
-from image_msgs.msg import *
+from sensor_msgs.msg import *
from roslib import rostime
class ImagePointStats() :
Modified: pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/msg_cache.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/msg_cache.py 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/msg_cache.py 2009-06-30 22:03:33 UTC (rev 18011)
@@ -42,7 +42,7 @@
#from pr2_mechanism_controllers.srv import *
#from robot_msgs.msg import *
-#from image_msgs.msg import *
+#from sensor_msgs.msg import *
class MsgCache() :
def __init__(self, max_len) :
Modified: pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp 2009-06-30 22:03:33 UTC (rev 18011)
@@ -43,8 +43,8 @@
#include "std_msgs/Empty.h"
#include "robot_msgs/PointCloud.h"
-#include "image_msgs/Image.h"
-#include "image_msgs/CamInfo.h"
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/CamInfo.h"
#include "kinematic_calibration/CalSnapshot.h"
@@ -88,15 +88,15 @@
boost::mutex stereo_lock_ ;
ADD_MSG(robot_msgs::PointCloud, corners_left_) ;
ADD_MSG(robot_msgs::PointCloud, corners_right_) ;
- ADD_MSG(image_msgs::Image, left_debug_) ;
- ADD_MSG(image_msgs::Image, right_debug_) ;
- ADD_MSG(image_msgs::CamInfo, left_info_) ;
- ADD_MSG(image_msgs::CamInfo, right_info_) ;
+ ADD_MSG(sensor_msgs::Image, left_debug_) ;
+ ADD_MSG(sensor_msgs::Image, right_debug_) ;
+ ADD_MSG(sensor_msgs::CamInfo, left_info_) ;
+ ADD_MSG(sensor_msgs::CamInfo, right_info_) ;
// Laser Messages
boost::mutex laser_lock_ ;
ADD_MSG(robot_msgs::PointCloud, laser_measurement_) ;
- ADD_MSG(image_msgs::Image, laser_debug_) ;
+ ADD_MSG(sensor_msgs::Image, laser_debug_) ;
// Empty message used for callbacks
std_msgs::Empty capture_msg_ ;
@@ -118,9 +118,9 @@
waiting_for_keypress_ = false ;
node_->advertise<CalSnapshot>("~snapshots", 1) ;
- node_->advertise<image_msgs::Image>("~left", 1) ;
- node_->advertise<image_msgs::Image>("~right", 1) ;
- node_->advertise<image_msgs::Image>("~laser", 1) ;
+ node_->advertise<sensor_msgs::Image>("~left", 1) ;
+ node_->advertise<sensor_msgs::Image>("~right", 1) ;
+ node_->advertise<sensor_msgs::Image>("~laser", 1) ;
node_->subscribe("mechanism_state", mech_state_, &LaserHeadGrabber::mechStateCallback, this, 1) ;
@@ -391,7 +391,7 @@
return sample ;
}
- SensorSample buildCamInfoSample(const image_msgs::CamInfo& info, const string& sensor, const string& target)
+ SensorSample buildCamInfoSample(const sensor_msgs::CamInfo& info, const string& sensor, const string& target)
{
SensorSample sample ;
sample.sensor = sensor ;
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-06-30 22:03:33 UTC (rev 18011)
@@ -44,9 +44,9 @@
#include "std_msgs/Empty.h"
#include "robot_msgs/PointCloud.h"
-#include "image_msgs/RawStereo.h"
-#include "image_msgs/Image.h"
-#include "image_msgs/CamInfo.h"
+#include "sensor_msgs/RawStereo.h"
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/CamInfo.h"
#include "kinematic_calibration/CalibrationData2.h"
@@ -87,15 +87,15 @@
std_msgs::Empty capture_msg_ ;
// Dcam messages
- image_msgs::RawStereo raw_stereo_ ;
- image_msgs::RawStereo safe_raw_stereo_ ;
+ sensor_msgs::RawStereo raw_stereo_ ;
+ sensor_msgs::RawStereo safe_raw_stereo_ ;
boost::mutex raw_stereo_lock_ ;
// HiRes Camera messages
- image_msgs::Image hi_res_image_ ;
- image_msgs::Image safe_hi_res_image_ ;
- image_msgs::CamInfo hi_res_info_ ;
- image_msgs::CamInfo safe_hi_res_info_ ;
+ sensor_msgs::Image hi_res_image_ ;
+ sensor_msgs::Image safe_hi_res_image_ ;
+ sensor_msgs::CamInfo hi_res_info_ ;
+ sensor_msgs::CamInfo safe_hi_res_info_ ;
boost::mutex hi_res_lock_ ;
// Point Cloud Messages
Modified: pkg/trunk/calibration/optical_flag_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/optical_flag_calibration/manifest.xml 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/optical_flag_calibration/manifest.xml 2009-06-30 22:03:33 UTC (rev 18011)
@@ -9,7 +9,7 @@
<depend package="std_msgs" />
<!-- depend package="robot_kinematics" / -->
<!-- depend package="topic_synchronizer" / -->
- <!-- depend package="image_msgs" / -->
+ <!-- depend package="sensor_msgs" / -->
<!-- depend package="newmat10"/ -->
<depend package="pr2_mechanism_controllers" />
<depend package="manipulation_msgs" />
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml 2009-06-30 22:03:33 UTC (rev 18011)
@@ -10,6 +10,6 @@
<depend package="std_msgs" />
<depend package="tf" />
<depend package="laser_scan" />
- <depend package="image_msgs" />
+ <depend package="sensor_msgs" />
</package>
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-06-30 22:03:33 UTC (rev 18011)
@@ -40,7 +40,7 @@
#include "tf/transform_datatypes.h"
#include "mechanism_msgs/MechanismState.h"
#include "sensor_msgs/LaserScan.h"
-#include "image_msgs/RawStereo.h"
+#include "sensor_msgs/RawStereo.h"
class LaserHeadKinematics
{
@@ -62,14 +62,14 @@
&LaserHeadKinematics::updateTiltScan, this) ;
// Raw Stereo Updates
- stereo_pub_ = nh.advertise<image_msgs::RawStereo>("/stereo/raw_stereo_cal", 100) ;
+ stereo_pub_ = nh.advertise<sensor_msgs::RawStereo>("/stereo/raw_stereo_cal", 100) ;
stereo_sub_ = nh.subscribe("/stereo/raw_stereo", 100,
&LaserHeadKinematics::updateStereo, this) ;
}
- void updateStereo(const image_msgs::RawStereoConstPtr& msg_in)
+ void updateStereo(const sensor_msgs::RawStereoConstPtr& msg_in)
{
- image_msgs::RawStereo msg_out ;
+ sensor_msgs::RawStereo msg_out ;
msg_out = *msg_in ;
msg_out.header.frame_id = "stereo_optical_frame_cal" ;
stereo_pub_.publish(msg_out) ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-06-30 22:03:33 UTC (rev 18011)
@@ -40,7 +40,7 @@
#include "tf/transform_datatypes.h"
#include "mechanism_msgs/MechanismState.h"
#include "sensor_msgs/LaserScan.h"
-#include "image_msgs/RawStereo.h"
+#include "sensor_msgs/RawStereo.h"
class LaserHeadKinematics
{
@@ -62,14 +62,14 @@
&LaserHeadKinematics::updateTiltScan, this) ;
// Raw Stereo Updates
- stereo_pub_ = nh.advertise<image_msgs::RawStereo>("/stereo/raw_stereo_cal", 100) ;
+ stereo_pub_ = nh.advertise<sensor_msgs::RawStereo>("/stereo/raw_stereo_cal", 100) ;
stereo_sub_ = nh.subscribe("/stereo/raw_stereo", 100,
&LaserHeadKinematics::updateStereo, this) ;
}
- void updateStereo(const image_msgs::RawStereoConstPtr& msg_in)
+ void updateStereo(const sensor_msgs::RawStereoConstPtr& msg_in)
{
- image_msgs::RawStereo msg_out ;
+ sensor_msgs::RawStereo msg_out ;
msg_out = *msg_in ;
msg_out.header.frame_id = "stereo_optical_frame_cal" ;
msg_out.right_info.P[3] = PR_03_ ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-06-30 21:58:20 UTC (rev 18010)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-06-30 22:03:33 UTC (rev 18011)
@@ -40,7 +40,7 @@
#include "tf/transform_datatypes.h"
#include "mechanism_msgs/MechanismState.h"
#include "sensor_msgs/LaserScan.h"
-#include "image_msgs/RawStereo.h"
+#include "sensor_msgs/RawStereo.h"
class LaserHeadKinematics
{
@@ -62,14 +62,14 @@
&LaserHeadKinematics::updateTiltScan, this) ;
// Raw Stereo Updates
- stereo_pub_ = nh.advertise<image_msgs::RawStereo>("/stereo/raw_stereo_cal", 100) ;
+ stereo_pub_ = nh.advertise<sensor_msgs::RawStereo>("/stereo/raw_stereo_cal", 100) ;
stereo_sub_ = nh.subscribe("/stereo/raw_stereo", 100,
&LaserHeadKinematics::updateStereo, this) ;
}
- void updateStereo(const image_msgs::RawStereoConstPtr& msg_in)
+ void updateStereo(const sensor_msgs::RawStereoConstPtr& msg_in)
{
- image_msgs::RawStereo msg_out ;
+ sensor_msgs::RawStereo msg_out ;
msg_out = *msg_in ;
msg_out.header.frame_id = "stereo_optical_frame_cal" ;
msg_out.right_info.P[3] = PR_03_ ;
Modified: pkg/trunk/drivers/cam/camera_trigger_test/manifest...
[truncated message content] |
|
From: <is...@us...> - 2009-06-30 22:47:24
|
Revision: 18021
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18021&view=rev
Author: isucan
Date: 2009-06-30 22:47:21 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
moving mapping messages
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/manifest.xml
pkg/trunk/mapping/collision_map/src/collision_map.cpp
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/motion_planning/ompl_planning/manifest.xml
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_check_path.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/include/sbpl_door_planner_action/sbpl_door_planner_action.h
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/include/sbpl_planner_node/sbpl_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
pkg/trunk/stacks/semantic_mapping/door_handle_detector/include/door_handle_detector/doors_detector.h
pkg/trunk/stacks/semantic_mapping/door_handle_detector/include/door_handle_detector/handle_detector.h
pkg/trunk/stacks/semantic_mapping/door_handle_detector/manifest.xml
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/doors_detector.cpp
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector.cpp
pkg/trunk/stacks/visualization/rviz/manifest.xml
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/collision_map_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/collision_map_display.h
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/polygonal_map_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/polygonal_map_display.h
Added Paths:
-----------
pkg/trunk/stacks/common_msgs/mapping_msgs/
pkg/trunk/stacks/common_msgs/mapping_msgs/CMakeLists.txt
pkg/trunk/stacks/common_msgs/mapping_msgs/Makefile
pkg/trunk/stacks/common_msgs/mapping_msgs/mainpage.dox
pkg/trunk/stacks/common_msgs/mapping_msgs/manifest.xml
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/CollisionMap.msg
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/OrientedBoundingBox.msg
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/PolygonalMap.msg
Removed Paths:
-------------
pkg/trunk/stacks/common_msgs/robot_msgs/msg/CollisionMap.msg
pkg/trunk/stacks/common_msgs/robot_msgs/msg/OrientedBoundingBox.msg
pkg/trunk/stacks/common_msgs/robot_msgs/msg/PolygonalMap.msg
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-06-30 22:47:21 UTC (rev 18021)
@@ -16,6 +16,7 @@
<depend package="visualization_msgs" />
<depend package="tabletop_msgs" />
<depend package="tabletop_srvs" />
+ <depend package="mapping_msgs" />
<depend package="point_cloud_mapping" />
<depend package="robot_self_filter" />
</package>
Modified: pkg/trunk/mapping/collision_map/src/collision_map.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map.cpp 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/mapping/collision_map/src/collision_map.cpp 2009-06-30 22:47:21 UTC (rev 18021)
@@ -58,8 +58,8 @@
#include <boost/thread/mutex.hpp>
-#include <robot_msgs/OrientedBoundingBox.h>
-#include <robot_msgs/CollisionMap.h>
+#include <mapping_msgs/OrientedBoundingBox.h>
+#include <mapping_msgs/CollisionMap.h>
#include <tf/transform_listener.h>
#include <sys/time.h>
@@ -67,6 +67,7 @@
using namespace std;
using namespace std_msgs;
using namespace robot_msgs;
+using namespace mapping_msgs;
struct Leaf
{
Modified: pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-06-30 22:47:21 UTC (rev 18021)
@@ -54,8 +54,8 @@
#include <boost/thread/mutex.hpp>
-#include <robot_msgs/OrientedBoundingBox.h>
-#include <robot_msgs/CollisionMap.h>
+#include <mapping_msgs/OrientedBoundingBox.h>
+#include <mapping_msgs/CollisionMap.h>
#include <tabletop_srvs/RecordStaticMapTrigger.h>
#include <tabletop_srvs/SubtractObjectFromCollisionMap.h>
@@ -64,7 +64,7 @@
using namespace std;
using namespace robot_msgs;
-using namespace robot_msgs;
+using namespace mapping_msgs;
using namespace tabletop_srvs;
using namespace visualization_msgs;
Modified: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-06-30 22:47:21 UTC (rev 18021)
@@ -36,7 +36,7 @@
#include <ros/ros.h>
#include <robot_msgs/PointCloud.h>
-#include <robot_msgs/CollisionMap.h>
+#include <mapping_msgs/CollisionMap.h>
#include <tf/transform_listener.h>
#include <tf/message_notifier.h>
#include <robot_self_filter/self_mask.h>
@@ -57,10 +57,10 @@
loadParams();
// advertise our topics: full map and updates
- cmapPublisher_ = nh_.advertise<robot_msgs::CollisionMap>("collision_map_occ", 1);
- cmapUpdPublisher_ = nh_.advertise<robot_msgs::CollisionMap>("collision_map_occ_update", 1);
+ cmapPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ", 1);
+ cmapUpdPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_update", 1);
if (publishOcclusion_)
- occPublisher_ = nh_.advertise<robot_msgs::CollisionMap>("collision_map_occ_occlusion", 1);
+ occPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_occlusion", 1);
// create a message notifier (and enable subscription) for both the full map and for the updates
mnCloud_ = new tf::MessageNotifier<robot_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudCallback, this, _1), "cloud_in", "", 1);
@@ -719,14 +719,14 @@
void publishCollisionMap(const CMap &map, ros::Publisher &pub) const
{
- robot_msgs::CollisionMap cmap;
+ mapping_msgs::CollisionMap cmap;
cmap.header = header_;
const unsigned int ms = map.size();
for (CMap::const_iterator it = map.begin() ; it != map.end() ; ++it)
{
const CollisionPoint &cp = *it;
- robot_msgs::OrientedBoundingBox box;
+ mapping_msgs::OrientedBoundingBox box;
box.extents.x = box.extents.y = box.extents.z = bi_.resolution;
box.axis.x = box.axis.y = 0.0; box.axis.z = 1.0;
box.angle = 0.0;
Modified: pkg/trunk/motion_planning/ompl_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/manifest.xml 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/ompl_planning/manifest.xml 2009-06-30 22:47:21 UTC (rev 18021)
@@ -16,6 +16,8 @@
<depend package="visualization_msgs" />
<depend package="motion_planning_msgs" />
<depend package="motion_planning_srvs" />
+ <depend package="mapping_msgs" />
+ <depend package="tabletop_msgs" />
<depend package="planning_environment"/>
<depend package="ompl" />
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-30 22:47:21 UTC (rev 18021)
@@ -75,7 +75,7 @@
protected:
- void afterWorldUpdate(const robot_msgs::CollisionMapConstPtr &collisionMap)
+ void afterWorldUpdate(const mapping_msgs::CollisionMapConstPtr &collisionMap)
{
unsigned int n = collisionMap->get_boxes_size();
for (unsigned int i = 0 ; i < n ; ++i)
Modified: pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-30 22:47:21 UTC (rev 18021)
@@ -73,7 +73,7 @@
protected:
- void afterWorldUpdate(const robot_msgs::CollisionMapConstPtr &collisionMap)
+ void afterWorldUpdate(const mapping_msgs::CollisionMapConstPtr &collisionMap)
{
last_ = -1;
}
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-30 22:47:21 UTC (rev 18021)
@@ -41,7 +41,7 @@
#include "planning_environment/kinematic_model_state_monitor.h"
#include <tf/message_notifier.h>
-#include <robot_msgs/CollisionMap.h>
+#include <mapping_msgs/CollisionMap.h>
#include <tabletop_msgs/AttachedObject.h>
#include <boost/thread/mutex.hpp>
@@ -105,13 +105,13 @@
}
/** \brief Define a callback for before updating a map */
- void setOnBeforeMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
+ void setOnBeforeMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
{
onBeforeMapUpdate_ = callback;
}
/** \brief Define a callback for after updating a map */
- void setOnAfterMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
+ void setOnAfterMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
{
onAfterMapUpdate_ = callback;
}
@@ -144,25 +144,25 @@
protected:
void setupCSM(void);
- void updateCollisionSpace(const robot_msgs::CollisionMapConstPtr &collisionMap, bool clear);
- void collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap);
- void collisionMapUpdateCallback(const robot_msgs::CollisionMapConstPtr &collisionMap);
+ void updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear);
+ void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
+ void collisionMapUpdateCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
void attachObjectCallback(const tabletop_msgs::AttachedObjectConstPtr &attachedObject);
- CollisionModels *cm_;
- collision_space::EnvironmentModel *collisionSpace_;
- double boxScale_;
- boost::mutex mapUpdateLock_;
+ CollisionModels *cm_;
+ collision_space::EnvironmentModel *collisionSpace_;
+ double boxScale_;
+ boost::mutex mapUpdateLock_;
- bool haveMap_;
- ros::Time lastMapUpdate_;
- tf::MessageNotifier<robot_msgs::CollisionMap> *collisionMapNotifier_;
- tf::MessageNotifier<robot_msgs::CollisionMap> *collisionMapUpdateNotifier_;
- tf::MessageNotifier<tabletop_msgs::AttachedObject> *attachedBodyNotifier_;
+ bool haveMap_;
+ ros::Time lastMapUpdate_;
+ tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapNotifier_;
+ tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapUpdateNotifier_;
+ tf::MessageNotifier<tabletop_msgs::AttachedObject> *attachedBodyNotifier_;
- boost::function<void(const robot_msgs::CollisionMapConstPtr)> onBeforeMapUpdate_;
- boost::function<void(const robot_msgs::CollisionMapConstPtr)> onAfterMapUpdate_;
- boost::function<void(planning_models::KinematicModel::Link*)> onAfterAttachBody_;
+ boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onBeforeMapUpdate_;
+ boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onAfterMapUpdate_;
+ boost::function<void(planning_models::KinematicModel::Link*)> onAfterAttachBody_;
};
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-30 22:47:21 UTC (rev 18021)
@@ -18,6 +18,7 @@
<depend package="tabletop_msgs" />
<depend package="mechanism_msgs" />
<depend package="motion_planning_msgs" />
+ <depend package="mapping_msgs" />
<depend package="geometric_shapes" />
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-30 22:47:21 UTC (rev 18021)
@@ -89,10 +89,10 @@
ROS_INFO("Added static plane %fx + %fy + %fz + %f = 0", planeValues[i * 4], planeValues[i * 4 + 1], planeValues[i * 4 + 2], planeValues[i * 4 + 3]);
}
- collisionMapNotifier_ = new tf::MessageNotifier<robot_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1), "collision_map", getFrameId(), 1);
+ collisionMapNotifier_ = new tf::MessageNotifier<mapping_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1), "collision_map", getFrameId(), 1);
ROS_DEBUG("Listening to collision_map using message notifier with target frame %s", collisionMapNotifier_->getTargetFramesString().c_str());
- collisionMapUpdateNotifier_ = new tf::MessageNotifier<robot_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapUpdateCallback, this, _1), "collision_map_update", getFrameId(), 1);
+ collisionMapUpdateNotifier_ = new tf::MessageNotifier<mapping_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapUpdateCallback, this, _1), "collision_map_update", getFrameId(), 1);
ROS_DEBUG("Listening to collision_map_update using message notifier with target frame %s", collisionMapUpdateNotifier_->getTargetFramesString().c_str());
if (cm_->loadedModels())
@@ -127,18 +127,18 @@
ROS_INFO("Map received!");
}
-void planning_environment::CollisionSpaceMonitor::collisionMapUpdateCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
+void planning_environment::CollisionSpaceMonitor::collisionMapUpdateCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap)
{
if (collisionMap->boxes.size() > 0)
updateCollisionSpace(collisionMap, false);
}
-void planning_environment::CollisionSpaceMonitor::collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
+void planning_environment::CollisionSpaceMonitor::collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap)
{
updateCollisionSpace(collisionMap, true);
}
-void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const robot_msgs::CollisionMapConstPtr &collisionMap, bool clear)
+void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
{
boost::mutex::scoped_lock lock(mapUpdateLock_);
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_check_path.h
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_check_path.h 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_check_path.h 2009-06-30 22:47:21 UTC (rev 18021)
@@ -40,7 +40,7 @@
/** Messages needed for trajectory control and collision map**/
#include <robot_msgs/Pose.h>
#include <manipulation_msgs/JointTraj.h>
-#include <robot_msgs/CollisionMap.h>
+#include <mapping_msgs/CollisionMap.h>
#include <manipulation_msgs/JointTrajPoint.h>
/** sbpl planner include files **/
@@ -78,7 +78,7 @@
std::string node_name_;
- robot_msgs::CollisionMap collision_map_;
+ mapping_msgs::CollisionMap collision_map_;
MDPConfig mdp_cfg_;
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h 2009-06-30 22:47:21 UTC (rev 18021)
@@ -44,7 +44,7 @@
/** Messages needed for trajectory control and collision map**/
#include <robot_msgs/Pose.h>
#include <manipulation_msgs/JointTraj.h>
-#include <robot_msgs/CollisionMap.h>
+#include <mapping_msgs/CollisionMap.h>
#include <manipulation_msgs/JointTrajPoint.h>
#include <motion_planning_msgs/KinematicPath.h>
#include <motion_planning_msgs/KinematicState.h>
@@ -103,9 +103,9 @@
std::vector<std::string> joint_names_;
- robot_msgs::CollisionMap collision_map_;
+ mapping_msgs::CollisionMap collision_map_;
- robot_msgs::CollisionMap sbpl_collision_map_;
+ mapping_msgs::CollisionMap sbpl_collision_map_;
MDPConfig mdp_cfg_;
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml 2009-06-30 22:47:21 UTC (rev 18021)
@@ -8,6 +8,7 @@
<depend package="sbpl_arm_planner" />
<depend package="std_msgs"/>
<depend package="robot_msgs" />
+ <depend package="mapping_msgs" />
<depend package="manipulation_msgs" />
<depend package="motion_planning_msgs" />
<depend package="motion_planning_srvs" />
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp 2009-06-30 22:47:21 UTC (rev 18021)
@@ -65,7 +65,7 @@
// collision map
subscribe(collision_map_topic_, collision_map_, &SBPLArmPlannerNode::collisionMapCallback,1);
- advertise<robot_msgs::CollisionMap> ("sbpl_collision_map", 1);
+ advertise<mapping_msgs::CollisionMap> ("sbpl_collision_map", 1);
//initialize planner
planner_ = new ARAPlanner(&pr2_arm_env_, forward_search_);
Modified: pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/include/sbpl_door_planner_action/sbpl_door_planner_action.h
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/include/sbpl_door_planner_action/sbpl_door_planner_action.h 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/include/sbpl_door_planner_action/sbpl_door_planner_action.h 2009-06-30 22:47:21 UTC (rev 18021)
@@ -46,7 +46,7 @@
#include <robot_msgs/Pose.h>
#include <manipulation_msgs/JointTraj.h>
#include <manipulation_msgs/JointTrajPoint.h>
-#include <robot_msgs/CollisionMap.h>
+#include <mapping_msgs/CollisionMap.h>
// Costmap used for the map representation
#include <costmap_2d/costmap_2d_ros.h>
Modified: pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/manifest.xml 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/manifest.xml 2009-06-30 22:47:21 UTC (rev 18021)
@@ -19,6 +19,7 @@
<depend package="door_handle_detector"/>
<depend package="door_functions"/>
<depend package="door_msgs"/>
+ <depend package="mapping_msgs"/>
<depend package="pr2_ik"/>
<url></url>
Modified: pkg/trunk/motion_planning/planning_research/sbpl_planner_node/include/sbpl_planner_node/sbpl_planner_node.h
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_planner_node/include/sbpl_planner_node/sbpl_planner_node.h 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_research/sbpl_planner_node/include/sbpl_planner_node/sbpl_planner_node.h 2009-06-30 22:47:21 UTC (rev 18021)
@@ -45,7 +45,7 @@
#include <robot_msgs/Pose.h>
#include <manipulation_msgs/JointTraj.h>
#include <manipulation_msgs/JointTrajPoint.h>
-#include <robot_msgs/CollisionMap.h>
+#include <mapping_msgs/CollisionMap.h>
// Costmap used for the map representation
#include <costmap_2d/costmap_2d_ros.h>
Modified: pkg/trunk/motion_planning/planning_research/sbpl_planner_node/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_planner_node/manifest.xml 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/motion_planning/planning_research/sbpl_planner_node/manifest.xml 2009-06-30 22:47:21 UTC (rev 18021)
@@ -9,6 +9,7 @@
<depend package="rosconsole"/>
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
+ <depend package="mapping_msgs"/>
<depend package="costmap_2d"/>
<depend package="tf"/>
<depend package="mpglue"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-06-30 22:47:21 UTC (rev 18021)
@@ -11,6 +11,7 @@
<depend package="pr2_mechanism_controllers"/>
<depend package="tf"/>
<depend package="robot_msgs"/>
+ <depend package="mapping_msgs"/>
<depend package="mechanism_msgs" />
<depend package="manipulation_msgs" />
<depend package="mocap_msgs"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-06-30 22:47:15 UTC (rev 18020)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-06-30 22:47:21 UTC (rev 18021)
@@ -27,7 +27,7 @@
#define COLLISIONMAP_MOCAP_SYSTEM
#include "rossensorsystem.h"
-#include <robot_msgs/CollisionMap.h>
+#include <mapping_msgs/CollisionMap.h>
// used to update objects through a mocap system
class CollisionMapXMLID
@@ -36,16 +36,16 @@
static const char* GetXMLId() { return "collisionmap"; }
};
-class CollisionMapSystem : public ROSSensorSystem<robot_msgs::CollisionMap, CollisionMapXMLID>
+class CollisionMapSystem : public ROSSensorSystem<mapping_msgs::CollisionMap, CollisionMapXMLID>
{
public:
CollisionMapSystem(EnvironmentBase* penv)
- : ROSSensorSystem<robot_msgs::CollisionMap, CollisionMapXMLID>(penv), _robotid(0), _nNextId(1), _bEnableCollisions(true)
+ : ROSSensorSystem<mapping_msgs::CollisionMap, CollisionMapXMLID>(penv), _robotid(0), _nNextId(1), _bEnableCollisions(true)
{
}
virtual ~CollisionMapSystem() {
stopsubscriptions(); // need to stop the subscriptions because the virtual destructor will not call the overridden stopsubscriptions
- ROSSensorSystem<robot_msgs::CollisionMap, CollisionMapXMLID>::Destroy();
+ ROSSensorSystem<mapping_msgs::CollisionMap, CollisionMapXMLID>::Destroy();
}
virtual bool Init(istream& sinput)
@@ -88,7 +88,7 @@
private:
virtual void startsubscriptions()
{
- ROSSensorSystem<robot_msgs::CollisionMap, CollisionMapXMLID>::startsubscriptions();
+ ROSSensorSystem<mapping_msgs::CollisionMap, CollisionMapXMLID>::startsubscriptions();
if( _bSubscribed ) {
boost::mutex::scoped_lock lock(_mutex);
@@ -117,7 +117,7 @@
virtual void stopsubscriptions()
{
- ROSSensorSystem<robot_msgs::CollisionMap, CollisionMapXMLID>::stopsubscriptions();
+ ROSSensorSystem<mapping_msgs::CollisionMap, CollisionMapXMLID>::stopsubscriptions();
_tf.reset();
}
Added: pkg/trunk/stacks/common_msgs/mapping_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/common_msgs/mapping_msgs/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/common_msgs/mapping_msgs/CMakeLists.txt 2009-06-30 22:47:21 UTC (rev 18021)
@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+rospack(mapping_msgs)
+
+genmsg()
+gensrv()
Added: pkg/trunk/stacks/common_msgs/mapping_msgs/Makefile
===================================================================
--- pkg/trunk/stacks/common_msgs/mapping_msgs/Makefile (rev 0)
+++ pkg/trunk/stacks/common_msgs/mapping_msgs/Makefile 2009-06-30 22:47:21 UTC (rev 18021)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/stacks/common_msgs/mapping_msgs/mainpage.dox
===================================================================
--- pkg/trunk/stacks/common_msgs/mapping_msgs/mainpage.dox (rev 0)
+++ pkg/trunk/stacks/common_msgs/mapping_msgs/mainpage.dox 2009-06-30 22:47:21 UTC (rev 18021)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b mapping_msgs is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you ...
[truncated message content] |
|
From: <is...@us...> - 2009-06-30 23:45:05
|
Revision: 18033
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18033&view=rev
Author: isucan
Date: 2009-06-30 23:45:03 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
fixes for polygonal map
Modified Paths:
--------------
pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/annotated_map_lib.h
pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/binding.h
pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/projection.h
pkg/trunk/mapping/annotated_planar_patch_map/manifest.xml
pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_info.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_lib.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_pcd_via_service.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_planar_patch_map.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map_via_service.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/empty_annotated_map.cpp
pkg/trunk/mapping/annotated_planar_patch_map/src/image_value_node.cpp
pkg/trunk/mapping/door_stereo_detector/manifest.xml
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/mapping/door_tracker/manifest.xml
pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
pkg/trunk/mapping/hallway_tracker/manifest.xml
pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
pkg/trunk/mapping/planar_patch_map/manifest.xml
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp
pkg/trunk/mapping/stereo_convex_patch_histogram/manifest.xml
pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
pkg/trunk/mapping/table_object_detector/manifest.xml
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/stacks/semantic_mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h
pkg/trunk/stacks/semantic_mapping/plug_onbase_detector/manifest.xml
pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/semantic_point_annotator.cpp
Modified: pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/annotated_map_lib.h
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/annotated_map_lib.h 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/annotated_map_lib.h 2009-06-30 23:45:03 UTC (rev 18033)
@@ -49,7 +49,7 @@
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <annotated_map_msgs/TaggedPolygonalMap.h>
#include <annotated_map_msgs/TaggedPolygon3D.h>
@@ -63,8 +63,8 @@
//This will generate compile time error if you got no implementation.
void transformAnyObject(const std::string & target_frame,
const tf::Transform* net_transform,
- const robot_msgs::PolygonalMap & polymap_in,
- robot_msgs::PolygonalMap & polymap_out);
+ const mapping_msgs::PolygonalMap & polymap_in,
+ mapping_msgs::PolygonalMap & polymap_out);
void transformAnyObject(const std::string & target_frame,
Modified: pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/binding.h
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/binding.h 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/binding.h 2009-06-30 23:45:03 UTC (rev 18033)
@@ -49,7 +49,7 @@
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <annotated_map_msgs/TaggedPolygonalMap.h>
#include <annotated_map_msgs/TaggedPolygon3D.h>
Modified: pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/projection.h
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/projection.h 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/include/annotated_planar_patch_map/projection.h 2009-06-30 23:45:03 UTC (rev 18033)
@@ -49,7 +49,7 @@
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <sensor_msgs/StereoInfo.h>
#include <sensor_msgs/CamInfo.h>
#include <annotated_map_msgs/TaggedPolygonalMap.h>
@@ -72,7 +72,7 @@
-void projectPolygonalMap(const sensor_msgs::StereoInfo& stereo_info_, const robot_msgs::PolygonalMap& transformed_map_3D, robot_msgs::PolygonalMap &transformed_map_2D);
+void projectPolygonalMap(const sensor_msgs::StereoInfo& stereo_info_, const mapping_msgs::PolygonalMap& transformed_map_3D, mapping_msgs::PolygonalMap &transformed_map_2D);
void projectPolygonPoints(double* projection,double img_w, double img_h, robot_msgs::Polygon3D polyIn,robot_msgs::Polygon3D& polyOut);
void projectPolygonPointsNOP(double* projection,double img_w, double img_h, robot_msgs::Polygon3D polyIn,robot_msgs::Polygon3D& polyOut);
Modified: pkg/trunk/mapping/annotated_planar_patch_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/manifest.xml 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/manifest.xml 2009-06-30 23:45:03 UTC (rev 18033)
@@ -11,6 +11,7 @@
<depend package="roscpp" />
<depend package="std_msgs"/>
<depend package="robot_msgs" />
+ <depend package="mapping_msgs" />
<depend package="sensor_msgs" />
<depend package="point_cloud_mapping" />
<depend package="cv_mech_turk" />
Modified: pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_info.cpp
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_info.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_info.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -48,7 +48,7 @@
#include <cv.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <sensor_msgs/StereoInfo.h>
#include <cv_mech_turk/ExternalAnnotation.h>
Modified: pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_lib.cpp
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_lib.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/src/annotated_map_lib.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -45,8 +45,8 @@
void annotated_map_lib::transformAnyObject(const std::string & target_frame,
const tf::Transform* net_transform,
- const robot_msgs::PolygonalMap & polymapIn,
- robot_msgs::PolygonalMap & polymapOut)
+ const mapping_msgs::PolygonalMap & polymapIn,
+ mapping_msgs::PolygonalMap & polymapOut)
{
boost::numeric::ublas::matrix<double> transform = transformAsMatrix(*net_transform);
Modified: pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_pcd_via_service.cpp
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_pcd_via_service.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_pcd_via_service.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -52,7 +52,7 @@
#include <cv.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <sensor_msgs/StereoInfo.h>
#include <sensor_msgs/CamInfo.h>
#include "tf/message_notifier.h"
Modified: pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_planar_patch_map.cpp
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_planar_patch_map.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_planar_patch_map.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -52,7 +52,7 @@
#include <cv.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <sensor_msgs/StereoInfo.h>
#include <cv_mech_turk/ExternalAnnotation.h>
@@ -149,9 +149,9 @@
void liftAnnotation(cv_mech_turk::ExternalAnnotation annotation2d_object_,annotated_map_msgs::TaggedPolygonalMap& polymapOut)
{
- robot_msgs::PolygonalMap transformed_map_3D;
- robot_msgs::PolygonalMap transformed_map_3D_fixed_frame;
- robot_msgs::PolygonalMap transformed_map_2D;
+ mapping_msgs::PolygonalMap transformed_map_3D;
+ mapping_msgs::PolygonalMap transformed_map_3D_fixed_frame;
+ mapping_msgs::PolygonalMap transformed_map_2D;
//Get the 3D map into the coordinate frame of the camera
ROS_DEBUG("Transform 3D map to frame: %s",annotation2d_object_.reference_frame.c_str());
@@ -174,7 +174,7 @@
}
- void bindAnnotationsToMap(cv_mech_turk::ExternalAnnotation annotation2d_object, robot_msgs::PolygonalMap transformed_map_3D, robot_msgs::PolygonalMap transformed_map_2D,annotated_map_msgs::TaggedPolygonalMap &polymapOut)
+ void bindAnnotationsToMap(cv_mech_turk::ExternalAnnotation annotation2d_object, mapping_msgs::PolygonalMap transformed_map_3D, mapping_msgs::PolygonalMap transformed_map_2D,annotated_map_msgs::TaggedPolygonalMap &polymapOut)
{
//CvMemStorage* storage = cvCreateMemStorage();
@@ -326,7 +326,7 @@
- void printPolygon3D(robot_msgs::PolygonalMap transformed_map,std::string tag)
+ void printPolygon3D(mapping_msgs::PolygonalMap transformed_map,std::string tag)
{
std::string fname=std::string("/u/sorokin/bags/run_may_21/dump/polygons3D__")+tag+std::string(".txt");
@@ -357,7 +357,7 @@
tf::TransformListener *tf_;
cv_mech_turk::ExternalAnnotation annotation2d_object_;
- robot_msgs::PolygonalMap unlabeled_map_;
+ mapping_msgs::PolygonalMap unlabeled_map_;
sensor_msgs::StereoInfo stereo_info_;
std::string target_frame_;
Modified: pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map.cpp
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -52,7 +52,7 @@
#include <cv.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <sensor_msgs/StereoInfo.h>
#include <sensor_msgs/CamInfo.h>
@@ -353,7 +353,7 @@
- void printPolygon3D(robot_msgs::PolygonalMap transformed_map,std::string tag)
+ void printPolygon3D(mapping_msgs::PolygonalMap transformed_map,std::string tag)
{
std::string fname=std::string("/u/sorokin/bags/run_may_21/dump/polygons3D__")+tag+std::string(".txt");
Modified: pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map_via_service.cpp
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map_via_service.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/src/annotation2d_lifter_to_tagged_patch_map_via_service.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -52,7 +52,7 @@
#include <cv.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <sensor_msgs/StereoInfo.h>
#include <sensor_msgs/CamInfo.h>
#include "tf/message_notifier.h"
Modified: pkg/trunk/mapping/annotated_planar_patch_map/src/empty_annotated_map.cpp
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/src/empty_annotated_map.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/src/empty_annotated_map.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -49,7 +49,7 @@
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <sensor_msgs/StereoInfo.h>
#include <annotated_map_msgs/TaggedPolygonalMap.h>
@@ -84,20 +84,20 @@
if (fixed_frame_ == "ERROR_NO_NAME")
ROS_ERROR("Need to set parameter fixed_frame") ;
- sub_=node_handle_.subscribe<robot_msgs::PolygonalMap>("planar_map", 100, &EmptyAnnotatedMap::handleUnlabeledMap, this);
+ sub_=node_handle_.subscribe<mapping_msgs::PolygonalMap>("planar_map", 100, &EmptyAnnotatedMap::handleUnlabeledMap, this);
pub_=node_handle_.advertise<annotated_map_msgs::TaggedPolygonalMap>(out_topic_name_,1);
};
- void handleUnlabeledMap(const robot_msgs::PolygonalMapConstPtr& unlabeled_map)
+ void handleUnlabeledMap(const mapping_msgs::PolygonalMapConstPtr& unlabeled_map)
{
try
{
annotated_map_msgs::TaggedPolygonalMap polymapOut;
- robot_msgs::PolygonalMap transformed_map_3D;
+ mapping_msgs::PolygonalMap transformed_map_3D;
annotated_map_lib::transformAnyObject(fixed_frame_,tf_,*unlabeled_map,transformed_map_3D);
unsigned int num_polygons = transformed_map_3D.get_polygons_size();
Modified: pkg/trunk/mapping/annotated_planar_patch_map/src/image_value_node.cpp
===================================================================
--- pkg/trunk/mapping/annotated_planar_patch_map/src/image_value_node.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/annotated_planar_patch_map/src/image_value_node.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -48,7 +48,7 @@
#include <cv.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <sensor_msgs/CamInfo.h>
#include <cv_mech_turk/ExternalAnnotation.h>
Modified: pkg/trunk/mapping/door_stereo_detector/manifest.xml
===================================================================
--- pkg/trunk/mapping/door_stereo_detector/manifest.xml 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/door_stereo_detector/manifest.xml 2009-06-30 23:45:03 UTC (rev 18033)
@@ -11,6 +11,7 @@
<depend package="roscpp" />
<depend package="robot_msgs" />
<depend package="robot_srvs" />
+ <depend package="mapping_msgs" />
<depend package="kdl" />
<depend package="angles" />
<depend package="point_cloud_mapping" />
Modified: pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -46,7 +46,7 @@
// ROS messages
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <robot_msgs/Point32.h>
#include <visualization_msgs/Marker.h>
Modified: pkg/trunk/mapping/door_tracker/manifest.xml
===================================================================
--- pkg/trunk/mapping/door_tracker/manifest.xml 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/door_tracker/manifest.xml 2009-06-30 23:45:03 UTC (rev 18033)
@@ -18,6 +18,7 @@
<depend package="angles" />
<depend package="point_cloud_mapping" />
<depend package="std_msgs" />
+ <depend package="mapping_msgs" />
<depend package="door_handle_detector" />
<depend package="filters" />
<depend package="visualization_msgs" />
Modified: pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
===================================================================
--- pkg/trunk/mapping/door_tracker/src/door_tracker.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/door_tracker/src/door_tracker.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -48,7 +48,7 @@
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <robot_msgs/Point32.h>
#include <door_msgs/Door.h>
@@ -89,6 +89,7 @@
using namespace std;
using namespace robot_msgs;
+using namespace mapping_msgs;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Comparison operator for a vector of vectors
Modified: pkg/trunk/mapping/hallway_tracker/manifest.xml
===================================================================
--- pkg/trunk/mapping/hallway_tracker/manifest.xml 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/hallway_tracker/manifest.xml 2009-06-30 23:45:03 UTC (rev 18033)
@@ -11,6 +11,7 @@
<depend package="roscpp" />
<depend package="robot_msgs" />
<depend package="robot_srvs" />
+ <depend package="mapping_msgs" />
<depend package="kdl" />
<depend package="point_cloud_mapping" />
<depend package="std_msgs" />
Modified: pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
===================================================================
--- pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -49,7 +49,7 @@
// ROS messages
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <robot_msgs/Point32.h>
//#include <robot_msgs/Hallway.h>
@@ -86,6 +86,7 @@
using namespace std;
using namespace robot_msgs;
+using namespace mapping_msgs;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
Modified: pkg/trunk/mapping/planar_patch_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/planar_patch_map/manifest.xml 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/planar_patch_map/manifest.xml 2009-06-30 23:45:03 UTC (rev 18033)
@@ -10,5 +10,6 @@
<depend package="roscpp" />
<depend package="robot_msgs" />
+ <depend package="mapping_msgs" />
<depend package="point_cloud_mapping" />
</package>
Modified: pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp
===================================================================
--- pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -44,7 +44,7 @@
// ROS messages
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
// Sample Consensus
#include <point_cloud_mapping/sample_consensus/sac.h>
@@ -67,6 +67,7 @@
using namespace std;
using namespace robot_msgs;
+using namespace mapping_msgs;
class PlanarPatchMap
{
Modified: pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp
===================================================================
--- pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -44,7 +44,7 @@
// ROS messages
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
// Sample Consensus
#include <point_cloud_mapping/sample_consensus/sac.h>
@@ -65,6 +65,7 @@
using namespace std;
using namespace robot_msgs;
+using namespace mapping_msgs;
class PlanarPatchMap
{
Modified: pkg/trunk/mapping/stereo_convex_patch_histogram/manifest.xml
===================================================================
--- pkg/trunk/mapping/stereo_convex_patch_histogram/manifest.xml 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/stereo_convex_patch_histogram/manifest.xml 2009-06-30 23:45:03 UTC (rev 18033)
@@ -12,5 +12,6 @@
<depend package="roscpp" />
<depend package="robot_msgs" />
+ <depend package="mapping_msgs" />
<depend package="point_cloud_mapping" />
</package>
Modified: pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
===================================================================
--- pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -46,7 +46,7 @@
// ROS messages
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
// Sample Consensus
#include <point_cloud_mapping/sample_consensus/sac.h>
@@ -66,6 +66,7 @@
using namespace std;
using namespace robot_msgs;
+using namespace mapping_msgs;
class ConvexPatchHistogram
{
Modified: pkg/trunk/mapping/table_object_detector/manifest.xml
===================================================================
--- pkg/trunk/mapping/table_object_detector/manifest.xml 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/table_object_detector/manifest.xml 2009-06-30 23:45:03 UTC (rev 18033)
@@ -16,4 +16,5 @@
<depend package="robot_srvs" />
<depend package="tabletop_msgs" />
<depend package="tabletop_srvs" />
+ <depend package="mapping_msgs" />
</package>
Modified: pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
===================================================================
--- pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -44,7 +44,7 @@
// ROS messages
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
// Sample Consensus
#include <point_cloud_mapping/sample_consensus/sac.h>
@@ -77,6 +77,7 @@
using namespace std;
using namespace std_msgs;
using namespace robot_msgs;
+using namespace mapping_msgs;
using namespace tabletop_msgs;
using namespace tabletop_srvs;
Modified: pkg/trunk/stacks/semantic_mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h
===================================================================
--- pkg/trunk/stacks/semantic_mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/stacks/semantic_mapping/plug_onbase_detector/include/plug_onbase_detector/plug_onbase_detector.h 2009-06-30 23:45:03 UTC (rev 18033)
@@ -46,7 +46,7 @@
// ROS messages
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
#include <plugs_msgs/PlugStow.h>
// Sample Consensus
@@ -81,6 +81,7 @@
using namespace std;
using namespace std_msgs;
using namespace robot_msgs;
+using namespace mapping_msgs;
// Comparison operator for a vector of vectors
inline bool
Modified: pkg/trunk/stacks/semantic_mapping/plug_onbase_detector/manifest.xml
===================================================================
--- pkg/trunk/stacks/semantic_mapping/plug_onbase_detector/manifest.xml 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/stacks/semantic_mapping/plug_onbase_detector/manifest.xml 2009-06-30 23:45:03 UTC (rev 18033)
@@ -12,6 +12,7 @@
<depend package="std_msgs" />
<depend package="point_cloud_mapping" />
<depend package="robot_msgs" />
+ <depend package="mapping_msgs" />
<depend package="plugs_msgs"/>
<export>
<cpp cflags="-I${prefix}/msg/cpp -I${prefix}/include"/>
Modified: pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/semantic_point_annotator.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/semantic_point_annotator.cpp 2009-06-30 23:44:21 UTC (rev 18032)
+++ pkg/trunk/stacks/semantic_mapping/semantic_point_annotator/src/semantic_point_annotator.cpp 2009-06-30 23:45:03 UTC (rev 18033)
@@ -45,7 +45,7 @@
// ROS messages
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/PolygonalMap.h>
+#include <mapping_msgs/PolygonalMap.h>
// Sample Consensus
#include <point_cloud_mapping/sample_consensus/sac.h>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-07-01 01:08:07
|
Revision: 18045
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18045&view=rev
Author: rob_wheeler
Date: 2009-07-01 00:39:41 +0000 (Wed, 01 Jul 2009)
Log Message:
-----------
Mark old realtime_publisher constructor as deprecated and switch pr2_etherCAT to use the new constructor.
Modified Paths:
--------------
pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_publisher.h
Modified: pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp 2009-07-01 00:34:21 UTC (rev 18044)
+++ pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp 2009-07-01 00:39:41 UTC (rev 18045)
@@ -181,13 +181,13 @@
void *controlLoop(void *)
{
ros::NodeHandle node;
- realtime_tools::RealtimePublisher<diagnostic_msgs::DiagnosticMessage> publisher("/diagnostics", 2);
+ realtime_tools::RealtimePublisher<diagnostic_msgs::DiagnosticMessage> publisher(node, "/diagnostics", 2);
realtime_tools::RealtimePublisher<pr2_etherCAT::RealtimeStats> *rtpublisher = 0;
accumulator_set<double, stats<tag::max, tag::mean> > acc;
if (g_options.stats)
{
- rtpublisher = new realtime_tools::RealtimePublisher<pr2_etherCAT::RealtimeStats> ("/realtime", 2);
+ rtpublisher = new realtime_tools::RealtimePublisher<pr2_etherCAT::RealtimeStats> (node, "/realtime", 2);
}
// Initialize the hardware interface
Modified: pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_publisher.h
===================================================================
--- pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_publisher.h 2009-07-01 00:34:21 UTC (rev 18044)
+++ pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_publisher.h 2009-07-01 00:39:41 UTC (rev 18045)
@@ -70,7 +70,7 @@
public:
// Deprecated
- RealtimePublisher(const std::string &topic, int queue_size)
+ __attribute__((deprecated)) RealtimePublisher(const std::string &topic, int queue_size)
: topic_(topic), is_running_(false), keep_running_(false), turn_(REALTIME)
{
construct(queue_size);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-07-01 01:08:09
|
Revision: 18046
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18046&view=rev
Author: rob_wheeler
Date: 2009-07-01 00:40:18 +0000 (Wed, 01 Jul 2009)
Log Message:
-----------
Delete empty directories left after tree restructuring.
Removed Paths:
-------------
pkg/trunk/drivers/imu/
pkg/trunk/drivers/motor/
pkg/trunk/highlevel/executive_trex/
pkg/trunk/robot_descriptions/pr2/pr2_gazebo/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-07-01 01:24:13
|
Revision: 18059
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18059&view=rev
Author: vijaypradeep
Date: 2009-07-01 01:24:04 +0000 (Wed, 01 Jul 2009)
Log Message:
-----------
LaserScannerTrajController now publishes laser_scanner_signal on custom periodic trajectories
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/util/trajectory/include/trajectory/trajectory.h
pkg/trunk/util/trajectory/src/trajectory.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-07-01 01:23:31 UTC (rev 18058)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-07-01 01:24:04 UTC (rev 18059)
@@ -86,6 +86,8 @@
inline double getCurProfileTime() ;
//! \brief Returns the length (in seconds) of our current profile
inline double getProfileDuration() ;
+ //! \brief Returns the current trajectory segment we're executing in our current profile
+ inline int getCurProfileSegment() ;
private:
@@ -149,7 +151,7 @@
mechanism::RobotState *robot_ ;
std::string service_prefix_ ;
- double prev_profile_time_ ; //!< The time in the current profile when update() was last called
+ int prev_profile_segment_ ; //!< The segment in the current profile when update() was last called
pr2_msgs::PeriodicCmd cmd_ ;
pr2_msgs::LaserTrajCmd traj_cmd_ ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-07-01 01:23:31 UTC (rev 18058)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-07-01 01:24:04 UTC (rev 18059)
@@ -228,6 +228,12 @@
return traj_duration_ ;
}
+int LaserScannerTrajController::getCurProfileSegment()
+{
+ double cur_time = getCurProfileTime() ;
+ return traj_.findTrajectorySegment(cur_time) ;
+}
+
bool LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
{
while (!traj_lock_.try_lock())
@@ -404,6 +410,7 @@
ROS_REGISTER_CONTROLLER(LaserScannerTrajControllerNode)
LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::Node::instance()), c_()
{
+ prev_profile_segment_ = -1 ;
need_to_send_msg_ = false ; // Haven't completed a sweep yet, so don't need to send a msg
publisher_ = NULL ; // We don't know our topic yet, so we can't build it
}
@@ -428,29 +435,20 @@
{
c_.update() ;
- double cur_profile_time = c_.getCurProfileTime() ;
+ int cur_profile_segment = c_.getCurProfileSegment() ;
- // Check if we crossed the middle point of our profile
- if (cur_profile_time >= c_.getProfileDuration()/2.0 &&
- prev_profile_time_ < c_.getProfileDuration()/2.0)
+ if (cur_profile_segment != prev_profile_segment_)
{
// Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
ros::Time cur_time ;
cur_time.fromSec(robot_->hw_->current_time_) ;
m_scanner_signal_.header.stamp = cur_time ;
- m_scanner_signal_.signal = 0 ;
+ m_scanner_signal_.signal = cur_profile_segment ;
need_to_send_msg_ = true ;
}
- else if (cur_profile_time < prev_profile_time_) // Check if we wrapped around
- {
- ros::Time cur_time ;
- cur_time.fromSec(robot_->hw_->current_time_) ;
- m_scanner_signal_.header.stamp = cur_time ;
- m_scanner_signal_.signal = 1 ;
- need_to_send_msg_ = true ;
- }
- prev_profile_time_ = cur_profile_time ;
+ prev_profile_segment_ = cur_profile_segment ;
+
// Use the realtime_publisher to try to send the message.
// If it fails sending, it's not a big deal, since we can just try again 1 ms later. No one will notice.
if (need_to_send_msg_)
@@ -462,8 +460,6 @@
publisher_->unlockAndPublish() ;
need_to_send_msg_ = false ;
}
- //printf("tilt_laser: Signal trigger (%u)\n", m_scanner_signal_.signal) ;
- //std::cout << std::flush ;
}
}
@@ -491,7 +487,7 @@
delete publisher_ ;
publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
- prev_profile_time_ = 0.0 ;
+ prev_profile_segment_ = -1 ;
ROS_INFO("Successfully spawned %s", service_prefix_.c_str()) ;
@@ -508,6 +504,7 @@
else
{
res.start_time = ros::Time::now();
+ prev_profile_segment_ = -1 ;
return true;
}
}
@@ -515,6 +512,7 @@
void LaserScannerTrajControllerNode::setPeriodicCmd()
{
c_.setPeriodicCmd(cmd_) ;
+ prev_profile_segment_ = -1 ;
}
bool LaserScannerTrajControllerNode::setTrajSrv(pr2_srvs::SetLaserTrajCmd::Request &req,
@@ -527,6 +525,7 @@
else
{
res.start_time = ros::Time::now();
+ prev_profile_segment_ = -1 ;
return true;
}
}
@@ -535,6 +534,7 @@
void LaserScannerTrajControllerNode::setTrajCmd()
{
c_.setTrajCmd(traj_cmd_) ;
+ prev_profile_segment_ = -1 ;
}
/*void LaserScannerTrajControllerNode::setTrackLinkCmd()
Modified: pkg/trunk/util/trajectory/include/trajectory/trajectory.h
===================================================================
--- pkg/trunk/util/trajectory/include/trajectory/trajectory.h 2009-07-01 01:23:31 UTC (rev 18058)
+++ pkg/trunk/util/trajectory/include/trajectory/trajectory.h 2009-07-01 01:24:04 UTC (rev 18059)
@@ -211,6 +211,14 @@
void setJointWraps(int index);
+ /*!
+ \brief finds the trajectory segment corresponding to a particular time
+ \param input time (in seconds)
+ \return segment index
+ */
+ int findTrajectorySegment(double time);
+
+
protected:
std::string interp_method_; /** string representation of interpolation method */
@@ -320,13 +328,6 @@
*/
void sampleBlendedLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
- /*!
- \brief finds the trajectory segment corresponding to a particular time
- \param input time (in seconds)
- \return segment index
- */
- inline int findTrajectorySegment(double time);
-
double calculateMinTimeCubic(double q0, double q1, double v0, double v1, double vmax, int index);
/*!
Modified: pkg/trunk/util/trajectory/src/trajectory.cpp
===================================================================
--- pkg/trunk/util/trajectory/src/trajectory.cpp 2009-07-01 01:23:31 UTC (rev 18058)
+++ pkg/trunk/util/trajectory/src/trajectory.cpp 2009-07-01 01:24:04 UTC (rev 18059)
@@ -258,7 +258,7 @@
parameterize();
}
-inline int Trajectory::findTrajectorySegment(double time)
+int Trajectory::findTrajectorySegment(double time)
{
int result = 0;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-01 05:56:07
|
Revision: 18086
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18086&view=rev
Author: isucan
Date: 2009-07-01 05:55:45 +0000 (Wed, 01 Jul 2009)
Log Message:
-----------
some documentation
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/mainpage.dox
pkg/trunk/motion_planning/ompl_planning/manifest.xml
pkg/trunk/motion_planning/planning_environment/mainpage.dox
pkg/trunk/motion_planning/planning_models/mainpage.dox
pkg/trunk/util/geometric_shapes/mainpage.dox
pkg/trunk/world_models/collision_space/mainpage.dox
Modified: pkg/trunk/motion_planning/ompl_planning/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/mainpage.dox 2009-07-01 05:44:40 UTC (rev 18085)
+++ pkg/trunk/motion_planning/ompl_planning/mainpage.dox 2009-07-01 05:55:45 UTC (rev 18086)
@@ -2,43 +2,43 @@
\mainpage
\htmlinclude manifest.html
-@b ompl_planning is a node capable of planning kinematic paths for
-a set of robot models. Each robot model is a complete model specified
-in URDF or consists of an URDF group.
+@section summary Summary
+@b ompl_planning is a node capable of planning kinematic paths for a
+set of robot models. Each robot model is a group of links/joints from a
+URDF file, not necessarily the complete robot model. The available
+models (groups of links) are defined as described in the
+<a href="../../planning_environment/html">planning_environment</a> package.
+
Organization:
- - there are multiple models (a model is a group of links/joints we plan for)
- - there are multiple planners that can be used for each model
- - there are multiple types of planning requests
-The code is mostly implemented in the included RKP* files (ROS
-Kinematic Planning). There exists one basic class (RKPRequestHandler)
-that can handle different requests.
+ - there are multiple models (defined by @b group_list as in <a href="../../planning_environment/html">planning_environment</a>)
-A model is defined for each loaded URDF model, and for each of the
-URDF groups marked for planning. This model includes a kinematic
-model, a collision space (shared between models) and a set of
-planners. If a planner is used for different models, it is
-instantiated each time. Since planners may require different
-setup/configuration code, there exists a base class that defines the
-functionality and an inherited class for each type of planner that can
-be instantiated. The planners are associated to string names:
-kinematic::RRT, kinematic::LazyRRT, kinematic::EST, kinematic::SBL,
-kinematic::IKSBL. These string names can be used for the planner_id
-component of the planning request. If no planner_id is specified, an
-appropriate planner is automatically selected. This is in fact the
-prefered use.
+ - there are multiple planners that can be used for each model (defined by @b planner_configs as in <a href="../../planning_environment/html">planning_environment</a>)
+
+For each model, a kinematic structure (as in <a
+href="../../planning_models/html">planning_models</a>), a collision
+space (as in <a href="../../collision_space/html">collision_space</a>)
+and a set of planners (from <a href="../../ompl/html">ompl</a>) are
+maintained. The collision space is shared between models. If a
+planner is used for different models, it is instantiated each
+time.
+
+The planners are associated to string names: kinematic::RRT,
+kinematic::LazyRRT, kinematic::EST, kinematic::SBL, kinematic::IKSBL,
+kinematic::KPIECE, dynamic::RRT. These string names can be used for
+the @b planner_id component of the planning request. If no @b
+planner_id is specified, an appropriate planner is automatically
+selected. This is in fact the preferred use.
+
When checking states for validity, a resolution at which paths are
check needs to be defined. To make things easier for the user, this
-parameter is computed by default by the SpaceInformationRKPModel
-class. The current settings work fine for the PR2, but if another
-robot is to be used, different settings man need to be used.
+parameter is computed by default by the
+ompl_planning::SpaceInformationKinematicModel class. The current
+settings work fine for the PR2, but if another robot is to be used,
+different settings may need to be employed.
-\todo
-- Find a better way to specify resolution for state validity
-checking.
-
<!--
\section codeapi Code API
-->
@@ -71,10 +71,10 @@
\subsubsection topics ROS topics
Subscribes to:
-- only topics PlanningMonitor from planning_environment package subscribes to
+- only topics planning_environment::PlanningMonitor from <a href="../../planning_environment/html">planning_environment</a> package subscribes to
\subsubsection parameters ROS parameters
-- "~planning_frame_id"/string : if the default frame is not to be used when planning, this parameter allows changing that frame.
+- @b "~planning_frame_id"/string : if the default frame is not to be used when planning, this parameter allows changing that frame.
\subsubsection services ROS services
@@ -94,4 +94,4 @@
<!-- END: copy for each node -->
-*/
\ No newline at end of file
+*/
Modified: pkg/trunk/motion_planning/ompl_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/manifest.xml 2009-07-01 05:44:40 UTC (rev 18085)
+++ pkg/trunk/motion_planning/ompl_planning/manifest.xml 2009-07-01 05:55:45 UTC (rev 18086)
@@ -1,5 +1,5 @@
<package>
- <description>Kinematic motion planning using OMPL</description>
+ <description>Sampling-based motion planning using OMPL</description>
<author>Ioan A. Sucan</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-07-01 05:44:40 UTC (rev 18085)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-07-01 05:55:45 UTC (rev 18086)
@@ -2,142 +2,204 @@
\mainpage
\htmlinclude manifest.html
+@section summary Summary
+
\b planning_environment is a library that allows users to instantiate
robot models and collision models based on data from the parameter
server with minimal user input. Additionally, state information for
both robot models and collision environments can be monitored.
-<!--
-In addition to providing an overview of your package,
-this is the section where the specification and design/architecture
-should be detailed. While the original specification may be done on the
-wiki, it should be transferred here once your package starts to take shape.
-You can then link to this documentation page from the Wiki.
--->
+@section conventions Conventions
+A URDF robot description is assumed to be loaded on the parameter
+server. The name of the parameter under which this description exists
+is taken as an argument to class constructors. Additionally, .yaml
+files describing the collision and planning information should be
+loaded on the parameter server under the same prefix as the robot
+description.
-\section codeapi Code API
+The collision.yaml file should define the follwing:
-The intended use for this package is to instantiante one of the two
-model classes and potentially one of the monitor classes.
+ - the robot links (same names as in the URDF description) that should be considered for collision checking under @b collision_links
-The model classes are:
-- \b RobotModels : allows the instantiation of various robot models (for example, a kinematic one) based on data from the parameter server. The URDF robot description and the .yaml files describing collision and planning information are assumed to be loaded.
+ - the self collision groups under @b self_collision_groups
-- \b CollisionModels : allows the instantiation of various robot models (for example, a kinematic one) and various collision spaces, based on data from the parameter server. This class inherits from \b RobotModels. The URDF robot description and the .yaml files describing collision and planning information are assumed to be loaded.
+ - the set of robot links that the robot can perceive with its sensors @b self_see ; (scaling and padding for these links is set with @b self_see_scale, @b self_see_padd)
-<hr>
-The monitor classes are:
-- \b KinematicModelStateMonitor : monitors the kinematic state of the robot. Optionally, monitors the base location. It uses the 'mechanism_state' topic.
-- \b CollisionSpaceMonitor : same as \b KinematicModelStateMonitor except it also monitors the state of the collision environment. It uses the 'collision_map' topic to receive new full maps and the 'collision_map_update' to receive updates. Attaching objects to robot links is possible using the 'attach_object' topic. The '~box_scale' parameter is used as the constant that gets multiplied to a box's maximum extent to obtain the radius of the sphere used in collision checking.
-- \b PlanningMonitor : same as \b CollisionSpaceMonitor except it also offers the ability to evaluate kinematic constraints and check paths and states for validity.
+An example collision.yaml file:
- <!-- Provide links to specific auto-generated API
-documentation within your package that is of particular interest to a
-reader. Doxygen will document pretty much every part of your code, so
-do your best here to point the reader to the actual API.
+@verbatim
-If your codebase is fairly large or has different sets of APIs, you
-should use the doxygen 'group' tag to keep these APIs together. For
-example, the roscpp documentation has 'libros' group.
--->
+## links for which collision checking with the environment should be performed
+collision_links:
+ base_link
+ torso_lift_link
+ head_pan_link
+ head_tilt_link
+ laser_tilt_mount_link
+ base_laser
+ ...
-\section rosapi ROS API
+## self collision is performed between groups of links; the names for the groups can be anything, but they must contain 'a' and 'b' as subgroups
+self_collision_groups: scg_r scg_l
-<!--
-Names are very important in ROS because they can be remapped on the
-command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
-APPEAR IN THE CODE. You should list names of every topic, service and
-parameter used in your code. There is a template below that you can
-use to document each node separately.
+## -- for right arm; self-collision if any link in 'a' collides with some link in 'b'
+scg_r:
+ a: r_forearm_link r_gripper_palm_link r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
-List of nodes:
-- \b node_name1
-- \b node_name2
--->
+## -- for left arm; self-collision if any link in 'a' collides with some link in 'b'
+scg_l:
+ a: l_forearm_link l_gripper_palm_link l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
-<!-- START: copy from here to 'END' for each node
+## list of links that the robot can see with its sensors (used to remove
+## parts of the robot from scanned data)
+self_see:
+ r_upper_arm_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_link
+ r_forearm_roll_link
+ ...
-<hr>
+## The padding to be added for the body parts the robot can see
+self_see_padd: 0.1
-\subsection node_name node_name
+## The scaling to be considered for the body parts the robot can see
+self_see_scale: 1.0
-node_name does (provide a basic description of your node)
+@endverbatim
-\subsubsection Usage
-\verbatim
-$ node_type1 [standard ROS args]
-\endverbatim
-\par Example
+The planning.yaml file defines the groups of links/joints motion planning can be performed for and contains the following:
-\verbatim
-$ node_type1
-\endverbatim
--->
+ - the names of the groups (the model ID's) planning is done for, under @b group_list. This is important as motion planners will use these names as input.
-\subsubsection topics ROS topics
+ - the definition of each of the groups is under @b groups. Each definition contains
+ - @b links : the names of the robot links to plan for; the actuated joints are the ones that connect the links with their parents, as described in the URDF
+ - @b planner_configs : these are names of configurations for planners; each planner configuration contains a planner-specific set of parameters. The only parameter that must exist is 'type', which specified the planner to be used.
-Subscribes to:
-- @b "mechanism_state"/MechanismState : the parameters describing the robot's current state
-- @b "collision_map"/CollisionMap : data describing the 3D environment
-- @b "collision_map_update"/CollisionMap : updates to data describing the 3D environment
-- @b "attach_object"/AttachedObject : data describing an object to be attached to a link
+ - the content of each planning configuration under @ b planner_configs
-\subsubsection parameters ROS parameters
-Reads the following parameters from the parameter server
+An example planning.yaml file:
-Reads the following parameters from the parameter server
-- @b "~refresh_interval_collision_map"/double : if more than this interval passes, the maintained map is considered invalid
+@verbatim
-- @b "~refresh_interval_kinematic_state"/double : if more than this interval passes, the maintained kinematic state is considered invalid
+## the list of groups for which motion planning can be performed
+group_list:
+ base
+ left_arm
+ ...
-- @b "~bounding_planes"/string : a sequence of plane equations specified as "a1 b1 c1 d1 a2 b2 c2 d2 ..." where each plane is defined by the equation ax+by+cz+d=0
+## the definition of each group
+groups:
-- @b "~box_scale"/double : boxes from the collision map are approximated with spheres using the extents of the boxes. The maximum extent of the box is multiplied by the constant specified by this parameter to obtain the radius of the sphere approximating the box
+ base:
+ links:
+ base_link
+ planner_configs:
+ RRTkConfig1 RRTdConfig1 SBLkConfig1 KPIECEkConfig1
-A robot description and its corresponding planning and collision descriptions are assumed to be loaded on the parameter server as well.
+ left_arm:
+ links:
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ l_upper_arm_roll_link
+ l_upper_arm_link
+ l_elbow_flex_link
+ l_forearm_roll_link
+ l_forearm_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ planner_configs:
+ SBLkConfig2 LBKPIECEkConfig2l
-Sets the following parameters on the parameter server
+ ...
-- Sets the parameters it reads to default values
+## the planner configurations; each config must have a type, which specifies
+## the planner to be used; other parameters can be specified as well, depending
+## on the planner
-<!--
-\subsubsection services ROS services
-- \b "foo_service": [std_srvs/FooType] description of foo_service
+planner_configs:
+
+ RRTkConfig1:
+ type: kinematic::RRT
+ range: 0.75
+ RRTdConfig1:
+ type: dynamic::RRT
+ range: 0.75
-END: copy for each node -->
+ SBLkConfig1:
+ type: kinematic::SBL
+ projection: 0 1
+ celldim: 1 1
+ range: 0.5
+ KPIECEkConfig1:
+ type: kinematic::KPIECE
+ projection: 0 1
+ celldim: 1 1
+ range: 0.5
-<!-- START: Uncomment if you have any command-line tools
+ SBLkConfig2:
+ type: kinematic::SBL
+ projection: 5 6
+ celldim: 0.1 0.1
-\section commandline Command-line tools
+ LBKPIECEkConfig2l:
+ type: kinematic::LBKPIECE
+ projection: link l_wrist_roll_link
+ celldim: 0.1 0.1 0.1
-This section is a catch-all for any additional tools that your package
-provides or uses that may be of use to the reader. For example:
+@endverbatim
-- tools/scripts (e.g. rospack, roscd)
-- roslaunch .launch files
-- xmlparam files
+\section codeapi Code API
-\subsection script_name script_name
+The intended use for this package is to instantiante one of the two
+model classes and potentially one of the monitor classes.
-Description of what this script/file does.
+The model classes are:
+- \b planning_environment::RobotModels : allows the instantiation of various robot models (for example, a kinematic one) based on data from the parameter server. The URDF robot description and the .yaml files describing collision and planning information are assumed to be loaded.
-\subsubsection Usage
-\verbatim
-$ ./script_name [args]
-\endverbatim
+- \b planning_environment::CollisionModels : allows the instantiation of various robot models (for example, a kinematic one) and various collision spaces, based on data from the parameter server. This class inherits from \b planning_environment::RobotModels. The URDF robot description and the .yaml files describing collision and planning information are assumed to be loaded.
-\par Example
+<hr>
-\verbatim
-$ ./script_name foo bar
-\endverbatim
+The monitor classes are:
+- \b planning_environment::KinematicModelStateMonitor : monitors the kinematic state of the robot. Optionally, monitors the base location. It uses the 'mechanism_state' topic.
+- \b planning_environment::CollisionSpaceMonitor : same as \b planning_environment::KinematicModelStateMonitor except it also monitors the state of the collision environment. It uses the 'collision_map' topic to receive new full maps and the 'collision_map_update' to receive updates. Attaching objects to robot links is possible using the 'attach_object' topic. The '~box_scale' parameter is used as the constant that gets multiplied to a box's maximum extent to obtain the radius of the sphere used in collision checking.
+- \b planning_environment::PlanningMonitor : same as \b planning_environment::CollisionSpaceMonitor except it also offers the ability to evaluate kinematic constraints and check paths and states for validity.
-END: Command-Line Tools Section -->
+\section rosapi ROS API
-*/
\ No newline at end of file
+\subsection topics ROS topics
+
+Subscribes to:
+ - @b "mechanism_state"/MechanismState : the parameters describing the robot's current state
+ - @b "collision_map"/CollisionMap : data describing the 3D environment
+ - @b "collision_map_update"/CollisionMap : updates (additive) to data describing the 3D environment
+ - @b "attach_object"/AttachedObject : data describing an object to be attached to a link
+
+\subsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+ - @b "~refresh_interval_collision_map"/double : if more than this interval passes, the maintained map is considered invalid
+
+ - @b "~refresh_interval_kinematic_state"/double : if more than this interval passes, the maintained kinematic state is considered invalid
+
+ - @b "~bounding_planes"/string : a sequence of plane equations specified as "a1 b1 c1 d1 a2 b2 c2 d2 ..." where each plane is defined by the equation ax+by+cz+d=0
+
+ - @b "~box_scale"/double : boxes from the collision map are approximated with spheres using the extents of the boxes. The maximum extent of the box is multiplied by the constant specified by this parameter to obtain the radius of the sphere approximating the box
+
+A robot description and its corresponding planning and collision descriptions are assumed to be loaded on the parameter server as well.
+
+Sets the following parameters on the parameter server
+
+ - Sets the parameters it reads to default values
+
+
+*/
Modified: pkg/trunk/motion_planning/planning_models/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_models/mainpage.dox 2009-07-01 05:44:40 UTC (rev 18085)
+++ pkg/trunk/motion_planning/planning_models/mainpage.dox 2009-07-01 05:55:45 UTC (rev 18086)
@@ -2,31 +2,15 @@
\mainpage
\htmlinclude manifest.html
-\b planning_models is used for describing a kinematic robot model loaded from URDF. Visual geometry is ignored (only collision geometry is considered). This package allows performing forward kinematics for various groups of joints, for potentially multiple robots. The states for different groups of joints can be easily extractes using the KinematicModel class. The StateParams class allows easy updating of various state values by using the joint names specified in URDF.
+@section summary Summary
-<!--
-In addition to providing an overview of your package,
-this is the section where the specification and design/architecture
-should be detailed. While the original specification may be done on the
-wiki, it should be transferred here once your package starts to take shape.
-You can then link to this documentation page from the Wiki.
--->
+\b planning_models is used for describing a kinematic robot model loaded from URDF. Visual geometry is ignored (only collision geometry is considered). This package allows performing forward kinematics for various groups of joints, for potentially multiple robots. The states for different groups of joints can be easily extractes using the planning_models::KinematicModel class. The planning_models::StateParams class allows easy updating of various state values by using the joint names specified in URDF.
+
\section codeapi Code API
-- see the KinematicModel class
-- see the StateParams class
+- see the planning_models::KinematicModel class
+- see the planning_models::StateParams class
-<!--
-Provide links to specific auto-generated API documentation within your
-package that is of particular interest to a reader. Doxygen will
-document pretty much every part of your code, so do your best here to
-point the reader to the actual API.
-If your codebase is fairly large or has different sets of APIs, you
-should use the doxygen 'group' tag to keep these APIs together. For
-example, the roscpp documentation has 'libros' group.
--->
-
-
*/
\ No newline at end of file
Modified: pkg/trunk/util/geometric_shapes/mainpage.dox
===================================================================
--- pkg/trunk/util/geometric_shapes/mainpage.dox 2009-07-01 05:44:40 UTC (rev 18085)
+++ pkg/trunk/util/geometric_shapes/mainpage.dox 2009-07-01 05:55:45 UTC (rev 18086)
@@ -2,6 +2,8 @@
\mainpage
\htmlinclude manifest.html
+@section summary Summary
+
\b geometric_shapes is a package containing the definition of simple shapes such as spheres, boxes, cylinders and meshes. These shapes can be converted into bodies and routines like tests for point inclusion or volume computation are also available.
<!--
Modified: pkg/trunk/world_models/collision_space/mainpage.dox
===================================================================
--- pkg/trunk/world_models/collision_space/mainpage.dox 2009-07-01 05:44:40 UTC (rev 18085)
+++ pkg/trunk/world_models/collision_space/mainpage.dox 2009-07-01 05:55:45 UTC (rev 18086)
@@ -2,27 +2,16 @@
\mainpage
\htmlinclude manifest.html
-\b collision_space is used to represent collisions between a given robot model and the environment. Self collision checking is performed and contact positions and normals can be computed.
+@section summary Summary
-<!--
-In addition to providing an overview of your package,
-this is the section where the specification and design/architecture
-should be detailed. While the original specification may be done on the
-wiki, it should be transferred here once your package starts to take shape.
-You can then link to this documentation page from the Wiki.
--->
+\b collision_space is used to represent collisions between a given
+robot model (as in <a href="../../planning_models/html">planning_models</a>) and the
+environment. Self collision checking is performed and contact
+positions and normals can be computed.
-<!--
-Provide links to specific auto-generated API documentation within your
-package that is of particular interest to a reader. Doxygen will
-document pretty much every part of your code, so do your best here to
-point the reader to the actual API.
+An abstract interface (collision_space::EnvironmentModel) is provided
+to a set of collision checking libraries.
-If your codebase is fairly large or has different sets of APIs, you
-should use the doxygen 'group' tag to keep these APIs together. For
-example, the roscpp documentation has 'libros' group.
--->
-
*/
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-01 20:20:01
|
Revision: 18122
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18122&view=rev
Author: isucan
Date: 2009-07-01 20:19:58 +0000 (Wed, 01 Jul 2009)
Log Message:
-----------
renaming service name from KinematicPlan to MotionPlan
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/RequestHandler.h
pkg/trunk/motion_planning/ompl_planning/mainpage.dox
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp
pkg/trunk/sandbox/chomp_motion_planner/include/chomp_motion_planner/chomp_planner_node.h
pkg/trunk/sandbox/chomp_motion_planner/src/chomp_planner_node.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/motion_planning_srvs/srv/MotionPlan.srv
Removed Paths:
-------------
pkg/trunk/motion_planning/motion_planning_srvs/srv/KinematicPlan.srv
Deleted: pkg/trunk/motion_planning/motion_planning_srvs/srv/KinematicPlan.srv
===================================================================
--- pkg/trunk/motion_planning/motion_planning_srvs/srv/KinematicPlan.srv 2009-07-01 20:13:15 UTC (rev 18121)
+++ pkg/trunk/motion_planning/motion_planning_srvs/srv/KinematicPlan.srv 2009-07-01 20:19:58 UTC (rev 18122)
@@ -1,38 +0,0 @@
-# This service contains the definition for a request to the motion
-# planner and the output it provides
-
-# Parameters for the state space
-motion_planning_msgs/KinematicSpaceParameters params
-
-# Starting state updates. If certain joints should be considered
-# at positions other than the current ones, these positions should
-# be set here
-motion_planning_msgs/KinematicJoint[] start_state
-
-# The goal state for the model to plan for. The goal is achieved
-# if all constraints are satisfied
-motion_planning_msgs/KinematicConstraints goal_constraints
-
-# No state in the produced motion plan will violate these constraints
-motion_planning_msgs/KinematicConstraints path_constraints
-
-# The number of times this plan is to be computed. Shortest solution
-# will be reported.
-int32 times
-
-# The maximum amount of time the motion planner is allowed to plan for
-float64 allowed_time
-
----
-
-# A solution path, if found
-motion_planning_msgs/KinematicPath path
-
-# distance to goal as computed by the heuristic
-float64 distance
-
-# set to 1 if the computed path was approximate
-byte approximate
-
-# if state information was not up to date when planning, this is set to 1
-byte unsafe
Copied: pkg/trunk/motion_planning/motion_planning_srvs/srv/MotionPlan.srv (from rev 18088, pkg/trunk/motion_planning/motion_planning_srvs/srv/KinematicPlan.srv)
===================================================================
--- pkg/trunk/motion_planning/motion_planning_srvs/srv/MotionPlan.srv (rev 0)
+++ pkg/trunk/motion_planning/motion_planning_srvs/srv/MotionPlan.srv 2009-07-01 20:19:58 UTC (rev 18122)
@@ -0,0 +1,38 @@
+# This service contains the definition for a request to the motion
+# planner and the output it provides
+
+# Parameters for the state space
+motion_planning_msgs/KinematicSpaceParameters params
+
+# Starting state updates. If certain joints should be considered
+# at positions other than the current ones, these positions should
+# be set here
+motion_planning_msgs/KinematicJoint[] start_state
+
+# The goal state for the model to plan for. The goal is achieved
+# if all constraints are satisfied
+motion_planning_msgs/KinematicConstraints goal_constraints
+
+# No state in the produced motion plan will violate these constraints
+motion_planning_msgs/KinematicConstraints path_constraints
+
+# The number of times this plan is to be computed. Shortest solution
+# will be reported.
+int32 times
+
+# The maximum amount of time the motion planner is allowed to plan for
+float64 allowed_time
+
+---
+
+# A solution path, if found
+motion_planning_msgs/KinematicPath path
+
+# distance to goal as computed by the heuristic
+float64 distance
+
+# set to 1 if the computed path was approximate
+byte approximate
+
+# if state information was not up to date when planning, this is set to 1
+byte unsafe
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/RequestHandler.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/RequestHandler.h 2009-07-01 20:13:15 UTC (rev 18121)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/RequestHandler.h 2009-07-01 20:19:58 UTC (rev 18122)
@@ -39,7 +39,7 @@
#include "ompl_planning/Model.h"
#include <ros/ros.h>
-#include <motion_planning_srvs/KinematicPlan.h>
+#include <motion_planning_srvs/MotionPlan.h>
/** \brief Main namespace */
namespace ompl_planning
@@ -81,10 +81,10 @@
void disableDebugMode(void);
/** \brief Check if the request is valid */
- bool isRequestValid(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req);
+ bool isRequestValid(ModelMap &models, motion_planning_srvs::MotionPlan::Request &req);
/** \brief Check and compute a motion plan. Return true if the plan was succesfully computed */
- bool computePlan(ModelMap &models, const planning_models::StateParams *start, motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
+ bool computePlan(ModelMap &models, const planning_models::StateParams *start, motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res);
private:
@@ -98,13 +98,13 @@
/** \brief Set up all the data needed by motion planning based on a request and lock the planner setup
* using this data */
- void configure(const planning_models::StateParams *startState, motion_planning_srvs::KinematicPlan::Request &req, PlannerSetup *psetup);
+ void configure(const planning_models::StateParams *startState, motion_planning_srvs::MotionPlan::Request &req, PlannerSetup *psetup);
/** \brief Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
bool callPlanner(PlannerSetup *psetup, int times, double allowed_time, Solution &sol);
/** \brief Fill the response with solution data */
- void fillResult(PlannerSetup *psetup, const planning_models::StateParams *start, motion_planning_srvs::KinematicPlan::Response &res, const Solution &sol);
+ void fillResult(PlannerSetup *psetup, const planning_models::StateParams *start, motion_planning_srvs::MotionPlan::Response &res, const Solution &sol);
/** \brief Send visualization markers */
void display(PlannerSetup *psetup);
Modified: pkg/trunk/motion_planning/ompl_planning/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/mainpage.dox 2009-07-01 20:13:15 UTC (rev 18121)
+++ pkg/trunk/motion_planning/ompl_planning/mainpage.dox 2009-07-01 20:19:58 UTC (rev 18122)
@@ -78,7 +78,7 @@
\subsubsection services ROS services
-- @b "plan_kinematic_path"/KinematicPlan : given a robot model, starting and goal states,
+- @b "plan_kinematic_path"/MotionPlan : given a robot model, starting and goal states,
this service computes a collision free path.
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-01 20:13:15 UTC (rev 18121)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-01 20:19:58 UTC (rev 18122)
@@ -39,7 +39,7 @@
#include <ros/console.h>
#include <sstream>
-bool ompl_planning::RequestHandler::isRequestValid(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req)
+bool ompl_planning::RequestHandler::isRequestValid(ModelMap &models, motion_planning_srvs::MotionPlan::Request &req)
{
ModelMap::const_iterator pos = models.find(req.params.model_id);
@@ -114,7 +114,7 @@
return true;
}
-void ompl_planning::RequestHandler::configure(const planning_models::StateParams *startState, motion_planning_srvs::KinematicPlan::Request &req, PlannerSetup *psetup)
+void ompl_planning::RequestHandler::configure(const planning_models::StateParams *startState, motion_planning_srvs::MotionPlan::Request &req, PlannerSetup *psetup)
{
/* clear memory */
psetup->si->clearGoal();
@@ -194,7 +194,7 @@
}
bool ompl_planning::RequestHandler::computePlan(ModelMap &models, const planning_models::StateParams *start,
- motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
+ motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res)
{
if (!isRequestValid(models, req))
return false;
@@ -236,7 +236,7 @@
return true;
}
-void ompl_planning::RequestHandler::fillResult(PlannerSetup *psetup, const planning_models::StateParams *start, motion_planning_srvs::KinematicPlan::Response &res, const Solution &sol)
+void ompl_planning::RequestHandler::fillResult(PlannerSetup *psetup, const planning_models::StateParams *start, motion_planning_srvs::MotionPlan::Response &res, const Solution &sol)
{
std::vector<planning_models::KinematicModel::Joint*> joints;
psetup->model->kmodel->getJoints(joints);
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-01 20:13:15 UTC (rev 18121)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-01 20:19:58 UTC (rev 18122)
@@ -128,7 +128,7 @@
return NULL;
}
- bool planToGoal(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
+ bool planToGoal(motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res)
{
ROS_INFO("Received request for planning");
bool st = false;
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h 2009-07-01 20:13:15 UTC (rev 18121)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h 2009-07-01 20:19:58 UTC (rev 18122)
@@ -55,7 +55,7 @@
/** services **/
#include <sbpl_arm_planner_node/PlanPathSrv.h>
-#include <motion_planning_srvs/KinematicPlan.h>
+#include <motion_planning_srvs/MotionPlan.h>
namespace sbpl_arm_planner_node
{
@@ -70,7 +70,7 @@
~SBPLArmPlannerNode();
- bool planKinematicPath(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
+ bool planKinematicPath(motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res);
private:
@@ -125,8 +125,8 @@
bool setGoalState(const std::vector<motion_planning_msgs::JointConstraint> &joint_constraint);
- bool planToState(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
- bool planToPosition(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
+ bool planToState(motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res);
+ bool planToPosition(motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res);
bool plan(motion_planning_msgs::KinematicPath &arm_path);
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp 2009-07-01 20:13:15 UTC (rev 18121)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp 2009-07-01 20:19:58 UTC (rev 18122)
@@ -346,7 +346,7 @@
}
/** plan to joint space goal */
-bool SBPLArmPlannerNode::planToState(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
+bool SBPLArmPlannerNode::planToState(motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res)
{
motion_planning_msgs::KinematicPath arm_path;
@@ -406,7 +406,7 @@
}
/** plan to cartesian goal(s) */
-bool SBPLArmPlannerNode::planToPosition(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
+bool SBPLArmPlannerNode::planToPosition(motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res)
{
// for(unsigned int i=0; i < req.get_start_state_size(); i++)
// {
@@ -510,7 +510,7 @@
}
/** call back function */
-bool SBPLArmPlannerNode::planKinematicPath(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
+bool SBPLArmPlannerNode::planKinematicPath(motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res)
{
ROS_INFO("Callback called\n");
Modified: pkg/trunk/sandbox/chomp_motion_planner/include/chomp_motion_planner/chomp_planner_node.h
===================================================================
--- pkg/trunk/sandbox/chomp_motion_planner/include/chomp_motion_planner/chomp_planner_node.h 2009-07-01 20:13:15 UTC (rev 18121)
+++ pkg/trunk/sandbox/chomp_motion_planner/include/chomp_motion_planner/chomp_planner_node.h 2009-07-01 20:19:58 UTC (rev 18122)
@@ -38,7 +38,7 @@
#define CHOMP_PLANNER_NODE_H_
#include <ros/ros.h>
-#include <motion_planning_srvs/KinematicPlan.h>
+#include <motion_planning_srvs/MotionPlan.h>
#include <chomp_motion_planner/chomp_robot_model.h>
namespace chomp
@@ -77,7 +77,7 @@
/**
* \brief Main entry point for motion planning (callback for the plan_kinematic_path service)
*/
- bool planKinematicPath(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
+ bool planKinematicPath(motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res);
private:
ros::NodeHandle node_handle_; /**< ROS Node handle */
Modified: pkg/trunk/sandbox/chomp_motion_planner/src/chomp_planner_node.cpp
===================================================================
--- pkg/trunk/sandbox/chomp_motion_planner/src/chomp_planner_node.cpp 2009-07-01 20:13:15 UTC (rev 18121)
+++ pkg/trunk/sandbox/chomp_motion_planner/src/chomp_planner_node.cpp 2009-07-01 20:19:58 UTC (rev 18122)
@@ -83,7 +83,7 @@
return 0;
}
-bool ChompPlannerNode::planKinematicPath(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
+bool ChompPlannerNode::planKinematicPath(motion_planning_srvs::MotionPlan::Request &req, motion_planning_srvs::MotionPlan::Response &res)
{
// get the planning group:
const ChompRobotModel::ChompPlanningGroup* group = chomp_robot_model_.getPlanningGroup(req.params.model_id);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-07-01 22:08:57
|
Revision: 18138
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18138&view=rev
Author: hsujohnhsu
Date: 2009-07-01 22:08:10 +0000 (Wed, 01 Jul 2009)
Log Message:
-----------
* clean up gazebo package, remove unused files. added launch scripts for worlds.
* clean up gazebo demos.
* fix corresponding gazebo tests.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.launch
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/arm_gazebo/l_arm.launch
pkg/trunk/demos/arm_gazebo/l_gripper.launch
pkg/trunk/demos/arm_gazebo/r_arm.launch
pkg/trunk/demos/arm_gazebo/r_gripper.launch
pkg/trunk/demos/examples_gazebo/dual_link.launch
pkg/trunk/demos/examples_gazebo/multi_link.launch
pkg/trunk/demos/examples_gazebo/single_link.launch
pkg/trunk/demos/examples_gazebo/table.launch
pkg/trunk/demos/pr2_gazebo/cups.launch
pkg/trunk/demos/pr2_gazebo/head_cart_simple.launch
pkg/trunk/demos/pr2_gazebo/pr2_balcony.launch
pkg/trunk/demos/pr2_gazebo/pr2_cups.launch
pkg/trunk/demos/pr2_gazebo/pr2_empty.launch
pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch
pkg/trunk/demos/pr2_gazebo/pr2_simple.launch
pkg/trunk/demos/pr2_gazebo/pr2_simple_office.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
pkg/trunk/demos/test_2dnav_gazebo/CMakeLists.txt
pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/CMakeLists.txt
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/CMakeLists.txt
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/CMakeLists.txt
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml
pkg/trunk/stacks/simulators/gazebo/Makefile
pkg/trunk/stacks/simulators/gazebo/gazebo_new_patch.diff
Added Paths:
-----------
pkg/trunk/demos/pr2_gazebo/prototype1_default_controllers.launch
pkg/trunk/demos/pr2_gazebo/prototype1_empty.launch
pkg/trunk/demos/pr2_gazebo/prototype1_simple.launch
pkg/trunk/demos/pr2_gazebo/prototype1_tables.launch
pkg/trunk/demos/pr2_gazebo/prototype1_wg.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_amcl_axis.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_axis.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_diagonal.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_odom.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_rotation.launch
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_wg.launch
pkg/trunk/drivers/simulator/test_gazebo_plugin/test_pendulum.launch
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/my_hztest.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.launch
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_image.launch
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_scan.launch
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.launch
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/camera.world
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/scan.world
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/slide.world
pkg/trunk/stacks/simulators/gazebo/launch/
pkg/trunk/stacks/simulators/gazebo/launch/balcony_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/empty_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/empty_world_no_x.launch
pkg/trunk/stacks/simulators/gazebo/launch/office_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/simple_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/slide_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/wg_world.launch
Removed Paths:
-------------
pkg/trunk/demos/examples_gazebo/dual_link_no_x.launch
pkg/trunk/demos/pr2_prototype1_gazebo/
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_amcl_axis.xml
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_axis.xml
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_diagonal.xml
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_odom.xml
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_empty_rotation.xml
pkg/trunk/demos/test_2dnav_gazebo/test_2dnav_wg.xml
pkg/trunk/drivers/simulator/test_gazebo_plugin/test_pendulum.xml
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/my_hztest.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_image.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_scan.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/
pkg/trunk/stacks/simulators/gazebo/empty_world.launch
pkg/trunk/stacks/simulators/gazebo/gazebo_patch.diff
pkg/trunk/stacks/simulators/gazebo/jointforce.diff
pkg/trunk/stacks/simulators/gazebo/patches/
pkg/trunk/stacks/simulators/gazebo/test_geom_mass.diff
pkg/trunk/stacks/simulators/gazebo/test_scons_patch.diff
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,7 +1,7 @@
<launch>
<!-- start up robot -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_simple.launch"/>
+ <include file="$(find pr2_gazebo)/prototype1_simple.launch"/>
<!-- load map -->
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map3.png 0.1" respawn="true" machine="three" />
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,7 +1,7 @@
<launch>
<!-- start up robot -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_wg.launch"/>
+ <include file="$(find pr2_gazebo)/prototype1_wg.launch"/>
<!-- load map -->
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/willowMap.png 0.1" respawn="true" machine="three" />
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-07-01 22:08:10 UTC (rev 18138)
@@ -17,7 +17,6 @@
<depend package="trex_pr2"/>
<depend package="robot_pose_ekf"/>
<depend package="pr2_gazebo"/>
- <depend package="pr2_prototype1_gazebo"/>
<depend package="point_cloud_assembler"/>
<depend package="semantic_point_annotator"/>
<depend package="or_robot_self_filter"/>
Modified: pkg/trunk/demos/arm_gazebo/l_arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/arm_gazebo/l_arm.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -2,17 +2,11 @@
<!--
Run a PR2 left arm in simulation
-->
- <param name="/use_sim_time" value="true" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
+
<!-- send pr2_l_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_arm.xacro.xml'" />
- <!-- -g flag runs gazebo in gui-less mode -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
Modified: pkg/trunk/demos/arm_gazebo/l_gripper.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_gripper.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/arm_gazebo/l_gripper.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -2,17 +2,11 @@
<!--
Run a PR2 left arm in simulation
-->
- <param name="/use_sim_time" value="true" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
+
<!-- send pr2_l_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_gripper.xacro.xml'" />
- <!-- -g flag runs gazebo in gui-less mode -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
Modified: pkg/trunk/demos/arm_gazebo/r_arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/arm_gazebo/r_arm.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -2,17 +2,11 @@
<!--
Run a PR2 right arm in simulation
-->
- <param name="/use_sim_time" value="true" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
+
<!-- send pr2_r_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
- <!-- -g flag runs gazebo in gui-less mode -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
Modified: pkg/trunk/demos/arm_gazebo/r_gripper.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_gripper.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/arm_gazebo/r_gripper.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -2,17 +2,11 @@
<!--
Run a PR2 left arm in simulation
-->
- <param name="/use_sim_time" value="true" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
+
<!-- send pr2_r_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_gripper.xacro.xml'" />
- <!-- -g flag runs gazebo in gui-less mode -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
Modified: pkg/trunk/demos/examples_gazebo/dual_link.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/examples_gazebo/dual_link.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,15 +1,9 @@
<launch>
- <group name="wg">
- <!-- send dual_link.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/dual_link_defs/dual_link.xml'" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
- </group>
+ <!-- send dual_link.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/dual_link_defs/dual_link.xml'" />
+
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
Deleted: pkg/trunk/demos/examples_gazebo/dual_link_no_x.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link_no_x.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/examples_gazebo/dual_link_no_x.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,19 +0,0 @@
-<launch>
- <group name="wg">
- <!-- send dual_link.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/dual_link_defs/dual_link.xml'" />
-
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
- </group>
- <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
-
- <!--node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/dual_link_defs/controllers_dual_link.xml" respawn="false" output="screen" /--> <!-- load default arm controller -->
- <!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
-</launch>
-
Modified: pkg/trunk/demos/examples_gazebo/multi_link.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/examples_gazebo/multi_link.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,15 +1,9 @@
<launch>
- <group name="wg">
- <!-- send multi_link.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/multi_link_defs/multi_link.xml'" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
- </group>
+ <!-- send multi_link.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/multi_link_defs/multi_link.xml'" />
+
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
Modified: pkg/trunk/demos/examples_gazebo/single_link.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/examples_gazebo/single_link.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,15 +1,9 @@
<launch>
- <group name="wg">
- <!-- send single_link.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/single_link_defs/single_link.xml'" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
- </group>
+ <!-- send single_link.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/single_link_defs/single_link.xml'" />
+
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
Modified: pkg/trunk/demos/examples_gazebo/table.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/table.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/examples_gazebo/table.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,15 +1,8 @@
<launch>
- <group name="wg">
- <!-- send table.xml to param server -->
- <param name="robotdesc/table" command="$(find xacro)/xacro.py '$(find examples_gazebo)/table_defs/table.xml'" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
- </group>
+ <!-- send table.xml to param server -->
+ <param name="robotdesc/table" command="$(find xacro)/xacro.py '$(find examples_gazebo)/table_defs/table.xml'" />
<!-- push robotdesc/table to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/table" respawn="false" output="screen" /> <!-- load default arm controller -->
Modified: pkg/trunk/demos/pr2_gazebo/cups.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/cups.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/pr2_gazebo/cups.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,18 +1,11 @@
<launch>
- <param name="/use_sim_time" value="true" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
<!-- send pr2.xml to param server -->
<param name="ikea_cup" textfile="$(find gazebo_robot_description)/gazebo_objects/ikea_cup.model" />
<param name="wine" textfile="$(find gazebo_robot_description)/gazebo_objects/000.580.67.model" />
<param name="bowl" textfile="$(find gazebo_robot_description)/gazebo_objects/001.327.79.model" />
<param name="coffee_cup" textfile="$(find gazebo_robot_description)/gazebo_objects/coffee_cup.model" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="xml2factory" args="ikea_cup 0 -.8 0 0 0 0 ikea_cup" respawn="false" output="screen" />
<node pkg="gazebo_plugin" type="xml2factory" args="wine 1 0.0 0 0 0 0 wine" respawn="false" output="screen" />
Modified: pkg/trunk/demos/pr2_gazebo/head_cart_simple.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/head_cart_simple.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/pr2_gazebo/head_cart_simple.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,15 +1,9 @@
<launch>
- <param name="/use_sim_time" value="true" />
+ <include file="$(find gazebo)/launch/simple_world.launch" />
+
<!-- send head_cart.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hca.xacro.xml'" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_balcony.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_balcony.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/pr2_gazebo/pr2_balcony.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,14 +1,9 @@
<launch>
+ <include file="$(find gazebo)/launch/balcony_world.launch" />
+
<!-- send urdf to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prototype1.xacro.xml'" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/balcony.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 5 5 0 0 0 0" respawn="false" output="screen" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_cups.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_cups.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/pr2_gazebo/pr2_cups.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,5 +1,6 @@
<launch>
- <param name="/use_sim_time" value="true" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
+
<!-- send pr2.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
<param name="ikea_cup" textfile="$(find gazebo_robot_description)/gazebo_objects/ikea_cup.model" />
@@ -7,13 +8,6 @@
<param name="bowl" textfile="$(find gazebo_robot_description)/gazebo_objects/001.327.79.model" />
<param name="coffee_cup" textfile="$(find gazebo_robot_description)/gazebo_objects/coffee_cup.model" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -2 0 0 0 0" respawn="false" output="screen" />
<node pkg="gazebo_plugin" type="xml2factory" args="ikea_cup 0 -.8 0 0 0 0 ikea_cup" respawn="false" output="screen" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_empty.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_empty.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/pr2_gazebo/pr2_empty.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,15 +1,9 @@
<launch>
- <param name="/use_sim_time" value="true" />
+ <include file="$(find gazebo)/launch/empty_world.launch" />
+
<!-- send pr2.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch 2009-07-01 21:38:39 UTC (rev 18137)
+++ pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch 2009-07-01 22:08:10 UTC (rev 18138)
@@ -1,15 +1,9 @@
<launch>
- <param name="/use_sim_time" value="true" />
+ <include file="$(find gazebo)/launch/empty_world_no_x.launch" />
+
<!-- send pr2.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
- <!-- start gazebo -->
- <node pkg="gazebo" launch-prefix="xvfb-run" type="gazebo" args="-u $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_simple.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_simple.lau...
[truncated message content] |
|
From: <hsu...@us...> - 2009-07-01 23:22:27
|
Revision: 18139
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18139&view=rev
Author: hsujohnhsu
Date: 2009-07-01 22:09:50 +0000 (Wed, 01 Jul 2009)
Log Message:
-----------
some fixes on launch scripts for trex. untested.
Modified Paths:
--------------
pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_tables.xml
pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_wg.xml
pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_tables.xml
pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_wg_no_x.xml
pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_tables.xml
pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_wg.xml
pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_tables.xml
Removed Paths:
-------------
pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_tables_no_x.xml
pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_tables_no_x.xml
pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_tables_no_x.xml
pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_tables_no_x.xml
pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_wg_no_x.xml
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_tables.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_tables.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_tables.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -2,7 +2,7 @@
<master auto="start"/>
<group name="wg">
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_tables.launch"/>
+ <include file="$(find pr2_gazebo)/prototype1_tables.launch"/>
<!-- Common parameter settings /-->
<param name="/trex/ping_frequency" value="1"/>
Deleted: pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_tables_no_x.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_tables_no_x.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_tables_no_x.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -1,28 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
-
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_tables_no_x.launch"/>
-
- <!-- Common parameter settings /-->
- <param name="/trex/ping_frequency" value="1"/>
-
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map_blank.png 0.1" respawn="false" />
-
- <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
-
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="" respawn="false" />
- <node pkg="highlevel_controllers" type="recharge_controller" args="" respawn="false" />
-
- <!--
- <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2"/>
- <include file="$(find world_3d_map)/run.xml"/>
- <node pkg="highlevel_controllers" type="move_arm" args="right" respawn="false"/>
- <node pkg="highlevel_controllers" type="move_arm" args="left" respawn="false" />
- <node pkg="highlevel_controllers" type="move_end_effector" args="right" respawn="false"/>
- -->
-
- <!--node pkg="nav_view" type="nav_view" respawn="false" /-->
-
- </group>
-</launch>
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_wg.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_wg.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.0/launch_gazebo_prototype1_wg.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -2,7 +2,7 @@
<master auto="start"/>
<group name="wg">
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_wg.launch"/>
+ <include file="$(find pr2_gazebo)/prototype1_wg.launch"/>
<!-- Common parameter settings /-->
<param name="/trex/ping_frequency" value="1"/>
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_tables.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_tables.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_tables.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -2,7 +2,7 @@
<master auto="start"/>
<group name="wg">
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_tables.launch"/>
+ <include file="$(find pr2_gazebo)/prototype1_tables.launch"/>
<!-- Common parameter settings /-->
<param name="/trex/ping_frequency" value="1"/>
Deleted: pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_tables_no_x.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_tables_no_x.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_tables_no_x.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -1,28 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
-
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_tables_no_x.launch"/>
-
- <!-- Common parameter settings /-->
- <param name="/trex/ping_frequency" value="1"/>
-
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map_blank.png 0.1" respawn="false" />
-
- <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
-
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="" respawn="false" />
- <node pkg="highlevel_controllers" type="recharge_controller" args="" respawn="false" />
-
- <!--
- <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2"/>
- <include file="$(find world_3d_map)/run.xml"/>
- <node pkg="highlevel_controllers" type="move_arm" args="right" respawn="false"/>
- <node pkg="highlevel_controllers" type="move_arm" args="left" respawn="false" />
- <node pkg="highlevel_controllers" type="move_end_effector" args="right" respawn="false"/>
- -->
-
- <!--node pkg="nav_view" type="nav_view" respawn="false" /-->
-
- </group>
-</launch>
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_wg_no_x.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_wg_no_x.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/wpc_gazebo.1/launch_gazebo_prototype1_wg_no_x.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -2,7 +2,7 @@
<master auto="start"/>
<group name="wg">
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_wg_no_x.launch"/>
+ <include file="$(find pr2_gazebo)/prototype1_tables.launch"/>
<!-- Common parameter settings /-->
<param name="/trex/ping_frequency" value="1"/>
Modified: pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_tables.xml
===================================================================
--- pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_tables.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_tables.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -2,7 +2,7 @@
<master auto="start"/>
<group name="wg">
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_tables.launch"/>
+ <include file="$(find pr2_gazebo)/prototype1_tables.launch"/>
<!-- Common parameter settings /-->
<param name="/trex/ping_frequency" value="1"/>
Deleted: pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_tables_no_x.xml
===================================================================
--- pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_tables_no_x.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_tables_no_x.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -1,28 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
-
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_tables_no_x.launch"/>
-
- <!-- Common parameter settings /-->
- <param name="/trex/ping_frequency" value="1"/>
-
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map_blank.png 0.1" respawn="false" />
-
- <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
-
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="" respawn="false" />
- <node pkg="highlevel_controllers" type="recharge_controller" args="" respawn="false" />
-
- <!--
- <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2"/>
- <include file="$(find world_3d_map)/run.xml"/>
- <node pkg="highlevel_controllers" type="move_arm" args="right" respawn="false"/>
- <node pkg="highlevel_controllers" type="move_arm" args="left" respawn="false" />
- <node pkg="highlevel_controllers" type="move_end_effector" args="right" respawn="false"/>
- -->
-
- <!--node pkg="nav_view" type="nav_view" respawn="false" /-->
-
- </group>
-</launch>
Modified: pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_wg.xml
===================================================================
--- pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_wg.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.0/launch_gazebo_prototype1_wg.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -2,7 +2,7 @@
<master auto="start"/>
<group name="wg">
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_wg.launch"/>
+ <include file="$(find pr2_gazebo)/prototype1_tables.launch"/>
<!-- Common parameter settings /-->
<param name="/trex/ping_frequency" value="1"/>
Modified: pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_tables.xml
===================================================================
--- pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_tables.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_tables.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -2,7 +2,7 @@
<master auto="start"/>
<group name="wg">
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_tables.launch"/>
+ <include file="$(find pr2_gazebo)/prototype1_tables.launch"/>
<!-- Common parameter settings /-->
<param name="/trex/ping_frequency" value="1"/>
Deleted: pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_tables_no_x.xml
===================================================================
--- pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_tables_no_x.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_tables_no_x.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -1,28 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
-
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_tables_no_x.launch"/>
-
- <!-- Common parameter settings /-->
- <param name="/trex/ping_frequency" value="1"/>
-
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map_blank.png 0.1" respawn="false" />
-
- <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
-
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="" respawn="false" />
- <node pkg="highlevel_controllers" type="recharge_controller" args="" respawn="false" />
-
- <!--
- <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2"/>
- <include file="$(find world_3d_map)/run.xml"/>
- <node pkg="highlevel_controllers" type="move_arm" args="right" respawn="false"/>
- <node pkg="highlevel_controllers" type="move_arm" args="left" respawn="false" />
- <node pkg="highlevel_controllers" type="move_end_effector" args="right" respawn="false"/>
- -->
-
- <!--node pkg="nav_view" type="nav_view" respawn="false" /-->
-
- </group>
-</launch>
Deleted: pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_wg_no_x.xml
===================================================================
--- pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_wg_no_x.xml 2009-07-01 22:08:10 UTC (rev 18138)
+++ pkg/trunk/stacks/trex/trex_ros/deprecated/wpc_gazebo.1/launch_gazebo_prototype1_wg_no_x.xml 2009-07-01 22:09:50 UTC (rev 18139)
@@ -1,32 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
-
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_wg_no_x.launch"/>
-
- <!-- Common parameter settings /-->
- <param name="/trex/ping_frequency" value="1"/>
-
- <!-- blank map fails on timeout -->
- <!--
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map_blank.png 0.1" respawn="false" />
- -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" />
-
- <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
-
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="" respawn="false" />
- <node pkg="highlevel_controllers" type="recharge_controller" args="" respawn="false" />
-
- <!--
- <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2"/>
- <include file="$(find world_3d_map)/run.xml"/>
- <node pkg="highlevel_controllers" type="move_arm" args="right" respawn="false"/>
- <node pkg="highlevel_controllers" type="move_arm" args="left" respawn="false" />
- <node pkg="highlevel_controllers" type="move_end_effector" args="right" respawn="false"/>
- -->
-
- <!--node pkg="nav_view" type="nav_view" respawn="false" /-->
-
- </group>
-</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-07-01 23:45:00
|
Revision: 18149
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18149&view=rev
Author: jfaustwg
Date: 2009-07-01 23:44:00 +0000 (Wed, 01 Jul 2009)
Log Message:
-----------
Move tf::MessageFilter into tf
Modified Paths:
--------------
pkg/trunk/sandbox/message_filters/CMakeLists.txt
pkg/trunk/sandbox/message_filters/include/message_filters/consumer.h
pkg/trunk/sandbox/message_filters/manifest.xml
pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.h
Added Paths:
-----------
pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h
Removed Paths:
-------------
pkg/trunk/sandbox/message_filters/include/message_filters/tf.h
Modified: pkg/trunk/sandbox/message_filters/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/message_filters/CMakeLists.txt 2009-07-01 23:35:04 UTC (rev 18148)
+++ pkg/trunk/sandbox/message_filters/CMakeLists.txt 2009-07-01 23:44:00 UTC (rev 18149)
@@ -3,7 +3,6 @@
rospack(message_filters)
rospack_add_executable(filter_example src/filter_example.cpp)
-rospack_add_executable(tf_example src/tf_example.cpp)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
Modified: pkg/trunk/sandbox/message_filters/include/message_filters/consumer.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/consumer.h 2009-07-01 23:35:04 UTC (rev 18148)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/consumer.h 2009-07-01 23:44:00 UTC (rev 18149)
@@ -52,7 +52,7 @@
class Consumer
{
public:
- typedef boost::shared_ptr<M const> MPtr;
+ typedef boost::shared_ptr<M const> MConstPtr;
template<class A>
Consumer(A& a)
@@ -70,7 +70,7 @@
* involve pushing data onto queues, and pushing it along the pipeline into
* another filter
*/
- void processData(const MPtr& msg)
+ void processData(const MConstPtr& msg)
{
ROS_INFO("Called Consumer Callback!") ;
}
Deleted: pkg/trunk/sandbox/message_filters/include/message_filters/tf.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/tf.h 2009-07-01 23:35:04 UTC (rev 18148)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/tf.h 2009-07-01 23:44:00 UTC (rev 18149)
@@ -1,498 +0,0 @@
-/*
- * 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, 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.
- */
-
-/** \author Josh Faust */
-
-#ifndef TF_MESSAGE_FILTER_H
-#define TF_MESSAGE_FILTER_H
-
-#include <ros/ros.h>
-#include <tf/tf.h>
-#include <tf/tfMessage.h>
-#include <tf/message_notifier_base.h>
-
-#include <string>
-#include <list>
-#include <vector>
-#include <boost/function.hpp>
-#include <boost/bind.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-#include <boost/thread.hpp>
-#include <boost/signals.hpp>
-
-#include <ros/callback_queue.h>
-
-#define TF_MESSAGEFILTER_DEBUG(fmt, ...) \
- ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
-
-#define TF_MESSAGEFILTER_WARN(fmt, ...) \
- ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
-
-namespace tf
-{
-/**
- * \class MessageFilter
- *
- */
-template<class M>
-class MessageFilter
-{
-public:
- typedef boost::shared_ptr<M const> MConstPtr;
- typedef boost::function<void (const MConstPtr&)> Callback;
- typedef boost::signal<void (const MConstPtr&)> Signal;
- typedef boost::shared_ptr<Signal> SignalPtr;
- typedef boost::weak_ptr<Signal> SignalWPtr;
-
- MessageFilter(Transformer& tf, const std::string& target_frame, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle(), ros::Duration max_rate = ros::Duration(0.01), ros::Duration min_rate = ros::Duration(0.1))
- : tf_(tf)
- , nh_(nh)
- , min_rate_(min_rate)
- , max_rate_(max_rate)
- , queue_size_(queue_size)
- {
-
- }
-
- template<class F>
- MessageFilter(F& f, Transformer& tf, const std::string& target_frame, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle(), ros::Duration max_rate = ros::Duration(0.01), ros::Duration min_rate = ros::Duration(0.1))
- : tf_(tf)
- , nh_(nh)
- , min_rate_(min_rate)
- , max_rate_(max_rate)
- , queue_size_(queue_size)
- {
- init();
-
- target_frames_.resize(1);
- target_frames_[0] = target_frame;
- target_frames_string_ = target_frame;
-
- tf_connection_ = tf.addTransformChangedListener(boost::bind(&MessageFilter::transformsChanged, this));
-
- subscribeTo(f);
-
- if (min_rate > ros::Duration(0))
- {
- min_rate_timer_ = nh_.createTimer(min_rate, &MessageFilter::minRateTimerCallback, this);
- }
- }
-
- template<class F>
- void subscribeTo(F& f)
- {
- if (message_connection_.connected())
- {
- message_connection_.disconnect();
- }
-
- message_connection_ = f.connect(boost::bind(&MessageFilter::incomingMessage, this, _1));
- }
-
- /**
- * \brief Destructor
- */
- ~MessageFilter()
- {
- message_connection_.disconnect();
- tf_connection_.disconnect();
-
- clear();
-
- TF_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Failed Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu",
- successful_transform_count_, failed_transform_count_, failed_out_the_back_count_, transform_message_count_, incoming_message_count_, dropped_message_count_);
-
- }
-
- /**
- * \brief Set the frame you need to be able to transform to before getting a message callback
- */
- void setTargetFrame(const std::string& target_frame)
- {
- std::vector<std::string> frames;
- frames.push_back(target_frame);
- setTargetFrame(frames);
- }
-
- /**
- * \brief Set the frames you need to be able to transform to before getting a message callback
- */
- void setTargetFrame(const std::vector<std::string>& target_frames)
- {
- boost::mutex::scoped_lock list_lock(messages_mutex_);
- boost::mutex::scoped_lock string_lock(target_frames_string_mutex_);
-
- target_frames_ = target_frames;
-
- std::stringstream ss;
- for (std::vector<std::string>::const_iterator it = target_frames_.begin(); it != target_frames_.end(); ++it)
- {
- ss << *it << " ";
- }
- target_frames_string_ = ss.str();
- }
- /**
- * \brief Get the target frames as a string for debugging
- */
- std::string getTargetFramesString()
- {
- boost::mutex::scoped_lock lock(target_frames_string_mutex_);
- return target_frames_string_;
- };
-
- /**
- * \brief Set the required tolerance for the notifier to return true
- */
- void setTolerance(const ros::Duration& tolerance)
- {
- time_tolerance_ = tolerance;
- }
-
- /**
- * \brief Clear any messages currently in the queue
- */
- void clear()
- {
- boost::mutex::scoped_lock lock(messages_mutex_);
-
- messages_.clear();
- message_count_ = 0;
-
- invalidateCallbacks();
- }
-
- void enqueueMessage(const MConstPtr& message)
- {
- if (testMessage(message))
- {
- return;
- }
-
- boost::mutex::scoped_lock lock(messages_mutex_);
-
- // If this message is about to push us past our queue size, erase the oldest message
- if (queue_size_ != 0 && message_count_ + 1 > queue_size_)
- {
- ++dropped_message_count_;
- TF_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_, messages_.front()->header.frame_id.c_str(), messages_.front()->header.stamp.toSec());
- messages_.pop_front();
- --message_count_;
- }
-
- // Add the message to our list
- messages_.push_back(message);
- ++message_count_;
-
- TF_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", message->header.frame_id.c_str(), message->header.stamp.toSec(), message_count_);
-
- ++incoming_message_count_;
- }
-
- boost::signals::connection connect(const Callback& callback)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- return signal_.connect(callback);
- }
-
-private:
-
- void init()
- {
- message_count_ = 0;
- new_transforms_ = false;
- successful_transform_count_ = 0;
- failed_transform_count_ = 0;
- failed_out_the_back_count_ = 0;
- transform_message_count_ = 0;
- incoming_message_count_ = 0;
- dropped_message_count_ = 0;
- time_tolerance_ = ros::Duration(0.0);
- }
-
- typedef std::list<MConstPtr> L_Message;
-
- class ROSCallback : public ros::CallbackInterface
- {
- public:
- ROSCallback(MessageFilter* filter, const MConstPtr& message)
- : filter_(filter)
- , message_(message)
- , invalid_(false)
- {
- }
-
- void invalidate()
- {
- boost::mutex::scoped_lock lock(mutex_);
- invalid_ = true;
- message_.reset();
- }
-
- virtual CallResult call()
- {
- boost::mutex::scoped_lock lock(mutex_);
- if (invalid_)
- {
- return Invalid;
- }
-
- filter_->signal(message_);
- filter_->callbackFinished(this);
- return Success;
- }
- private:
- MessageFilter* filter_;
- MConstPtr message_;
- boost::mutex mutex_;
- bool invalid_;
- };
- typedef boost::shared_ptr<ROSCallback> ROSCallbackPtr;
- typedef std::set<ROSCallback*> S_ROSCallbackDumbPtr;
-
- void invalidateCallbacks()
- {
- typename S_ROSCallbackDumbPtr::iterator it = ros_callbacks_.begin();
- typename S_ROSCallbackDumbPtr::iterator end = ros_callbacks_.end();
- for (; it != end; ++it)
- {
- (*it)->invalidate();
- }
- }
-
- void signal(const MConstPtr& message)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- signal_(message);
- }
-
- void callbackFinished(ROSCallback* cb)
- {
- ros_callbacks_.erase(cb);
- }
-
- bool testMessage(const MConstPtr& message)
- {
- //Throw out messages which are too old
- //! \todo combine getLatestCommonTime call with the canTransform call
- for (std::vector<std::string>::iterator target_it = target_frames_.begin(); target_it != target_frames_.end(); ++target_it)
- {
- const std::string& target_frame = *target_it;
-
- if (target_frame != message->header.frame_id)
- {
- ros::Time latest_transform_time ;
- std::string error_string ;
-
- tf_.getLatestCommonTime(message->header.frame_id, target_frame, latest_transform_time, &error_string) ;
- if (message->header.stamp + tf_.getCacheLength() < latest_transform_time)
- {
- ++failed_out_the_back_count_;
- ++dropped_message_count_;
- TF_MESSAGEFILTER_DEBUG("Discarding Message, in frame %s, Out of the back of Cache Time(stamp: %.3f + cache_length: %.3f < latest_transform_time %.3f. Message Count now: %d", message->header.frame_id.c_str(), message->header.stamp.toSec(), tf_.getCacheLength().toSec(), latest_transform_time.toSec(), message_count_);
-
- last_out_the_back_stamp_ = message->header.stamp;
- last_out_the_back_frame_ = message->header.frame_id;
- return true;
- }
- }
-
- }
-
- bool ready = !target_frames_.empty();
- for (std::vector<std::string>::iterator target_it = target_frames_.begin(); ready && target_it != target_frames_.end(); ++target_it)
- {
- std::string& target_frame = *target_it;
- if (time_tolerance_ != ros::Duration(0.0))
- {
- ready = ready && (tf_.canTransform(target_frame, message->header.frame_id, message->header.stamp) &&
- tf_.canTransform(target_frame, message->header.frame_id, message->header.stamp + time_tolerance_) );
- }
- else
- {
- ready = ready && tf_.canTransform(target_frame, message->header.frame_id, message->header.stamp);
- }
- }
-
- if (ready)
- {
- TF_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", message->header.frame_id.c_str(), message->header.stamp.toSec(), message_count_);
-
- ++successful_transform_count_;
-
- ROSCallbackPtr cb(new ROSCallback(this, message));
- ros_callbacks_.insert(cb.get());
-
- /// \todo Use getCallbackQueue() once 0.6.2 comes out.
- //ros::CallbackQueueInterface* queue = nh_.getCallbackQueue();
- ros::CallbackQueueInterface* queue = ros::getGlobalCallbackQueue();
-
- queue->addCallback(cb);
- }
- else
- {
- ++failed_transform_count_;
- }
-
- return ready;
- }
-
- void testMessages()
- {
- last_test_time_ = ros::Time::now();
-
- if (!messages_.empty() && getTargetFramesString() == " ")
- {
- ROS_WARN_NAMED("message_notifier", "MessageFilter [target=%s]: empty target frame", getTargetFramesString().c_str());
- }
-
- int i = 0;
-
- typename L_Message::iterator it = messages_.begin();
- for (; it != messages_.end(); ++i)
- {
- MConstPtr& message = *it;
-
- if (testMessage(message))
- {
- --message_count_;
- it = messages_.erase(it);
- }
- else
- {
- ++it;
- }
- }
- }
-
- void minRateTimerCallback(const ros::TimerEvent&)
- {
- if (new_transforms_)
- {
- testMessages();
- new_transforms_ = false;
- }
-
- checkFailures();
- }
-
- /**
- * \brief Callback that happens when we receive a message on the message topic
- */
- void incomingMessage(const MConstPtr msg)
- {
- enqueueMessage(msg);
- }
-
- void transformsChanged()
- {
- if (ros::Time::now() - last_test_time_ > max_rate_)
- {
- testMessages();
- }
- else
- {
- new_transforms_ = true;
- }
-
- ++transform_message_count_;
- }
-
- void checkFailures()
- {
- if (next_failure_warning_.isZero())
- {
- next_failure_warning_ = ros::Time::now() + ros::Duration(15);
- }
-
- if (ros::Time::now() >= next_failure_warning_)
- {
- if (incoming_message_count_ - message_count_ == 0)
- {
- return;
- }
-
- double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_);
- if (dropped_pct > 0.95)
- {
- TF_MESSAGEFILTER_WARN("Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct*100, ROSCONSOLE_DEFAULT_NAME);
- next_failure_warning_ = ros::Time::now() + ros::Duration(60);
-
- if ((double)failed_out_the_back_count_ / (double)dropped_message_count_ > 0.5)
- {
- TF_MESSAGEFILTER_WARN(" The majority of dropped messages were due to messages growing older than the TF cache time. The last message's timestamp was: %f, and the last frame_id was: %s", last_out_the_back_stamp_.toSec(), last_out_the_back_frame_.c_str());
- }
- }
- }
- }
-
- Transformer& tf_; ///< The Transformer used to determine if transformation data is available
- ros::NodeHandle nh_; ///< The node used to subscribe to the topic
- ros::Duration min_rate_;
- ros::Duration max_rate_;
- ros::Timer min_rate_timer_;
- ros::Time last_test_time_;
- std::vector<std::string> target_frames_; ///< The frames we need to be able to transform to before a message is ready
- std::string target_frames_string_;
- boost::mutex target_frames_string_mutex_;
- uint32_t queue_size_; ///< The maximum number of messages we queue up
-
- Signal signal_;
- boost::mutex signal_mutex_;
- S_ROSCallbackDumbPtr ros_callbacks_;
-
- L_Message messages_; ///< The message list
- uint32_t message_count_; ///< The number of messages in the list. Used because messages_.size() has linear cost
- boost::mutex messages_mutex_; ///< The mutex used for locking message list operations
-
- bool new_messages_; ///< Used to skip waiting on new_data_ if new messages have come in while calling back
- volatile bool new_transforms_; ///< Used to skip waiting on new_data_ if new transforms have come in while calling back or transforming data
-
- uint64_t successful_transform_count_;
- uint64_t failed_transform_count_;
- uint64_t failed_out_the_back_count_;
- uint64_t transform_message_count_;
- uint64_t incoming_message_count_;
- uint64_t dropped_message_count_;
-
- ros::Time last_out_the_back_stamp_;
- std::string last_out_the_back_frame_;
-
- ros::Time next_failure_warning_;
-
- ros::Duration time_tolerance_; ///< Provide additional tolerance on time for messages which are stamped but can have associated duration
-
- boost::signals::connection tf_connection_;
- boost::signals::connection message_connection_;
-};
-
-} // namespace tf
-
-#endif
-
Modified: pkg/trunk/sandbox/message_filters/manifest.xml
===================================================================
--- pkg/trunk/sandbox/message_filters/manifest.xml 2009-07-01 23:35:04 UTC (rev 18148)
+++ pkg/trunk/sandbox/message_filters/manifest.xml 2009-07-01 23:44:00 UTC (rev 18149)
@@ -7,7 +7,6 @@
<depend package="std_msgs" />
<depend package="robot_msgs" />
<depend package="sensor_msgs" />
- <depend package="tf" />
<export>
<cpp cflags="-I${prefix}/include" />
</export>
Copied: pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h (from rev 18144, pkg/trunk/sandbox/message_filters/include/message_filters/tf.h)
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h (rev 0)
+++ pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h 2009-07-01 23:44:00 UTC (rev 18149)
@@ -0,0 +1,498 @@
+/*
+ * 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, 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.
+ */
+
+/** \author Josh Faust */
+
+#ifndef TF_MESSAGE_FILTER_H
+#define TF_MESSAGE_FILTER_H
+
+#include <ros/ros.h>
+#include <tf/tf.h>
+#include <tf/tfMessage.h>
+#include <tf/message_notifier_base.h>
+
+#include <string>
+#include <list>
+#include <vector>
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/thread.hpp>
+#include <boost/signals.hpp>
+
+#include <ros/callback_queue.h>
+
+#define TF_MESSAGEFILTER_DEBUG(fmt, ...) \
+ ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
+
+#define TF_MESSAGEFILTER_WARN(fmt, ...) \
+ ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
+
+namespace tf
+{
+/**
+ * \class MessageFilter
+ *
+ */
+template<class M>
+class MessageFilter
+{
+public:
+ typedef boost::shared_ptr<M const> MConstPtr;
+ typedef boost::function<void (const MConstPtr&)> Callback;
+ typedef boost::signal<void (const MConstPtr&)> Signal;
+ typedef boost::shared_ptr<Signal> SignalPtr;
+ typedef boost::weak_ptr<Signal> SignalWPtr;
+
+ MessageFilter(Transformer& tf, const std::string& target_frame, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle(), ros::Duration max_rate = ros::Duration(0.01), ros::Duration min_rate = ros::Duration(0.1))
+ : tf_(tf)
+ , nh_(nh)
+ , min_rate_(min_rate)
+ , max_rate_(max_rate)
+ , queue_size_(queue_size)
+ {
+
+ }
+
+ template<class F>
+ MessageFilter(F& f, Transformer& tf, const std::string& target_frame, uint32_t queue_size, ros::NodeHandle nh = ros::NodeHandle(), ros::Duration max_rate = ros::Duration(0.01), ros::Duration min_rate = ros::Duration(0.1))
+ : tf_(tf)
+ , nh_(nh)
+ , min_rate_(min_rate)
+ , max_rate_(max_rate)
+ , queue_size_(queue_size)
+ {
+ init();
+
+ target_frames_.resize(1);
+ target_frames_[0] = target_frame;
+ target_frames_string_ = target_frame;
+
+ tf_connection_ = tf.addTransformChangedListener(boost::bind(&MessageFilter::transformsChanged, this));
+
+ subscribeTo(f);
+
+ if (min_rate > ros::Duration(0))
+ {
+ min_rate_timer_ = nh_.createTimer(min_rate, &MessageFilter::minRateTimerCallback, this);
+ }
+ }
+
+ template<class F>
+ void subscribeTo(F& f)
+ {
+ if (message_connection_.connected())
+ {
+ message_connection_.disconnect();
+ }
+
+ message_connection_ = f.connect(boost::bind(&MessageFilter::incomingMessage, this, _1));
+ }
+
+ /**
+ * \brief Destructor
+ */
+ ~MessageFilter()
+ {
+ message_connection_.disconnect();
+ tf_connection_.disconnect();
+
+ clear();
+
+ TF_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Failed Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu",
+ successful_transform_count_, failed_transform_count_, failed_out_the_back_count_, transform_message_count_, incoming_message_count_, dropped_message_count_);
+
+ }
+
+ /**
+ * \brief Set the frame you need to be able to transform to before getting a message callback
+ */
+ void setTargetFrame(const std::string& target_frame)
+ {
+ std::vector<std::string> frames;
+ frames.push_back(target_frame);
+ setTargetFrame(frames);
+ }
+
+ /**
+ * \brief Set the frames you need to be able to transform to before getting a message callback
+ */
+ void setTargetFrame(const std::vector<std::string>& target_frames)
+ {
+ boost::mutex::scoped_lock list_lock(messages_mutex_);
+ boost::mutex::scoped_lock string_lock(target_frames_string_mutex_);
+
+ target_frames_ = target_frames;
+
+ std::stringstream ss;
+ for (std::vector<std::string>::const_iterator it = target_frames_.begin(); it != target_frames_.end(); ++it)
+ {
+ ss << *it << " ";
+ }
+ target_frames_string_ = ss.str();
+ }
+ /**
+ * \brief Get the target frames as a string for debugging
+ */
+ std::string getTargetFramesString()
+ {
+ boost::mutex::scoped_lock lock(target_frames_string_mutex_);
+ return target_frames_string_;
+ };
+
+ /**
+ * \brief Set the required tolerance for the notifier to return true
+ */
+ void setTolerance(const ros::Duration& tolerance)
+ {
+ time_tolerance_ = tolerance;
+ }
+
+ /**
+ * \brief Clear any messages currently in the queue
+ */
+ void clear()
+ {
+ boost::mutex::scoped_lock lock(messages_mutex_);
+
+ messages_.clear();
+ message_count_ = 0;
+
+ invalidateCallbacks();
+ }
+
+ void enqueueMessage(const MConstPtr& message)
+ {
+ if (testMessage(message))
+ {
+ return;
+ }
+
+ boost::mutex::scoped_lock lock(messages_mutex_);
+
+ // If this message is about to push us past our queue size, erase the oldest message
+ if (queue_size_ != 0 && message_count_ + 1 > queue_size_)
+ {
+ ++dropped_message_count_;
+ TF_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_, messages_.front()->header.frame_id.c_str(), messages_.front()->header.stamp.toSec());
+ messages_.pop_front();
+ --message_count_;
+ }
+
+ // Add the message to our list
+ messages_.push_back(message);
+ ++message_count_;
+
+ TF_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", message->header.frame_id.c_str(), message->header.stamp.toSec(), message_count_);
+
+ ++incoming_message_count_;
+ }
+
+ boost::signals::connection connect(const Callback& callback)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ return signal_.connect(callback);
+ }
+
+private:
+
+ void init()
+ {
+ message_count_ = 0;
+ new_transforms_ = false;
+ successful_transform_count_ = 0;
+ failed_transform_count_ = 0;
+ failed_out_the_back_count_ = 0;
+ transform_message_count_ = 0;
+ incoming_message_count_ = 0;
+ dropped_message_count_ = 0;
+ time_tolerance_ = ros::Duration(0.0);
+ }
+
+ typedef std::list<MConstPtr> L_Message;
+
+ class ROSCallback : public ros::CallbackInterface
+ {
+ public:
+ ROSCallback(MessageFilter* filter, const MConstPtr& message)
+ : filter_(filter)
+ , message_(message)
+ , invalid_(false)
+ {
+ }
+
+ void invalidate()
+ {
+ boost::mutex::scoped_lock lock(mutex_);
+ invalid_ = true;
+ message_.reset();
+ }
+
+ virtual CallResult call()
+ {
+ boost::mutex::scoped_lock lock(mutex_);
+ if (invalid_)
+ {
+ return Invalid;
+ }
+
+ filter_->signal(message_);
+ filter_->callbackFinished(this);
+ return Success;
+ }
+ private:
+ MessageFilter* filter_;
+ MConstPtr message_;
+ boost::mutex mutex_;
+ bool invalid_;
+ };
+ typedef boost::shared_ptr<ROSCallback> ROSCallbackPtr;
+ typedef std::set<ROSCallback*> S_ROSCallbackDumbPtr;
+
+ void invalidateCallbacks()
+ {
+ typename S_ROSCallbackDumbPtr::iterator it = ros_callbacks_.begin();
+ typename S_ROSCallbackDumbPtr::iterator end = ros_callbacks_.end();
+ for (; it != end; ++it)
+ {
+ (*it)->invalidate();
+ }
+ }
+
+ void signal(const MConstPtr& message)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_(message);
+ }
+
+ void callbackFinished(ROSCallback* cb)
+ {
+ ros_callbacks_.erase(cb);
+ }
+
+ bool testMessage(const MConstPtr& message)
+ {
+ //Throw out messages which are too old
+ //! \todo combine getLatestCommonTime call with the canTransform call
+ for (std::vector<std::string>::iterator target_it = target_frames_.begin(); target_it != target_frames_.end(); ++target_it)
+ {
+ const std::string& target_frame = *target_it;
+
+ if (target_frame != message->header.frame_id)
+ {
+ ros::Time latest_transform_time ;
+ std::string error_string ;
+
+ tf_.getLatestCommonTime(message->header.frame_id, target_frame, latest_transform_time, &error_string) ;
+ if (message->header.stamp + tf_.getCacheLength() < latest_transform_time)
+ {
+ ++failed_out_the_back_count_;
+ ++dropped_message_count_;
+ TF_MESSAGEFILTER_DEBUG("Discarding Message, in frame %s, Out of the back of Cache Time(stamp: %.3f + cache_length: %.3f < latest_transform_time %.3f. Message Count now: %d", message->header.frame_id.c_str(), message->header.stamp.toSec(), tf_.getCacheLength().toSec(), latest_transform_time.toSec(), message_count_);
+
+ last_out_the_back_stamp_ = message->header.stamp;
+ last_out_the_back_frame_ = message->header.frame_id;
+ return true;
+ }
+ }
+
+ }
+
+ bool ready = !target_frames_.empty();
+ for (std::vector<std::string>::iterator target_it = target_frames_.begin(); ready && target_it != target_frames_.end(); ++target_it)
+ {
+ std::string& target_frame = *target_it;
+ if (time_tolerance_ != ros::Duration(0.0))
+ {
+ ready = ready && (tf_.canTransform(target_frame, message->header.frame_id, message->header.stamp) &&
+ tf_.canTransform(target_frame, message->header.frame_id, message->header.stamp + time_tolerance_) );
+ }
+ else
+ {
+ ready = ready && tf_.canTransform(target_frame, message->header.frame_id, message->header.stamp);
+ }
+ }
+
+ if (ready)
+ {
+ TF_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", message->header.frame_id.c_str(), message->header.stamp...
[truncated message content] |
|
From: <is...@us...> - 2009-07-02 04:49:31
|
Revision: 18163
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18163&view=rev
Author: isucan
Date: 2009-07-02 04:49:29 +0000 (Thu, 02 Jul 2009)
Log Message:
-----------
more testing on planning with dynamics
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/KPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/src/KPIECE1.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/rrt/RRT.h
pkg/trunk/motion_planning/ompl/code/tests/dynamic/2dmap/2dmap.cpp
pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/PR2PropagationModels.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicRRTSetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/stacks/pr2/pr2_defs/planning/planning.yaml
Added Paths:
-----------
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/dynamicKPIECESetup.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicKPIECESetup.cpp
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/KPIECE1.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/KPIECE1.h 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/KPIECE1.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -82,11 +82,10 @@
m_projectionEvaluator = NULL;
m_projectionDimension = 0;
m_goalBias = 0.05;
- m_selectBorderPercentage = 0.9;
- m_badScoreFactor = 0.5;
+ m_selectBorderPercentage = 0.7;
+ m_badScoreFactor = 0.3;
m_goodScoreFactor = 0.9;
m_minValidPathStates = 3;
- m_rho = 0.5;
m_tree.grid.onCellUpdate(computeImportance, NULL);
}
@@ -123,30 +122,6 @@
return m_goalBias;
}
- /** Set the range the planner is supposed to use. This
- parameter greatly influences the runtime of the
- algorithm. It is probably a good idea to find what a good
- value is for each model the planner is used for. The range
- parameter influences how this @b qm along the path between
- @b qc and @b qr is chosen. @b qr may be too far, and it
- may not be best to have @b qm = @b qr all the time (range
- = 1.0 implies @b qm = @b qr. range should be less than
- 1.0). However, in a large space, it is also good to leave
- the neighborhood of @b qc (range = 0.0 implies @b qm = @b
- qc and no progress is made. rande should be larger than
- 0.0). Multiple values of this range parameter should be
- tried until a suitable one is found. */
- void setRange(double rho)
- {
- m_rho = rho;
- }
-
- /** \brief Get the range the planner is using */
- double getRange(void) const
- {
- return m_rho;
- }
-
/** \brief Set the projection evaluator. This class is able to
compute the projection of a given state. The simplest
option is to use an orthogonal projection; see
@@ -293,7 +268,6 @@
double m_badScoreFactor;
double m_selectBorderPercentage;
double m_goalBias;
- double m_rho;
random_utils::RNG m_rng;
};
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/src/KPIECE1.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/src/KPIECE1.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/kpiece/src/KPIECE1.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -82,10 +82,21 @@
double approxdif = INFINITY;
base::Control *rctrl = new base::Control(cdim);
+ std::vector<Grid::Coord> coords(si->getMaxControlDuration() + 1);
std::vector<base::State*> states(si->getMaxControlDuration() + 1);
for (unsigned int i = 0 ; i < states.size() ; ++i)
states[i] = new base::State(sdim);
+ // coordinates of the goal state and the best state seen so far
+ Grid::Coord best_coord, better_coord;
+ bool haveBestCoord = false;
+ bool haveBetterCoord = false;
+ if (goal_s)
+ {
+ m_projectionEvaluator->computeCoordinates(goal_s->state, best_coord);
+ haveBestCoord = true;
+ }
+
while (time_utils::Time::now() < endTime)
{
m_tree.iteration++;
@@ -93,7 +104,20 @@
/* Decide on a state to expand from */
Motion *existing = NULL;
Grid::Cell *ecell = NULL;
- selectMotion(existing, ecell);
+
+ if (m_rng.uniform(0.0, 1.0) < m_goalBias)
+ {
+ if (haveBestCoord)
+ ecell = m_tree.grid.getCell(best_coord);
+ if (!ecell && haveBetterCoord)
+ ecell = m_tree.grid.getCell(better_coord);
+ if (ecell)
+ existing = ecell->data->motions[m_rng.halfNormalInt(0, ecell->data->motions.size() - 1)];
+ else
+ selectMotion(existing, ecell);
+ }
+ else
+ selectMotion(existing, ecell);
assert(existing);
@@ -113,16 +137,63 @@
cd = 0;
}
- /* if we have */
+ /* if we have enough steps */
if (cd >= m_minValidPathStates)
{
- /* create a motion */
+
+ // split the motion into smaller ones, so we do not cross cell boundaries
+ for (unsigned int i = 0 ; i <= cd ; ++i)
+ m_projectionEvaluator->computeCoordinates(states[i], coords[i]);
+
+ unsigned int start = 0;
+ unsigned int curr = 1;
+ while (curr < cd)
+ {
+ // we have reached into a new cell
+ if (coords[start] != coords[curr])
+ {
+ /* create a motion */
+ Motion *motion = new Motion(sdim, cdim);
+ si->copyState(motion->state, states[curr - 1]);
+ si->copyControl(motion->control, rctrl);
+ motion->steps = curr - start;
+ motion->parent = existing;
+
+ double dist = 0.0;
+ bool solved = goal_r->isSatisfied(motion->state, &dist);
+ addMotion(motion, dist);
+
+ if (solved)
+ {
+ approxdif = dist;
+ solution = motion;
+ break;
+ }
+ if (dist < approxdif)
+ {
+ approxdif = dist;
+ approxsol = motion;
+ better_coord = coords[start];
+ haveBetterCoord = true;
+ }
+
+ // new parent will be the newly created motion
+ existing = motion;
+ start = curr;
+ }
+ curr++;
+ }
+
+ if (solution)
+ break;
+
+ /* create the last segment of the motion */
Motion *motion = new Motion(sdim, cdim);
si->copyState(motion->state, states[cd]);
si->copyControl(motion->control, rctrl);
- motion->steps = cd;
+ motion->steps = cd - start;
motion->parent = existing;
-
+
double dist = 0.0;
bool solved = goal_r->isSatisfied(motion->state, &dist);
addMotion(motion, dist);
@@ -138,6 +209,8 @@
approxdif = dist;
approxsol = motion;
}
+
+ // update cell score
ecell->data->score *= m_goodScoreFactor;
}
else
@@ -219,7 +292,7 @@
if (cell)
{
cell->data->motions.push_back(motion);
- cell->data->coverage += 1.0;
+ cell->data->coverage += motion->steps;
m_tree.grid.update(cell);
}
else
@@ -227,7 +300,7 @@
cell = m_tree.grid.createCell(coord);
cell->data = new CellData();
cell->data->motions.push_back(motion);
- cell->data->coverage = 1.0;
+ cell->data->coverage = motion->steps;
cell->data->iteration = m_tree.iteration;
cell->data->selections = 1;
cell->data->score = 1.0 / (1e-3 + dist);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/rrt/RRT.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/rrt/RRT.h 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/dynamic/extension/rrt/RRT.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -75,7 +75,6 @@
m_nn.setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
m_goalBias = 0.05;
m_hintBias = 0.75;
- m_rho = 0.5;
}
virtual ~RRT(void)
@@ -127,30 +126,6 @@
return m_hintBias;
}
- /** Set the range the planner is supposed to use. This
- parameter greatly influences the runtime of the
- algorithm. It is probably a good idea to find what a good
- value is for each model the planner is used for. The range
- parameter influences how this @b qm along the path between
- @b qc and @b qr is chosen. @b qr may be too far, and it
- may not be best to have @b qm = @b qr all the time (range
- = 1.0 implies @b qm = @b qr. range should be less than
- 1.0). However, in a large space, it is also good to leave
- the neighborhood of @b qc (range = 0.0 implies @b qm = @b
- qc and no progress is made. rande should be larger than
- 0.0). Multiple values of this range parameter should be
- tried until a suitable one is found. */
- void setRange(double rho)
- {
- m_rho = rho;
- }
-
- /** \brief Get the range the planner is using */
- double getRange(void) const
- {
- return m_rho;
- }
-
virtual void getStates(std::vector<const base::State*> &states) const;
protected:
@@ -210,7 +185,6 @@
double m_goalBias;
double m_hintBias;
- double m_rho;
random_utils::RNG m_rng;
};
Modified: pkg/trunk/motion_planning/ompl/code/tests/dynamic/2dmap/2dmap.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/tests/dynamic/2dmap/2dmap.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl/code/tests/dynamic/2dmap/2dmap.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -313,7 +313,6 @@
base::Planner* newPlanner(dynamic::SpaceInformationControlsIntegrator *si)
{
dynamic::RRT *rrt = new dynamic::RRT(si);
- rrt->setRange(0.95);
return rrt;
}
};
@@ -343,7 +342,6 @@
base::Planner* newPlanner(dynamic::SpaceInformationControlsIntegrator *si)
{
dynamic::KPIECE1 *kpiece = new dynamic::KPIECE1(si);
- kpiece->setRange(0.95);
std::vector<unsigned int> projection;
projection.push_back(0);
Modified: pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-07-02 04:49:29 UTC (rev 18163)
@@ -15,6 +15,7 @@
src/helpers/ompl_planner/kinematicLBKPIECESetup.cpp
src/helpers/ompl_planner/kinematicSBLSetup.cpp
src/helpers/ompl_planner/dynamicRRTSetup.cpp
+ src/helpers/ompl_planner/dynamicKPIECESetup.cpp
src/helpers/ompl_extensions/GoalDefinitions.cpp
src/helpers/ompl_extensions/SpaceInformation.cpp
src/helpers/ompl_extensions/ForwardPropagationModels.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -78,6 +78,7 @@
void add_kIKKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
void add_dRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
+ void add_dKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
};
typedef std::map<std::string, Model*> ModelMap;
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/PR2PropagationModels.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/PR2PropagationModels.h 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/PR2PropagationModels.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -58,15 +58,15 @@
*dimension = 2;
component.resize(*dimension);
component[0].type = ompl::base::ControlComponent::LINEAR;
- component[0].minValue = -0.2;
+ component[0].minValue = -1.0;
component[0].maxValue = 1.5;
component[1].type = ompl::base::ControlComponent::LINEAR;
- component[1].minValue = -0.5;
- component[1].maxValue = 0.5;
+ component[1].minValue = -0.9;
+ component[1].maxValue = 0.9;
- *resolution = 0.1;
- *minDuration = 1;
- *maxDuration = 5;
+ *resolution = 0.05;
+ *minDuration = 5;
+ *maxDuration = 10;
}
virtual void operator()(const ompl::base::State *begin, const ompl::base::Control *ctrl, double resolution, ompl::base::State *end) const
Copied: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/dynamicKPIECESetup.h (from rev 18088, pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/kinematicKPIECESetup.h)
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/dynamicKPIECESetup.h (rev 0)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/dynamicKPIECESetup.h 2009-07-02 04:49:29 UTC (rev 18163)
@@ -0,0 +1,58 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef OMPL_PLANNING_PLANNERS_DYNAMIC_KPIECE_SETUP_
+#define OMPL_PLANNING_PLANNERS_DYNAMIC_KPIECE_SETUP_
+
+#include "ompl_planning/planners/PlannerSetup.h"
+#include <ompl/extension/dynamic/extension/kpiece/KPIECE1.h>
+
+namespace ompl_planning
+{
+
+ class dynamicKPIECESetup : public PlannerSetup
+ {
+ public:
+
+ dynamicKPIECESetup(ModelBase *m);
+ virtual ~dynamicKPIECESetup(void);
+ virtual bool setup(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
+ };
+
+} // ompl_planning
+
+#endif
+
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -44,6 +44,7 @@
#include "ompl_planning/planners/kinematicLBKPIECESetup.h"
#include "ompl_planning/planners/dynamicRRTSetup.h"
+#include "ompl_planning/planners/dynamicKPIECESetup.h"
/* instantiate the planners that can be used */
void ompl_planning::Model::createMotionPlanningInstances(std::vector< boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> > cfgs)
@@ -76,6 +77,9 @@
if (type == "dynamic::RRT")
add_dRRT(cfgs[i]);
else
+ if (type == "dynamic::KPIECE")
+ add_dKPIECE(cfgs[i]);
+ else
ROS_WARN("Unknown planner type: %s", type.c_str());
}
}
@@ -143,6 +147,15 @@
delete kpiece;
}
+void ompl_planning::Model::add_dKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
+{
+ PlannerSetup *kpiece = new dynamicKPIECESetup(dynamic_cast<ModelBase*>(this));
+ if (kpiece->setup(options))
+ planners[kpiece->name] = kpiece;
+ else
+ delete kpiece;
+}
+
void ompl_planning::Model::add_kLBKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
{
PlannerSetup *kpiece = new kinematicLBKPIECESetup(dynamic_cast<ModelBase*>(this));
Copied: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicKPIECESetup.cpp (from rev 18088, pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp)
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicKPIECESetup.cpp (rev 0)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicKPIECESetup.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -0,0 +1,81 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include "ompl_planning/planners/dynamicKPIECESetup.h"
+
+ompl_planning::dynamicKPIECESetup::dynamicKPIECESetup(ModelBase *m) : PlannerSetup(m)
+{
+ name = "dynamic::KPIECE";
+ priority = 3;
+}
+
+ompl_planning::dynamicKPIECESetup::~dynamicKPIECESetup(void)
+{
+ if (dynamic_cast<ompl::dynamic::KPIECE1*>(mp))
+ {
+ ompl::base::ProjectionEvaluator *pe = dynamic_cast<ompl::dynamic::KPIECE1*>(mp)->getProjectionEvaluator();
+ if (pe)
+ delete pe;
+ }
+}
+
+bool ompl_planning::dynamicKPIECESetup::setup(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
+{
+ preSetup(options);
+
+ ompl::dynamic::KPIECE1 *kpiece = new ompl::dynamic::KPIECE1(dynamic_cast<ompl::dynamic::SpaceInformationControlsIntegrator*>(si));
+ mp = kpiece;
+
+ if (options->hasParam("goal_bias"))
+ {
+ kpiece->setGoalBias(options->getParamDouble("goal_bias", kpiece->getGoalBias()));
+ ROS_DEBUG("Goal bias is set to %g", kpiece->getGoalBias());
+ }
+
+ kpiece->setProjectionEvaluator(getProjectionEvaluator(options));
+
+ if (kpiece->getProjectionEvaluator() == NULL)
+ {
+ ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
+ return false;
+ }
+ else
+ {
+ postSetup(options);
+ return true;
+ }
+}
+
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicRRTSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicRRTSetup.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/dynamicRRTSetup.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -53,18 +53,18 @@
ompl::dynamic::RRT *rrt = new ompl::dynamic::RRT(dynamic_cast<ompl::dynamic::SpaceInformationControlsIntegrator*>(si));
mp = rrt;
- if (options->hasParam("range"))
- {
- rrt->setRange(options->getParamDouble("range", rrt->getRange()));
- ROS_DEBUG("Range is set to %g", rrt->getRange());
- }
-
if (options->hasParam("goal_bias"))
{
rrt->setGoalBias(options->getParamDouble("goal_bias", rrt->getGoalBias()));
ROS_DEBUG("Goal bias is set to %g", rrt->getGoalBias());
}
+ if (options->hasParam("hint_bias"))
+ {
+ rrt->setHintBias(options->getParamDouble("hint_bias", rrt->getHintBias()));
+ ROS_DEBUG("Goal bias is set to %g", rrt->getHintBias());
+ }
+
postSetup(options);
return true;
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/kinematicKPIECESetup.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -39,7 +39,7 @@
ompl_planning::kinematicKPIECESetup::kinematicKPIECESetup(ModelBase *m) : PlannerSetup(m)
{
name = "kinematic::KPIECE";
- priority = 3;
+ priority = 4;
}
ompl_planning::kinematicKPIECESetup::~kinematicKPIECESetup(void)
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -45,6 +45,9 @@
OMPLPlanning(void)
{
+ // display the first 3 coordinates of states in diffusion trees
+ requestHandler_.enableDebugMode(0, 1);
+
// register with ROS
collisionModels_ = new planning_environment::CollisionModels("robot_description");
if (nodeHandle_.hasParam("~planning_frame_id"))
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-02 04:49:29 UTC (rev 18163)
@@ -131,14 +131,14 @@
req.params.model_id = "base";
req.params.distance_metric = "L2Square";
- req.params.planner_id = "dynamic::RRT";
+ req.params.planner_id = "dynamic::KPIECE";
- req.params.volumeMin.x = -8;
- req.params.volumeMin.y = -8;
+ req.params.volumeMin.x = -3;
+ req.params.volumeMin.y = -3;
req.params.volumeMin.z = 0;
- req.params.volumeMax.x = 8;
- req.params.volumeMax.y = 8;
+ req.params.volumeMax.x = 3;
+ req.params.volumeMax.y = 3;
req.params.volumeMax.z = 0;
req.times = 1;
@@ -153,20 +153,20 @@
req.goal_constraints.joint_constraint[0].toleranceBelow.resize(3);
req.goal_constraints.joint_constraint[0].value[0] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceBelow[0] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceAbove[0] = 0.0;
+ req.goal_constraints.joint_constraint[0].toleranceBelow[0] = 0.05;
+ req.goal_constraints.joint_constraint[0].toleranceAbove[0] = 0.05;
req.goal_constraints.joint_constraint[0].value[1] = 1.0;
- req.goal_constraints.joint_constraint[0].toleranceBelow[1] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceAbove[1] = 0.0;
+ req.goal_constraints.joint_constraint[0].toleranceBelow[1] = 0.05;
+ req.goal_constraints.joint_constraint[0].toleranceAbove[1] = 0.05;
req.goal_constraints.joint_constraint[0].value[2] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceBelow[2] = 0.0;
- req.goal_constraints.joint_constraint[0].toleranceAbove[2] = 0.0;
+ req.goal_constraints.joint_constraint[0].toleranceBelow[2] = 0.1;
+ req.goal_constraints.joint_constraint[0].toleranceAbove[2] = 0.1;
// allow 1 second computation time
- req.allowed_time = 10.0;
+ req.allowed_time = 20.0;
// define the service messages
motion_planning_srvs::MotionPlan::Response res;
Modified: pkg/trunk/stacks/pr2/pr2_defs/planning/planning.yaml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/planning/planning.yaml 2009-07-02 04:27:52 UTC (rev 18162)
+++ pkg/trunk/stacks/pr2/pr2_defs/planning/planning.yaml 2009-07-02 04:49:29 UTC (rev 18163)
@@ -14,7 +14,7 @@
links:
base_link
planner_configs:
- RRTkConfig1 RRTdConfig1 LazyRRTkConfig1 SBLkConfig1 KPIECEkConfig1
+ RRTkConfig1 RRTdConfig1 LazyRRTkConfig1 SBLkConfig1 KPIECEkConfig1 KPIECEdConfig1
left_arm:
links:
@@ -79,7 +79,6 @@
RRTdConfig1:
type: dynamic::RRT
- range: 0.75
LazyRRTkConfig1:
type: kinematic::LazyRRT
@@ -96,7 +95,12 @@
projection: 0 1
celldim: 1 1
range: 0.5
-
+
+ KPIECEdConfig1:
+ type: dynamic::KPIECE
+ projection: 0 1
+ celldim: 0.5 0.5
+
RRTkConfig2:
type: kinematic::RRT
range: 0.75
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <goo...@us...> - 2009-07-02 17:04:39
|
Revision: 18185
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18185&view=rev
Author: goodfellow
Date: 2009-07-02 17:04:37 +0000 (Thu, 02 Jul 2009)
Log Message:
-----------
Moved stairvision package out of 3rdparty, into sandbox
Modified Paths:
--------------
pkg/trunk/vision/stereo_view/stereo_view.cpp
Added Paths:
-----------
pkg/trunk/sandbox/stairvision/
Removed Paths:
-------------
pkg/trunk/3rdparty/stairvision/
Property changes on: pkg/trunk/sandbox/stairvision
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/3rdparty/stairvision:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-07-02 17:02:02 UTC (rev 18184)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-07-02 17:04:37 UTC (rev 18185)
@@ -93,7 +93,7 @@
std::list<std::string> left_list;
left_list.push_back(mapName("stereo") + std::string("/left/image_rect_color"));
- left_list.push_back(mapName("stereo") + std::string("/left/image_rect"));
+ // left_list.push_back(mapName("stereo") + std::string("/left/image_rect"));
std::list<std::string> right_list;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-02 18:38:53
|
Revision: 18201
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18201&view=rev
Author: isucan
Date: 2009-07-02 18:37:50 +0000 (Thu, 02 Jul 2009)
Log Message:
-----------
automatic selection of planners
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-02 18:37:50 UTC (rev 18201)
@@ -1,7 +1,9 @@
<launch>
<include file="$(find pr2_defs)/pr2_planning_environment.launch" />
- <include file="$(find pr2_laser_tilt)/laser_tilt_controller.launch" />
+
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 4 .75 .25" />
<node pkg="laser_scan" type="scan_to_cloud" output="screen">
<param name="scan_topic" type="string" value="tilt_scan" />
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h 2009-07-02 18:37:50 UTC (rev 18201)
@@ -67,18 +67,10 @@
std::map<std::string, PlannerSetup*> planners;
protected:
+
+ template<typename _T>
+ void add_planner(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
- void add_kRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
- void add_kLazyRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
- void add_kEST(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
- void add_kSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
- void add_kIKSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
- void add_kKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
- void add_kLBKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
- void add_kIKKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
-
- void add_dRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
- void add_dKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
};
typedef std::map<std::string, Model*> ModelMap;
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp 2009-07-02 18:37:50 UTC (rev 18201)
@@ -53,114 +53,45 @@
for (unsigned int i = 0 ; i < cfgs.size() ; ++i)
{
std::string type = cfgs[i]->getParamString("type");
+
if (type == "kinematic::RRT")
- add_kRRT(cfgs[i]);
+ add_planner<kinematicRRTSetup>(cfgs[i]);
else
if (type == "kinematic::LazyRRT")
- add_kLazyRRT(cfgs[i]);
+ add_planner<kinematicLazyRRTSetup>(cfgs[i]);
else
if (type == "kinematic::EST")
- add_kEST(cfgs[i]);
+ add_planner<kinematicESTSetup>(cfgs[i]);
else
if (type == "kinematic::SBL")
- add_kSBL(cfgs[i]);
+ add_planner<kinematicSBLSetup>(cfgs[i]);
else
if (type == "kinematic::KPIECE")
- add_kKPIECE(cfgs[i]);
+ add_planner<kinematicKPIECESetup>(cfgs[i]);
else
if (type == "kinematic::LBKPIECE")
- add_kLBKPIECE(cfgs[i]);
+ add_planner<kinematicLBKPIECESetup>(cfgs[i]);
else
if (type == "kinematic::IKSBL")
- add_kIKSBL(cfgs[i]);
+ add_planner<kinematicIKSBLSetup>(cfgs[i]);
else
if (type == "dynamic::RRT")
- add_dRRT(cfgs[i]);
+ add_planner<dynamicRRTSetup>(cfgs[i]);
else
if (type == "dynamic::KPIECE")
- add_dKPIECE(cfgs[i]);
+ add_planner<dynamicKPIECESetup>(cfgs[i]);
else
ROS_WARN("Unknown planner type: %s", type.c_str());
}
}
-void ompl_planning::Model::add_dRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
+template<typename _T>
+void ompl_planning::Model::add_planner(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
{
- PlannerSetup *rrt = new dynamicRRTSetup(dynamic_cast<ModelBase*>(this));
- if (rrt->setup(options))
- planners[rrt->name] = rrt;
+ PlannerSetup *p = new _T(dynamic_cast<ModelBase*>(this));
+ if (p->setup(options))
+ planners[p->name + "[" + options->getName() + "]"] = p;
else
- delete rrt;
+ delete p;
}
-void ompl_planning::Model::add_kRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- PlannerSetup *rrt = new kinematicRRTSetup(dynamic_cast<ModelBase*>(this));
- if (rrt->setup(options))
- planners[rrt->name] = rrt;
- else
- delete rrt;
-}
-
-void ompl_planning::Model::add_kLazyRRT(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- PlannerSetup *rrt = new kinematicLazyRRTSetup(dynamic_cast<ModelBase*>(this));
- if (rrt->setup(options))
- planners[rrt->name] = rrt;
- else
- delete rrt;
-}
-
-void ompl_planning::Model::add_kEST(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- PlannerSetup *est = new kinematicESTSetup(dynamic_cast<ModelBase*>(this));
- if (est->setup(options))
- planners[est->name] = est;
- else
- delete est;
-}
-
-void ompl_planning::Model::add_kSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- PlannerSetup *sbl = new kinematicSBLSetup(dynamic_cast<ModelBase*>(this));
- if (sbl->setup(options))
- planners[sbl->name] = sbl;
- else
- delete sbl;
-}
-
-void ompl_planning::Model::add_kIKSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- PlannerSetup *sbl = new kinematicIKSBLSetup(dynamic_cast<ModelBase*>(this));
- if (sbl->setup(options))
- planners[sbl->name] = sbl;
- else
- delete sbl;
-}
-
-void ompl_planning::Model::add_kKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- PlannerSetup *kpiece = new kinematicKPIECESetup(dynamic_cast<ModelBase*>(this));
- if (kpiece->setup(options))
- planners[kpiece->name] = kpiece;
- else
- delete kpiece;
-}
-
-void ompl_planning::Model::add_dKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- PlannerSetup *kpiece = new dynamicKPIECESetup(dynamic_cast<ModelBase*>(this));
- if (kpiece->setup(options))
- planners[kpiece->name] = kpiece;
- else
- delete kpiece;
-}
-
-void ompl_planning::Model::add_kLBKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
-{
- PlannerSetup *kpiece = new kinematicLBKPIECESetup(dynamic_cast<ModelBase*>(this));
- if (kpiece->setup(options))
- planners[kpiece->name] = kpiece;
- else
- delete kpiece;
-}
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-02 18:37:50 UTC (rev 18201)
@@ -68,7 +68,14 @@
}
/* check if desired planner exists */
- std::map<std::string, PlannerSetup*>::iterator plannerIt = m->planners.find(req.params.planner_id);
+ std::map<std::string, PlannerSetup*>::iterator plannerIt = m->planners.end();
+ for (std::map<std::string, PlannerSetup*>::iterator it = m->planners.begin() ; it != m->planners.end() ; ++it)
+ if (it->first.find(req.params.planner_id) != std::string::npos)
+ {
+ plannerIt = it;
+ break;
+ }
+
if (plannerIt == m->planners.end())
{
ROS_ERROR("Motion planner not found: '%s'", req.params.planner_id.c_str());
@@ -201,14 +208,14 @@
if (!isRequestValid(models, req))
return false;
- ROS_INFO("Selected motion planner: '%s'", req.params.planner_id.c_str());
-
// find the data we need
Model *m = models[req.params.model_id];
// get the planner setup
PlannerSetup *psetup = m->planners[req.params.planner_id];
-
+
+ ROS_INFO("Selected motion planner: '%s', with priority %d", req.params.planner_id.c_str(), psetup->priority);
+
psetup->model->collisionSpace->lock();
psetup->model->kmodel->lock();
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-07-02 18:37:50 UTC (rev 18201)
@@ -68,7 +68,7 @@
{
}
- std::string getName(void);
+ const std::string& getName(void);
bool hasParam(const std::string ¶m);
std::string getParamString(const std::string ¶m, const std::string &def = "");
double getParamDouble(const std::string ¶m, double def);
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-07-02 18:36:58 UTC (rev 18200)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-07-02 18:37:50 UTC (rev 18201)
@@ -160,7 +160,7 @@
}
}
-std::string planning_environment::RobotModels::PlannerConfig::getName(void)
+const std::string& planning_environment::RobotModels::PlannerConfig::getName(void)
{
return config_;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-07-02 18:43:28
|
Revision: 18203
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18203&view=rev
Author: rdiankov
Date: 2009-07-02 18:42:25 +0000 (Thu, 02 Jul 2009)
Log Message:
-----------
update openrave and sensor_msgs octave scripts
Modified Paths:
--------------
pkg/trunk/openrave_planning/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/src/session.h
Added Paths:
-----------
pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_processImage.m
pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_writeImageMsg.m
Removed Paths:
-------------
pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_processImage.m
pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_writeImageMsg.m
Modified: pkg/trunk/openrave_planning/openrave/Makefile
===================================================================
--- pkg/trunk/openrave_planning/openrave/Makefile 2009-07-02 18:38:44 UTC (rev 18202)
+++ pkg/trunk/openrave_planning/openrave/Makefile 2009-07-02 18:42:25 UTC (rev 18203)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 815
+SVN_REVISION = -r 821
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2009-07-02 18:38:44 UTC (rev 18202)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2009-07-02 18:42:25 UTC (rev 18203)
@@ -437,10 +437,10 @@
};
// check that message constants match OpenRAVE constants
-BOOST_STATIC_ASSERT(EnvironmentBase::Clone_Bodies==openrave_session::Request::CloneBodies);
-BOOST_STATIC_ASSERT(EnvironmentBase::Clone_Viewer==openrave_session::Request::CloneViewer);
-BOOST_STATIC_ASSERT(EnvironmentBase::Clone_Simulation==openrave_session::Request::CloneSimulation);
-BOOST_STATIC_ASSERT(EnvironmentBase::Clone_RealControllers==openrave_session::Request::CloneRealControllers);
+BOOST_STATIC_ASSERT(Clone_Bodies==openrave_session::Request::CloneBodies);
+BOOST_STATIC_ASSERT(Clone_Viewer==openrave_session::Request::CloneViewer);
+BOOST_STATIC_ASSERT(Clone_Simulation==openrave_session::Request::CloneSimulation);
+BOOST_STATIC_ASSERT(Clone_RealControllers==openrave_session::Request::CloneRealControllers);
BOOST_STATIC_ASSERT(ActiveDOFs::DOF_X==RobotBase::DOF_X);
BOOST_STATIC_ASSERT(ActiveDOFs::DOF_Y==RobotBase::DOF_Y);
Deleted: pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_processImage.m
===================================================================
--- pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_processImage.m 2009-07-02 18:38:44 UTC (rev 18202)
+++ pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_processImage.m 2009-07-02 18:42:25 UTC (rev 18203)
@@ -1,14 +0,0 @@
-%% I = image_msgs_processImage(image_msg)
-%%
-%% returns an image matrix given a image_msgs/Image
-function I = image_msgs_processImage(image_msg)
-%res.camimage_msg.layout
-%I = zeros([data.height data.width 3]);
-layout = eval(sprintf('image_msg.%s_data.layout',image_msg.depth));
-dimsizes = [];
-for i = 1:length(layout.dim)
- dimsizes(i) = layout.dim{i}.size;
-end
-I = reshape(eval(sprintf('image_msg.%s_data.data',image_msg.depth)),dimsizes(end:-1:1));
-%% reverse the dimension order
-I = permute(I,length(dimsizes):-1:1);
Deleted: pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_writeImageMsg.m
===================================================================
--- pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_writeImageMsg.m 2009-07-02 18:38:44 UTC (rev 18202)
+++ pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_writeImageMsg.m 2009-07-02 18:42:25 UTC (rev 18203)
@@ -1,39 +0,0 @@
-%% image_msg = image_msgs_processImage(I)
-%%
-%% returns an image matrix given a image_msgs/Image
-%% If I is in integer format, assumes range for each channel is [0,255]
-%% otherwise range for each channel is [0,1]
-function image_msg = image_msgs_writeImageMsg(I)
-
-image_msg = image_msgs_Image();
-image_msg.label = 'OctaveImage';
-image_msg.depth = 'uint8';
-
-if( size(I,3) == 1 )
- image_msg.encoding = 'mono';
-elseif ( size(I,3) == 3 )
- image_msg.encoding = 'rgb';
-else
- error('unrecognized image format');
-end
-
-if( ~isinteger(I) )
- I = I*255;
-end
-
-s = size(I);
-dim = cell(3,1);
-dim{1} = std_msgs_MultiArrayDimension();
-dim{1}.label = 'height';
-dim{1}.size = s(1);
-dim{1}.stride = s(1)*s(2)*s(3);
-dim{2} = std_msgs_MultiArrayDimension();
-dim{2}.label = 'width';
-dim{2}.size = s(2);
-dim{2}.stride = s(2)*s(3);
-dim{3} = std_msgs_MultiArrayDimension();
-dim{3}.label = 'channel';
-dim{3}.size = s(3);
-dim{3}.stride = s(3);
-image_msg.uint8_data.layout.dim = dim;
-image_msg.uint8_data.data = reshape(permute(uint8(I),length(s):-1:1),[prod(s) 1]); %% reverse the dimension order
Copied: pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_processImage.m (from rev 18187, pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_processImage.m)
===================================================================
--- pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_processImage.m (rev 0)
+++ pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_processImage.m 2009-07-02 18:42:25 UTC (rev 18203)
@@ -0,0 +1,14 @@
+%% I = image_msgs_processImage(image_msg)
+%%
+%% returns an image matrix given a image_msgs/Image
+function I = image_msgs_processImage(image_msg)
+%res.camimage_msg.layout
+%I = zeros([data.height data.width 3]);
+layout = eval(sprintf('image_msg.%s_data.layout',image_msg.depth));
+dimsizes = [];
+for i = 1:length(layout.dim)
+ dimsizes(i) = layout.dim{i}.size;
+end
+I = reshape(eval(sprintf('image_msg.%s_data.data',image_msg.depth)),dimsizes(end:-1:1));
+%% reverse the dimension order
+I = permute(I,length(dimsizes):-1:1);
Property changes on: pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_processImage.m
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/common/sensor_msgs/octave/image_msgs_processImage.m:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_writeImageMsg.m (from rev 18187, pkg/trunk/stacks/common_msgs/sensor_msgs/octave/image_msgs_writeImageMsg.m)
===================================================================
--- pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_writeImageMsg.m (rev 0)
+++ pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_writeImageMsg.m 2009-07-02 18:42:25 UTC (rev 18203)
@@ -0,0 +1,39 @@
+%% image_msg = image_msgs_processImage(I)
+%%
+%% returns an image matrix given a image_msgs/Image
+%% If I is in integer format, assumes range for each channel is [0,255]
+%% otherwise range for each channel is [0,1]
+function image_msg = image_msgs_writeImageMsg(I)
+
+image_msg = image_msgs_Image();
+image_msg.label = 'OctaveImage';
+image_msg.depth = 'uint8';
+
+if( size(I,3) == 1 )
+ image_msg.encoding = 'mono';
+elseif ( size(I,3) == 3 )
+ image_msg.encoding = 'rgb';
+else
+ error('unrecognized image format');
+end
+
+if( ~isinteger(I) )
+ I = I*255;
+end
+
+s = size(I);
+dim = cell(3,1);
+dim{1} = std_msgs_MultiArrayDimension();
+dim{1}.label = 'height';
+dim{1}.size = s(1);
+dim{1}.stride = s(1)*s(2)*s(3);
+dim{2} = std_msgs_MultiArrayDimension();
+dim{2}.label = 'width';
+dim{2}.size = s(2);
+dim{2}.stride = s(2)*s(3);
+dim{3} = std_msgs_MultiArrayDimension();
+dim{3}.label = 'channel';
+dim{3}.size = s(3);
+dim{3}.stride = s(3);
+image_msg.uint8_data.layout.dim = dim;
+image_msg.uint8_data.data = reshape(permute(uint8(I),length(s):-1:1),[prod(s) 1]); %% reverse the dimension order
Property changes on: pkg/trunk/stacks/common_msgs/sensor_msgs/octave/sensor_msgs_writeImageMsg.m
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/common/sensor_msgs/octave/image_msgs_writeImageMsg.m:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-07-02 21:37:33
|
Revision: 18230
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18230&view=rev
Author: tfoote
Date: 2009-07-02 21:27:56 +0000 (Thu, 02 Jul 2009)
Log Message:
-----------
taking care of #1704
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/src/add_calibration_data.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
pkg/trunk/demos/plug_in/src/servo.cpp
pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_tongs.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_grasp_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_push_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_release_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_touch_door.cpp
pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in2.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp
pkg/trunk/highlevel/plugs/plugs_functions/src/plugs_functions.cpp
pkg/trunk/highlevel/writing/writing_test/src/test_move_to_helper.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp
pkg/trunk/motion_planning/planning_research/sbpl_arm_executive/src/pr2_arm_node.cpp
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/src/sbpl_door_planner_action.cpp
pkg/trunk/nav/people_aware_nav/src/is_person_on_path.cpp
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/nav/wavefront/src/cli.cpp
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/sandbox/descriptors_3d/.build_version
pkg/trunk/sandbox/descriptors_3d/.rosgcov_files
pkg/trunk/sandbox/labeled_object_detector/src/planar_object_detector.cpp
pkg/trunk/sandbox/poseviz/src/poseviz.cpp
pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation.cpp
pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/stacks/geometry/tf/include/tf/transform_datatypes.h
pkg/trunk/stacks/geometry/tf/src/change_notifier.cpp
pkg/trunk/stacks/geometry/tf/src/tf.cpp
pkg/trunk/stacks/geometry/tf/src/transform_broadcaster.cpp
pkg/trunk/stacks/geometry/tf/src/transform_listener.cpp
pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp
pkg/trunk/stacks/geometry/tf_tutorials/src/shuttle_observer.cpp
pkg/trunk/stacks/geometry/tf_tutorials/src/tutorial_listener.cpp
pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc
pkg/trunk/stacks/mechanism/robot_state_publisher/src/robot_state_publisher.cpp
pkg/trunk/stacks/nav/amcl/src/amcl_node.cpp
pkg/trunk/stacks/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/stacks/nav/fake_localization/fake_localization.cpp
pkg/trunk/stacks/nav/move_base/src/move_base.cpp
pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/stacks/nav/nav_view/src/nav_view/tools.cpp
pkg/trunk/stacks/nav/navfn/src/navfn_ros.cpp
pkg/trunk/stacks/semantic_mapping/door_handle_detector/test/door_checkerboard_detector.cpp
pkg/trunk/stacks/semantic_mapping/door_handle_detector/test/test_door_detection.cpp
pkg/trunk/stacks/semantic_mapping/door_handle_detector/test/test_door_detection_joystick.cpp
pkg/trunk/stacks/trex/trex_pr2/src/pr2_components.cpp
pkg/trunk/stacks/trex/trex_pr2/src/topological_map.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/calibrate_plug.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_tracker.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_util.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/plug_tracker.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/tracker_base.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/particle_cloud_2d_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/tools/pose_tool.cpp
pkg/trunk/util/pr2_ik/src/pr2_ik_controller.cpp
pkg/trunk/util/pr2_ik/src/pr2_ik_node.cpp
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/filter/people_tracking_node.cpp
pkg/trunk/vision/people/src/leg_detector.cpp
pkg/trunk/vision/people/src/stereo_color_tracker.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
Modified: pkg/trunk/calibration/kinematic_calibration/src/add_calibration_data.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/add_calibration_data.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/calibration/kinematic_calibration/src/add_calibration_data.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -123,15 +123,15 @@
list< pair<string, double> > calib_params ;
tf::Transform stereo_final_T ;
- tf::TransformMsgToTF(robot_cal_.stereo_final, stereo_final_T) ;
+ tf::transformMsgToTF(robot_cal_.stereo_final, stereo_final_T) ;
getCamCorrection(calib_params, robot_, stereo_final_T, "stereo") ;
tf::Transform high_res_final_T ;
- tf::TransformMsgToTF(robot_cal_.high_res_final, high_res_final_T) ;
+ tf::transformMsgToTF(robot_cal_.high_res_final, high_res_final_T) ;
getCamCorrection(calib_params, robot_, high_res_final_T, "high_res") ;
tf::Transform head_initial_T ;
- tf::TransformMsgToTF(robot_cal_.head_initial, head_initial_T) ;
+ tf::transformMsgToTF(robot_cal_.head_initial, head_initial_T) ;
getHeadInitialCorrection(calib_params, robot_, head_initial_T) ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -159,25 +159,25 @@
// Laser Chain
msg.transforms[0].parent_id = "torso_lift_link" ;
msg.transforms[0].header.frame_id = "laser_tilt_mount_link_cal" ;
- tf::TransformTFToMsg(laser_initial_T_*laser_joint_T, msg.transforms[0].transform) ;
+ tf::transformTFToMsg(laser_initial_T_*laser_joint_T, msg.transforms[0].transform) ;
msg.transforms[1].parent_id = "laser_tilt_mount_link_cal" ;
msg.transforms[1].header.frame_id = "laser_tilt_link_cal" ;
- tf::TransformTFToMsg(laser_final_T_, msg.transforms[1].transform) ;
+ tf::transformTFToMsg(laser_final_T_, msg.transforms[1].transform) ;
// Head Chain
msg.transforms[2].parent_id = "torso_lift_link" ;
msg.transforms[2].header.frame_id = "head_pan_link_cal" ;
- tf::TransformTFToMsg(head_initial_T_*pan_joint_T, msg.transforms[2].transform) ;
+ tf::transformTFToMsg(head_initial_T_*pan_joint_T, msg.transforms[2].transform) ;
msg.transforms[3].parent_id = "head_pan_link_cal" ;
msg.transforms[3].header.frame_id = "head_tilt_link_cal" ;
- //tf::TransformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
- tf::TransformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
+ //tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
+ tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
msg.transforms[4].parent_id = "head_tilt_link_cal" ;
msg.transforms[4].header.frame_id = "stereo_optical_frame_cal" ;
- tf::TransformTFToMsg(after_tilt_T_, msg.transforms[4].transform) ;
+ tf::transformTFToMsg(after_tilt_T_, msg.transforms[4].transform) ;
tf_pub_.publish(msg) ;
//printf("done!\n") ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -163,25 +163,25 @@
// Laser Chain
msg.transforms[0].parent_id = "torso_lift_link" ;
msg.transforms[0].header.frame_id = "laser_tilt_mount_link_cal" ;
- tf::TransformTFToMsg(laser_initial_T_*laser_joint_T, msg.transforms[0].transform) ;
+ tf::transformTFToMsg(laser_initial_T_*laser_joint_T, msg.transforms[0].transform) ;
msg.transforms[1].parent_id = "laser_tilt_mount_link_cal" ;
msg.transforms[1].header.frame_id = "laser_tilt_link_cal" ;
- tf::TransformTFToMsg(laser_final_T_, msg.transforms[1].transform) ;
+ tf::transformTFToMsg(laser_final_T_, msg.transforms[1].transform) ;
// Head Chain
msg.transforms[2].parent_id = "torso_lift_link" ;
msg.transforms[2].header.frame_id = "head_pan_link_cal" ;
- tf::TransformTFToMsg(head_initial_T_*pan_joint_T, msg.transforms[2].transform) ;
+ tf::transformTFToMsg(head_initial_T_*pan_joint_T, msg.transforms[2].transform) ;
msg.transforms[3].parent_id = "head_pan_link_cal" ;
msg.transforms[3].header.frame_id = "head_tilt_link_cal" ;
- //tf::TransformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
- tf::TransformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
+ //tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
+ tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
msg.transforms[4].parent_id = "head_tilt_link_cal" ;
msg.transforms[4].header.frame_id = "stereo_optical_frame_cal" ;
- tf::TransformTFToMsg(after_tilt_T_, msg.transforms[4].transform) ;
+ tf::transformTFToMsg(after_tilt_T_, msg.transforms[4].transform) ;
tf_pub_.publish(msg) ;
//printf("done!\n") ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -162,25 +162,25 @@
// Laser Chain
msg.transforms[0].parent_id = "torso_lift_link" ;
msg.transforms[0].header.frame_id = "laser_tilt_mount_link_cal" ;
- tf::TransformTFToMsg(laser_initial_T_*laser_joint_T, msg.transforms[0].transform) ;
+ tf::transformTFToMsg(laser_initial_T_*laser_joint_T, msg.transforms[0].transform) ;
msg.transforms[1].parent_id = "laser_tilt_mount_link_cal" ;
msg.transforms[1].header.frame_id = "laser_tilt_link_cal" ;
- tf::TransformTFToMsg(laser_final_T_, msg.transforms[1].transform) ;
+ tf::transformTFToMsg(laser_final_T_, msg.transforms[1].transform) ;
// Head Chain
msg.transforms[2].parent_id = "torso_lift_link" ;
msg.transforms[2].header.frame_id = "head_pan_link_cal" ;
- tf::TransformTFToMsg(head_initial_T_*pan_joint_T, msg.transforms[2].transform) ;
+ tf::transformTFToMsg(head_initial_T_*pan_joint_T, msg.transforms[2].transform) ;
msg.transforms[3].parent_id = "head_pan_link_cal" ;
msg.transforms[3].header.frame_id = "head_tilt_link_cal" ;
- //tf::TransformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
- tf::TransformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
+ //tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
+ tf::transformTFToMsg(after_pan_T_*tilt_joint_T, msg.transforms[3].transform) ;
msg.transforms[4].parent_id = "head_tilt_link_cal" ;
msg.transforms[4].header.frame_id = "stereo_optical_frame_cal" ;
- tf::TransformTFToMsg(after_tilt_T_, msg.transforms[4].transform) ;
+ tf::transformTFToMsg(after_tilt_T_, msg.transforms[4].transform) ;
tf_pub_.publish(msg) ;
//printf("done!\n") ;
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -91,7 +91,7 @@
{
_imu_mutex.lock();
double tmp, yaw; Transform tf;
- PoseMsgToTF(_imu.pos, tf);
+ poseMsgToTF(_imu.pos, tf);
tf.getBasis().getEulerZYX(yaw, tmp, tmp);
if (!_imu_active){
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -164,7 +164,7 @@
tf_.transformPose(odom_frame_name_, cmd, pose_odom) ;
tf::Quaternion orientation_odom ; // Orientation in the odometric frame
- tf::QuaternionMsgToTF(pose_odom.pose.orientation, orientation_odom) ;
+ tf::quaternionMsgToTF(pose_odom.pose.orientation, orientation_odom) ;
// For convenience, use 1-Letter names for quaternion terms
const double& w = orientation_odom.w() ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -382,7 +382,7 @@
string mount_link_name = "laser_tilt_mount_link" ;
target_link_ = robot_->getLinkState(track_link_cmd.link_name) ;
mount_link_ = robot_->getLinkState(mount_link_name) ;
- tf::PointMsgToTF(track_link_cmd.point, track_point_) ;
+ tf::pointMsgToTF(track_link_cmd.point, track_point_) ;
if (target_link_ == NULL)
{
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -51,7 +51,7 @@
{
tf::Transform tf;
tf::TransformKDLToTF(k, tf);
- tf::PoseTFToMsg(tf, m);
+ tf::poseTFToMsg(tf, m);
}
void TwistKDLToMsg(const KDL::Twist &k, robot_msgs::Twist &m)
@@ -545,7 +545,7 @@
pub_tf_->msg_.transforms[0].parent_id = c_.chain_.getLinkName();
tf::Transform t;
tf::TransformKDLToTF(c_.tool_frame_offset_, t);
- tf::TransformTFToMsg(t, pub_tf_->msg_.transforms[0].transform);
+ tf::transformTFToMsg(t, pub_tf_->msg_.transforms[0].transform);
pub_tf_->unlockAndPublish();
}
}
@@ -608,7 +608,7 @@
robot_msgs::PoseStamped tool_in_tip_msg;
tf::Transform tool_in_tip;
TF.transformPose(c_.chain_.getLinkName(-1), req.p, tool_in_tip_msg);
- tf::PoseMsgToTF(tool_in_tip_msg.pose, tool_in_tip);
+ tf::poseMsgToTF(tool_in_tip_msg.pose, tool_in_tip);
tool_in_tip.setOrigin(tf::Vector3(0,0,0));
tf::TransformTFToKDL(tool_in_tip, c_.tool_frame_offset_);
double rpy[3]; c_.tool_frame_offset_.M.GetRPY(rpy[0], rpy[1], rpy[2]);
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -188,7 +188,7 @@
if (state_pose_publisher_->trylock()){
Pose tmp;
frameToPose(pose_meas_, tmp);
- PoseStampedTFToMsg(Stamped<Pose>(tmp, ros::Time::now(), root_name_), state_pose_publisher_->msg_);
+ poseStampedTFToMsg(Stamped<Pose>(tmp, ros::Time::now(), root_name_), state_pose_publisher_->msg_);
state_pose_publisher_->unlockAndPublish();
}
}
@@ -213,7 +213,7 @@
{
// convert message to transform
Stamped<Pose> pose_stamped;
- PoseStampedMsgToTF(*pose_msg, pose_stamped);
+ poseStampedMsgToTF(*pose_msg, pose_stamped);
// convert to reference frame of root link of the controller chain
tf_.transformPose(root_name_, pose_stamped, pose_stamped);
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -147,7 +147,7 @@
// convert message to transform
Stamped<Pose> pose_stamped;
- PoseStampedMsgToTF(pose, pose_stamped);
+ poseStampedMsgToTF(pose, pose_stamped);
// convert to reference frame of root link of the controller chain
Duration timeout = Duration().fromSec(2.0);
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -417,7 +417,7 @@
{
tf::Transform transform;
tf::TransformKDLToTF(controller_.endeffector_frame_, transform);
- tf::TransformTFToMsg(transform, current_frame_publisher_->msg_);
+ tf::transformTFToMsg(transform, current_frame_publisher_->msg_);
current_frame_publisher_->unlockAndPublish() ;
}
if (internal_state_publisher_->trylock())
@@ -449,7 +449,7 @@
return;
}
tf::Pose outlet;
- tf::PoseMsgToTF(outlet_in_root_.pose, outlet);
+ tf::poseMsgToTF(outlet_in_root_.pose, outlet);
controller_.outlet_pt_(0) = outlet.getOrigin().x();
controller_.outlet_pt_(1) = outlet.getOrigin().y();
@@ -490,7 +490,7 @@
}
tf::Transform tool_offset;
- tf::PoseMsgToTF(tool_offset_msg.pose, tool_offset);
+ tf::poseMsgToTF(tool_offset_msg.pose, tool_offset);
controller_.setToolOffset(tool_offset);
return true;
}
Modified: pkg/trunk/demos/plug_in/src/servo.cpp
===================================================================
--- pkg/trunk/demos/plug_in/src/servo.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/demos/plug_in/src/servo.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -84,7 +84,7 @@
}
tf::Pose viz_offset;
- tf::PoseMsgToTF(viz_offset_msg.pose, viz_offset);
+ tf::poseMsgToTF(viz_offset_msg.pose, viz_offset);
static double last_standoff = 1.0e10;
double standoff = std::max(MIN_STANDOFF, viz_offset.getOrigin().length() * 2.5/4.0);
Modified: pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp
===================================================================
--- pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -98,7 +98,7 @@
catch(tf::TransformException e) {
continue;
}
- tf::PoseStampedTFToMsg(transform, scan_annotated.poses[i]) ;
+ tf::poseStampedTFToMsg(transform, scan_annotated.poses[i]) ;
}
publish("scan_annotated", scan_annotated);
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -105,7 +105,7 @@
// Build Transform Message
tf::Transform mytf;
- tf::TransformMsgToTF(body.pose,mytf);
+ tf::transformMsgToTF(body.pose,mytf);
if (publish_transform_)
m_tfServer->sendTransform(mytf,m_currentPos.header.stamp,"base","map");
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -138,7 +138,7 @@
// Define our starting frame
tf::Quaternion rot_phasespace ;
- tf::QuaternionMsgToTF(cur_body.pose.rotation, rot_phasespace) ;
+ tf::quaternionMsgToTF(cur_body.pose.rotation, rot_phasespace) ;
tf::Point trans_phasespace(cur_body.pose.translation.x*scale_trans_[0],
cur_body.pose.translation.y*scale_trans_[1],
cur_body.pose.translation.z*scale_trans_[2]) ;
@@ -152,8 +152,8 @@
pose_msg.header.stamp = ros::Time() ;
pose_msg.header.frame_id = frame_id_ ;
- tf::PointTFToMsg(pose_result.getOrigin(), pose_msg.pose.position) ;
- tf::QuaternionTFToMsg(pose_result.getRotation(), pose_msg.pose.orientation) ;
+ tf::pointTFToMsg(pose_result.getOrigin(), pose_msg.pose.position) ;
+ tf::quaternionTFToMsg(pose_result.getRotation(), pose_msg.pose.orientation) ;
pose_msg.header.stamp = ros::Time::now() - ros::Duration(.5) ;
publish("cmd", pose_msg) ;
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_tongs.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_tongs.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_tongs.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -109,12 +109,12 @@
if (snapshot_.bodies[i].id == left_id_)
{
left_found = true ;
- tf::TransformMsgToTF(snapshot_.bodies[i].pose, left) ;
+ tf::transformMsgToTF(snapshot_.bodies[i].pose, left) ;
}
if (snapshot_.bodies[i].id == right_id_)
{
right_found = true ;
- tf::TransformMsgToTF(snapshot_.bodies[i].pose, right) ;
+ tf::transformMsgToTF(snapshot_.bodies[i].pose, right) ;
}
}
@@ -135,7 +135,7 @@
tf::Stamped<tf::Pose> tf_pose(left, snapshot_.header.stamp, frame_id_) ;
robot_msgs::PoseStamped pose_msg ;
- tf::PoseStampedTFToMsg(tf_pose, pose_msg) ;
+ tf::poseStampedTFToMsg(tf_pose, pose_msg) ;
node_->publish("tong_spacing", tong_spacing) ;
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_grasp_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_grasp_handle.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_grasp_handle.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -99,7 +99,7 @@
gripper_pose.setOrigin( Vector3(handle(0) + (normal(0) * -0.15), handle(1) + (normal(1) * -0.15), handle(2) + (normal(2) * -0.15)));
gripper_pose.setRotation( Quaternion(getVectorAngle(x_axis, normal), 0, M_PI/2.0) );
gripper_pose.stamp_ = Time::now();
- PoseStampedTFToMsg(gripper_pose, req_moveto.pose);
+ poseStampedTFToMsg(gripper_pose, req_moveto.pose);
ROS_INFO("GraspHandleAction: move in front of handle");
if (!ros::service::call("r_arm_constraint_cartesian_trajectory_controller/move_to", req_moveto, res_moveto)){
@@ -119,7 +119,7 @@
gripper_pose.setOrigin( Vector3(handle(0) + (normal(0) * 0.05 ), handle(1) + (normal(1) * 0.05), handle(2) + (normal(2) * 0.05)));
gripper_pose.setRotation( Quaternion(getVectorAngle(x_axis, normal), 0, M_PI/2.0) );
gripper_pose.stamp_ = Time::now();
- PoseStampedTFToMsg(gripper_pose, req_moveto.pose);
+ poseStampedTFToMsg(gripper_pose, req_moveto.pose);
//req_moveto.tolerance.vel.x = 0.1;
//req_moveto.tolerance.vel.y = 0.1;
//req_moveto.tolerance.vel.z = 0.1;
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_push_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_push_door.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_push_door.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -122,7 +122,7 @@
// define griper pose
gripper_pose = getGripperPose(goal_tr, angle, push_dist);
gripper_pose.stamp_ = Time::now();
- PoseStampedTFToMsg(gripper_pose, gripper_pose_msg);
+ poseStampedTFToMsg(gripper_pose, gripper_pose_msg);
pose_pub_.publish(gripper_pose_msg);
// increase angle when pose error is small enough
@@ -138,7 +138,7 @@
void PushDoorAction::poseCallback(const PoseConstPtr& pose)
{
boost::mutex::scoped_lock lock(pose_mutex_);
- PoseStampedMsgToTF(*pose, pose_state_);
+ poseStampedMsgToTF(*pose, pose_state_);
pose_state_received_ = true;
}
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_release_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_release_handle.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_release_handle.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -101,7 +101,7 @@
// move gripper away from the door
Pose offset(Quaternion(0,0,0), Vector3(-0.2,0,0));
Pose gripper_goal = gripper_pose_ * offset;
- PoseStampedTFToMsg(Stamped<Pose>(gripper_goal, Time::now(), gripper_pose_.frame_id_), req_moveto.pose);
+ poseStampedTFToMsg(Stamped<Pose>(gripper_goal, Time::now(), gripper_pose_.frame_id_), req_moveto.pose);
ROS_INFO("ReleaseHandleAction: move gripper away from door ");
if (!ros::service::call("r_arm_constraint_cartesian_trajectory_controller/move_to", req_moveto, res_moveto)){
@@ -123,7 +123,7 @@
void ReleaseHandleAction::poseCallback(const PoseConstPtr& pose)
{
boost::mutex::scoped_lock lock(pose_mutex_);
- PoseStampedMsgToTF(*pose, gripper_pose_);
+ poseStampedMsgToTF(*pose, gripper_pose_);
pose_received_ = true;
}
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_touch_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_touch_door.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_touch_door.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -99,7 +99,7 @@
//}
Stamped<Pose> gripper_pose = getGripperPose(goal_tr, getNearestDoorAngle(shoulder_pose, goal_tr, 0.75, touch_dist), touch_dist);
gripper_pose.stamp_ = Time::now();
- PoseStampedTFToMsg(gripper_pose, req_moveto.pose);
+ poseStampedTFToMsg(gripper_pose, req_moveto.pose);
//req_moveto.tolerance.vel.x = 0.1;
//req_moveto.tolerance.vel.y = 0.1;
//req_moveto.tolerance.vel.z = 0.1;
Modified: pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-07-02 21:24:43 UTC (rev 18229)
+++ pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-07-02 21:27:56 UTC (rev 18230)
@@ -157,7 +157,7 @@
// approach door
robot_msgs::PoseStamped goal_msg;
- tf::PoseStampedTFToMsg(getRobotPose(res_detect_door, -0.6), goal_msg);
+ tf::poseStampedTFToMsg(getRobotPose(res_detect_door, -0.6), goal_msg);
cout << "move to pose " << goal_msg.pose.position.x << ", " << goal_msg.pose.position.y << ", "<< goal_msg.pose.position.z << endl;
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
Modified: pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp
===================================================...
[truncated message content] |
|
From: <hsu...@us...> - 2009-07-02 22:22:01
|
Revision: 18231
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18231&view=rev
Author: hsujohnhsu
Date: 2009-07-02 22:21:59 +0000 (Thu, 02 Jul 2009)
Log Message:
-----------
moving pr2 meshes out of gazebo_robot_description into pr2_ogre.
removing dependencies in gazebo_robot_description.
updating gazebo world launch scripts.
updating some build test launch scripts.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.launch
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml
pkg/trunk/stacks/simulators/gazebo/launch/balcony_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/empty_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/empty_world_no_x.launch
pkg/trunk/stacks/simulators/gazebo/launch/office_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/simple_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/slide_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/wg_world.launch
Added Paths:
-----------
pkg/trunk/stacks/simulators/gazebo/launch/camera_world.launch
pkg/trunk/stacks/simulators/gazebo/launch/scan_world.launch
Removed Paths:
-------------
pkg/trunk/robot_descriptions/gazebo_robot_description/Media/materials/scripts/pr2.material
Modified: pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/manifest.xml 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/manifest.xml 2009-07-02 22:21:59 UTC (rev 18231)
@@ -8,6 +8,7 @@
<url>http://pr.willowgarage.com/wiki/test_pr2_collision_gazebo</url>
<depend package="gazebo_plugin"/>
<depend package="pr2_gazebo" />
+ <depend package="pr2_ogre" />
<depend package="gazebo_robot_description" />
<depend package="robot_msgs" />
</package>
Modified: pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.launch
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -1,6 +1,12 @@
<launch>
<include file="$(find gazebo)/launch/slide_world.launch" />
+ <!-- Robot state publisher -->
+ <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
+ <param name="publish_frequency" type="double" value="50.0" />
+ <param name="tf_prefix" type="string" value="" />
+ </node>
+
<!-- send single_link.xml to param server -->
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml 2009-07-02 22:21:59 UTC (rev 18231)
@@ -10,6 +10,7 @@
<depend package="arm_gazebo" />
<depend package="pr2_mechanism_controllers" />
<depend package="pr2_gazebo" />
+ <depend package="pr2_ogre" />
<depend package="gazebo_robot_description" />
<depend package="deprecated_msgs" />
<depend package="robot_msgs" />
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml 2009-07-02 22:21:59 UTC (rev 18231)
@@ -9,6 +9,7 @@
<depend package="gazebo_plugin"/>
<depend package="arm_gazebo" />
<depend package="pr2_gazebo" />
+ <depend package="pr2_ogre" />
<depend package="gazebo_robot_description" />
<depend package="laser_scan" />
<depend package="sensor_msgs" />
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.launch
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -1,11 +1,6 @@
<launch>
- <param name="/use_sim_time" value="true" />
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/camera.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
+ <include file="$(find gazebo)/launch/camera_world.launch" />
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.launch
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -1,11 +1,6 @@
<launch>
- <param name="/use_sim_time" value="true" />
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/scan.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
+ <include file="$(find gazebo)/launch/scan_world.launch" />
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2009-07-02 22:21:59 UTC (rev 18231)
@@ -2,60 +2,3 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(gazebo_robot_description)
-# find needed paths
-find_ros_package(pr2_defs)
-find_ros_package(gazebo_robot_description)
-find_ros_package(ogre)
-
-# build the ogre mesh files from *.stl (and *.stlb from convex decomposition)
-file(GLOB pr2_stl_files ${pr2_defs_PACKAGE_PATH}/meshes/*.stl ${pr2_defs_PACKAGE_PATH}/meshes/convex/*.stlb)
-set(pr2_gen_files "")
-
-set(pr2_out_path ${CMAKE_CURRENT_SOURCE_DIR}/Media/models/pr2)
-
-MAKE_DIRECTORY(${pr2_out_path})
-
-foreach(it ${pr2_stl_files})
- get_filename_component(basename ${it} NAME_WE)
-
- # convert to ogre files
- add_custom_command(
- OUTPUT ${pr2_out_path}/${basename}.mesh
- COMMAND rosrun
- ARGS ogre_tools stl_to_mesh ${it} ${pr2_out_path}/${basename}.mesh
- DEPENDS ${it})
-
- set(pr2_gen_files ${pr2_gen_files} ${pr2_out_path}/${basename}.mesh)
-endforeach(it)
-
-add_custom_target(media_files ALL DEPENDS ${pr2_gen_files})
-
-
-find_ros_package(ikea_objects)
-if(EXISTS ikea_objects_PACKAGE_PATH)
- # build the ogre mesh files from *.stl (and *.stlb from convex decomposition)
- file(GLOB ikea_objects_stl_files ${ikea_objects_PACKAGE_PATH}/meshes/convex/*_convex.stl)
- set(ikea_objects_gen_files "")
-
- set(ikea_objects_out_path ${CMAKE_CURRENT_SOURCE_DIR}/Media/models/ikea_objects)
-
- MAKE_DIRECTORY(${ikea_objects_out_path})
-
- foreach(it ${ikea_objects_stl_files})
- get_filename_component(basename ${it} NAME)
-
- # convert to ogre files
- add_custom_command(
- OUTPUT ${ikea_objects_out_path}/${basename}.mesh
- COMMAND rosrun
- ARGS ogre_tools stl_to_mesh ${it} ${ikea_objects_out_path}/${basename}.mesh
- COMMAND ${ogre_PACKAGE_PATH}/ogre/bin/OgreMeshUpgrade
- ARGS ${ikea_objects_out_path}/${basename}.mesh
- DEPENDS ${it})
-
- set(ikea_objects_gen_files ${ikea_objects_gen_files} ${ikea_objects_out_path}/${basename}.mesh)
- endforeach(it)
-
- add_custom_target(ikea_objects_media_files ALL DEPENDS ${ikea_objects_gen_files})
-endif(EXISTS ikea_objects_PACKAGE_PATH)
-
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/Media/materials/scripts/pr2.material
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/Media/materials/scripts/pr2.material 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/Media/materials/scripts/pr2.material 2009-07-02 22:21:59 UTC (rev 18231)
@@ -1,366 +0,0 @@
-material PR2/floor_texture
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture map3.png
- tex_address_mode clamp
- }
- }
- }
-}
-
-material PR2/wall_texture
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture willowMap.png
- tex_address_mode clamp
- }
- }
- }
-}
-
-material PR2/wheel_right
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_right.png
- }
- }
- }
-}
-
-material PR2/wheel_left
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_left.png
- }
- }
- }
-}
-
-
-
-material PR2/fr_caster_l_wheel_link
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_right.png
- }
- }
- }
-}
-
-material PR2/fl_caster_l_wheel_link
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_left.png
- }
- }
- }
-}
-material PR2/br_caster_l_wheel_link
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_right.png
- }
- }
- }
-}
-
-material PR2/bl_caster_l_wheel_link
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_left.png
- }
- }
- }
-}
-
-
-material PR2/fr_caster_r_wheel_link
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_right.png
- }
- }
- }
-}
-
-material PR2/fl_caster_r_wheel_link
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_left.png
- }
- }
- }
-}
-material PR2/br_caster_r_wheel_link
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_right.png
- }
- }
- }
-}
-
-material PR2/bl_caster_r_wheel_link
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_left.png
- }
- }
- }
-}
-
-
-material PR2/RollLinks
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture pr2_wheel_left.png
- }
- }
- }
-}
-
-material PR2/Shiny
-{
- technique
- {
- pass
- {
- ambient 0.75 0.75 0.75
- texture_unit
- {
- texture plug_texture.jpg
- env_map spherical
- }
- }
- }
-}
-
-material PR2/Plug
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture plug_texture.jpg
- }
- }
- }
-}
-
-material PR2/Jack
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture jack_texture.jpg
- }
- }
- }
-}
-
-material PR2/Outlet
-{
- technique
- {
- pass
- {
- texture_unit
- {
- texture outlet_texture.jpg
- }
- }
- }
-}
-
-
-material PR2/White
-{
- receive_shadows on
- lighting on
-
- technique
- {
- pass
- {
- ambient 0.5 0.5 0.5 1.0
- diffuse 1.0 1.0 1.0 1.0
- specular 1.0 1.0 1.0 1.0
- shading gouraud
- }
- }
-}
-
-
-
-material PR2/Blue
-{
- receive_shadows on
-
- technique
- {
- pass
- {
- ambient 0.000000 0.000000 0.200000 1.000000
- diffuse 0.000000 0.000000 0.800000 1.000000
- specular 0.000000 0.000000 0.200000 1.000000
- emissive 0.000000 0.000000 0.000000 1.000000
- shading gouraud
- }
- }
-}
-
-material PR2/Grey2
-{
- receive_shadows on
- lighting on
-
- technique
- {
- pass
- {
- ambient 0.5 0.5 0.5 1.0
- diffuse 0.9 0.9 0.9 1.0
- specular 0.8 0.8 0.8 1
- lighting on
- }
- }
-}
-
-material PR2/Grey
-{
- receive_shadows on
- lighting on
-
- technique
- {
- pass
- {
- ambient 0.1 0.1 0.1 1.0
- diffuse 0.7 0.7 0.7 1.0
- specular 0.8 0.8 0.8 1
- }
- }
-}
-
-material PR2/Yellow
-{
- receive_shadows on
-
- technique
- {
- pass
- {
- ambient 0.200000 0.200000 0.000000 1.000000
- diffuse 0.800000 0.800000 0.000000 1.000000
- specular 0.200000 0.200000 0.000000 1.000000
- emissive 0.000000 0.000000 0.000000 1.000000
- lighting on
- }
- }
-}
-
-material PR2/Red
-{
- receive_shadows on
- technique
- {
- pass
- {
- ambient 0.200000 0.000000 0.000000 1.000000
- diffuse 0.800000 0.000000 0.000000 1.000000
- specular 0.200000 0.000000 0.000000 1.000000
- emissive 0.000000 0.000000 0.000000 1.000000
- lighting on
- shading phong
- }
- }
-}
-
-material PR2/Green
-{
- receive_shadows on
-
- technique
- {
- pass
- {
- ambient 0.000000 0.200000 0.000000 1.000000
- diffuse 0.000000 0.800000 0.000000 1.000000
- specular 0.000000 0.200000 0.000000 1.000000
- emissive 0.000000 0.000000 0.000000 0.000000
- lighting on
- shading phong
- }
- }
-}
-
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml 2009-07-02 22:21:59 UTC (rev 18231)
@@ -5,8 +5,6 @@
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/FIXME</url>
- <depend package="wg_robot_description_parser"/>
<depend package="pr2_defs"/>
- <depend package="tf"/>
- <depend package="ogre_tools"/>
+ <depend package="pr2_ogre"/>
</package>
Modified: pkg/trunk/stacks/simulators/gazebo/launch/balcony_world.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/balcony_world.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/stacks/simulators/gazebo/launch/balcony_world.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -5,7 +5,7 @@
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/balcony.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
</node>
Added: pkg/trunk/stacks/simulators/gazebo/launch/camera_world.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/camera_world.launch (rev 0)
+++ pkg/trunk/stacks/simulators/gazebo/launch/camera_world.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -0,0 +1,12 @@
+<launch>
+
+ <!-- start gazebo with an camera world -->
+ <param name="/use_sim_time" value="true" />
+
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/camera.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ </node>
+
+</launch>
Modified: pkg/trunk/stacks/simulators/gazebo/launch/empty_world.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/empty_world.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/stacks/simulators/gazebo/launch/empty_world.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -5,7 +5,7 @@
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
</node>
Modified: pkg/trunk/stacks/simulators/gazebo/launch/empty_world_no_x.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/empty_world_no_x.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/stacks/simulators/gazebo/launch/empty_world_no_x.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -5,7 +5,7 @@
<node pkg="gazebo" launch-prefix="xvfb-run" type="gazebo" args="-u $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
</node>
Modified: pkg/trunk/stacks/simulators/gazebo/launch/office_world.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/office_world.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/stacks/simulators/gazebo/launch/office_world.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -5,7 +5,7 @@
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/simple_office.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
</node>
Added: pkg/trunk/stacks/simulators/gazebo/launch/scan_world.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/scan_world.launch (rev 0)
+++ pkg/trunk/stacks/simulators/gazebo/launch/scan_world.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -0,0 +1,12 @@
+<launch>
+
+ <!-- start gazebo with an scan world -->
+ <param name="/use_sim_time" value="true" />
+
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/scan.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ </node>
+
+</launch>
Modified: pkg/trunk/stacks/simulators/gazebo/launch/simple_world.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/simple_world.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/stacks/simulators/gazebo/launch/simple_world.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -5,7 +5,7 @@
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
</node>
Modified: pkg/trunk/stacks/simulators/gazebo/launch/slide_world.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/slide_world.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/stacks/simulators/gazebo/launch/slide_world.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -5,7 +5,7 @@
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/slide.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
</node>
Modified: pkg/trunk/stacks/simulators/gazebo/launch/wg_world.launch
===================================================================
--- pkg/trunk/stacks/simulators/gazebo/launch/wg_world.launch 2009-07-02 21:27:56 UTC (rev 18230)
+++ pkg/trunk/stacks/simulators/gazebo/launch/wg_world.launch 2009-07-02 22:21:59 UTC (rev 18231)
@@ -5,7 +5,7 @@
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find pr2_ogre):$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
</node>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-03 05:44:54
|
Revision: 18264
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18264&view=rev
Author: isucan
Date: 2009-07-03 05:44:47 +0000 (Fri, 03 Jul 2009)
Log Message:
-----------
using resource location
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/util/geometric_shapes/src/bodies.cpp
pkg/trunk/util/robot_self_filter/self_filter.launch
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-07-03 05:06:26 UTC (rev 18263)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-07-03 05:44:47 UTC (rev 18264)
@@ -336,7 +336,7 @@
const double* computeTransform(const double *params, int groupID);
/** \brief Extract the information needed by the joint given the URDF description */
- void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot, const robot_desc::URDF &model);
};
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-07-03 05:06:26 UTC (rev 18263)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-07-03 05:44:47 UTC (rev 18264)
@@ -227,7 +227,7 @@
robot->stateBounds.push_back(limit[1]);
}
-void planning_models::KinematicModel::Link::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+void planning_models::KinematicModel::Link::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot, const robot_desc::URDF &model)
{
/* compute the geometry for this link */
switch (urdfLink->collision->geometry->type)
@@ -259,9 +259,10 @@
break;
case robot_desc::URDF::Link::Geometry::MESH:
{
- std::string filename = static_cast<const robot_desc::URDF::Link::Geometry::Mesh*>(urdfLink->collision->geometry->shape)->filename;
+ std::string filename = model.getResourceLocation() + "/" + static_cast<const robot_desc::URDF::Link::Geometry::Mesh*>(urdfLink->collision->geometry->shape)->filename;
if (filename.rfind(".stl") == std::string::npos)
filename += ".stl";
+ std::cout << "Loading '" << filename << "'" << std::endl;
shapes::Mesh *mesh = shapes::create_mesh_from_binary_stl(filename.c_str());
shape = mesh;
}
@@ -572,7 +573,9 @@
link->owner = robot;
robot->links.push_back(link);
- link->extractInformation(urdfLink, robot);
+ link->extractInformation(urdfLink, robot, model);
+ if (link->shape == NULL)
+ m_msg.error("Unable to construct shape for link '%s'", link->name.c_str());
for (unsigned int i = 0 ; i < urdfLink->children.size() ; ++i)
{
Modified: pkg/trunk/util/geometric_shapes/src/bodies.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-03 05:06:26 UTC (rev 18263)
+++ pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-03 05:44:47 UTC (rev 18264)
@@ -43,24 +43,25 @@
{
Body *body = NULL;
- switch (shape->type)
- {
- case shapes::Shape::BOX:
- body = new bodies::Box(shape);
- break;
- case shapes::Shape::SPHERE:
- body = new bodies::Sphere(shape);
- break;
- case shapes::Shape::CYLINDER:
- body = new bodies::Cylinder(shape);
- break;
- case shapes::Shape::MESH:
- body = new bodies::ConvexMesh(shape);
- break;
- default:
- std::cerr << "Creating body from shape: Unknown shape type" << shape->type << std::endl;
- break;
- }
+ if (shape)
+ switch (shape->type)
+ {
+ case shapes::Shape::BOX:
+ body = new bodies::Box(shape);
+ break;
+ case shapes::Shape::SPHERE:
+ body = new bodies::Sphere(shape);
+ break;
+ case shapes::Shape::CYLINDER:
+ body = new bodies::Cylinder(shape);
+ break;
+ case shapes::Shape::MESH:
+ body = new bodies::ConvexMesh(shape);
+ break;
+ default:
+ std::cerr << "Creating body from shape: Unknown shape type" << shape->type << std::endl;
+ break;
+ }
return body;
}
@@ -315,7 +316,7 @@
}
if (behindPlane > 0)
- std::cerr << "Approximate plane: " << behindPlane << " of " << m_vertices.size() << " points are behind the plane";
+ std::cerr << "Approximate plane: " << behindPlane << " of " << m_vertices.size() << " points are behind the plane" << std::endl;
m_planes.push_back(planeEquation);
}
Modified: pkg/trunk/util/robot_self_filter/self_filter.launch
===================================================================
--- pkg/trunk/util/robot_self_filter/self_filter.launch 2009-07-03 05:06:26 UTC (rev 18263)
+++ pkg/trunk/util/robot_self_filter/self_filter.launch 2009-07-03 05:44:47 UTC (rev 18264)
@@ -1,6 +1,6 @@
<launch>
<include file="$(find pr2_defs)/pr2_planning_environment.launch" />
- <node pkg="new_robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
<remap from="robot_description" to="robotdesc/pr2" />
<remap from="cloud_in" to="tilt_scan_cloud" />
<remap from="cloud_out" to="tilt_scan_cloud_filtered" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-07-03 17:18:13
|
Revision: 18270
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18270&view=rev
Author: mariusmuja
Date: 2009-07-03 17:18:03 +0000 (Fri, 03 Jul 2009)
Log Message:
-----------
Fixed stereo launch files
Modified Paths:
--------------
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/launch/spotting_bag.launch
pkg/trunk/vision/stereo_view/stereo_bag.launch
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/launch/spotting_bag.launch
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/launch/spotting_bag.launch 2009-07-03 17:17:29 UTC (rev 18269)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/launch/spotting_bag.launch 2009-07-03 17:18:03 UTC (rev 18270)
@@ -10,6 +10,12 @@
rectified: Rectification on chip
Provides: all 3 images available but no color
-->
+
+
+<!-- <node pkg="dcam" type="stereodcam" respawn="false"/> -->
+ <!--remap from="stereo" to="stereo_link" /-->
+<group ns="stereo">
+ <node pkg="stereo_image_proc" type="stereoproc" respawn="false">
<param name="stereo/videre_mode" type="str" value="none"/>
<param name="stereo/do_colorize" type="bool" value="True"/>
<param name="stereo/do_rectify" type="bool" value="True"/>
@@ -20,11 +26,8 @@
<param name="stereo/ndisp" value="128"/>
<param name="stereo/gain" type="int" value="10"/>
<param name="stereo/exposure" type="int" value="100"/>
-
-
-<!-- <node pkg="dcam" type="stereodcam" respawn="false"/> -->
- <!--remap from="stereo" to="stereo_link" /-->
- <node pkg="stereo_image_proc" type="stereoproc" respawn="false"/>
+ </node>
+</group>
<node pkg="outlet_detection" name="outlet_spotting" type="outlet_spotting2" output="screen">
<param name="display" type="bool" value="True"/>
<param name="save_patches" type="bool" value="False"/>
Modified: pkg/trunk/vision/stereo_view/stereo_bag.launch
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_bag.launch 2009-07-03 17:17:29 UTC (rev 18269)
+++ pkg/trunk/vision/stereo_view/stereo_bag.launch 2009-07-03 17:18:03 UTC (rev 18270)
@@ -1,13 +1,16 @@
<launch>
- <param name="/stereo/videre_mode" type="str" value="none"/>
- <param name="/stereo/do_colorize" type="bool" value="True"/>
- <param name="/stereo/do_rectify" type="bool" value="True"/>
- <param name="/stereo/do_stereo" type="bool" value="True"/>
- <param name="/stereo/do_calc_points" type="bool" value="True"/>
- <param name="/stereo/do_keep_points" type="bool" value="True"/>
- <node name="stereoproc" pkg="stereo_image_proc" type="stereoproc" respawn="false" />
+<group ns="stereo">
+ <node name="stereoproc" pkg="stereo_image_proc" type="stereoproc" respawn="false" >
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="do_colorize" type="bool" value="True"/>
+ <param name="do_rectify" type="bool" value="True"/>
+ <param name="do_stereo" type="bool" value="True"/>
+ <param name="do_calc_points" type="bool" value="True"/>
+ <param name="do_keep_points" type="bool" value="True"/>
+ </node>
+</group>
<node name="stereo_view" pkg="stereo_view" type="stereo_view_pixel_info" respawn="false" output="screen"/>
<node name="stereodcam_params" pkg="dcam" type="stereodcam_params.py" respawn="false" output="screen"/>
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-07-04 07:07:18
|
Revision: 18294
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18294&view=rev
Author: hsujohnhsu
Date: 2009-07-04 07:07:17 +0000 (Sat, 04 Jul 2009)
Log Message:
-----------
specify full path name and extension for collision meshes.
udpate urdf2gazebo according to this change.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-07-04 06:47:28 UTC (rev 18293)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-07-04 07:07:17 UTC (rev 18294)
@@ -245,6 +245,11 @@
addKeyValue(geom, "scale", values2str(3, mesh->scale));
/* set mesh file */
+ // strip extension from filename
+ std::string tmp_extension(".stl");
+ int pos1 = mesh->filename.find(tmp_extension,0);
+ mesh->filename.replace(pos1,mesh->filename.size()-pos1+1,std::string(""));
+ // add mesh filename
addKeyValue(geom, "mesh", "models/pr2/" + mesh->filename + ".mesh");
}
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml 2009-07-04 06:47:28 UTC (rev 18293)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml 2009-07-04 07:07:17 UTC (rev 18294)
@@ -73,7 +73,7 @@
<collision>
<origin xyz="0.0 0 0.0" rpy="0 0 0" />
<geometry name="${side}_shoulder_pan_collision">
- <mesh filename="shoulder_yaw_convex" />
+ <mesh filename="convex/shoulder_yaw_convex.stlb" />
</geometry>
</collision>
<map name="${side}_shoulder_pan_sensor" flag="gazebo">
@@ -143,7 +143,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_shoulder_lift_collision">
- <mesh filename="shoulder_lift_convex" />
+ <mesh filename="convex/shoulder_lift_convex.stlb" />
</geometry>
</collision>
<map name="${side}_shoulder_lift_sensor" flag="gazebo">
@@ -269,7 +269,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_upper_arm_collision">
- <mesh filename="upper_arm_convex" />
+ <mesh filename="convex/upper_arm_convex.stlb" />
</geometry>
</collision>
@@ -335,7 +335,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_elbow_flex_collision">
- <mesh filename="elbow_flex_convex" />
+ <mesh filename="convex/elbow_flex_convex.stlb" />
</geometry>
</collision>
<map name="${side}_elbow_flex_sensor" flag="gazebo">
@@ -490,7 +490,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_forearm_collision">
- <mesh filename="forearm_convex" />
+ <mesh filename="convex/forearm_convex.stlb" />
</geometry>
</collision>
<map name="${side}_forearm_sensor" flag="gazebo">
@@ -553,7 +553,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_wrist_flex_collision">
- <mesh filename="wrist_flex_convex" />
+ <mesh filename="convex/wrist_flex_convex.stlb" />
</geometry>
</collision>
<map name="${side}_wrist_flex_sensor" flag="gazebo">
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml 2009-07-04 06:47:28 UTC (rev 18293)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml 2009-07-04 07:07:17 UTC (rev 18294)
@@ -85,7 +85,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${prefix}_l_finger_collision">
- <mesh filename="upper_finger_l_convex" />
+ <mesh filename="convex/upper_finger_l_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -169,7 +169,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${prefix}_r_finger_collision">
- <mesh filename="upper_finger_r_convex" />
+ <mesh filename="convex/upper_finger_r_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -250,7 +250,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${prefix}_l_finger_tip_collision">
- <mesh filename="finger_tip_l_convex" />
+ <mesh filename="convex/finger_tip_l_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -332,7 +332,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${prefix}_r_finger_tip_collision">
- <mesh filename="finger_tip_r_convex" />
+ <mesh filename="convex/finger_tip_r_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -482,7 +482,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_gripper_palm_collision">
- <mesh filename="gripper_palm_convex" />
+ <mesh filename="convex/gripper_palm_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -707,7 +707,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_gripper_palm_collision">
- <mesh filename="gripper_palm_convex" />
+ <mesh filename="convex/gripper_palm_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-07-06 17:22:56
|
Revision: 18313
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18313&view=rev
Author: vijaypradeep
Date: 2009-07-06 17:22:54 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
Renaming msg_cache to cache
Now the JointPVFilter outputs position and velocity info
Starting to work on getting the DenseLaserAssemblerFilter going
DenseLaserSnapshotter and DenseLaserAssemblerFilter seem to be working
Modified Paths:
--------------
pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/build_dense_laser_snapshot.cpp
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp
pkg/trunk/sandbox/message_filters/src/filter_example.cpp
pkg/trunk/sandbox/message_filters/test/msg_cache_unittest.cpp
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/msg/JointPVArray.msg
pkg/trunk/sandbox/message_filters/include/message_filters/cache.h
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_extractor.h
pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h
Modified: pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg 2009-07-06 17:22:54 UTC (rev 18313)
@@ -27,6 +27,7 @@
# Store joint information associated with each scan
uint32 joint_encoding
float64[] joint_positions
+float64[] joint_velocities
string[] joint_names
uint32 POS_PER_PIXEL = 0
uint32 POS_PER_ROW_START = 1
@@ -34,5 +35,5 @@
# 1D Data storing meta information for each scan/row
# Currently in stable, time vectors are not serialized correctly
-#time[] scan_start
+time[] scan_start
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt 2009-07-06 17:22:54 UTC (rev 18313)
@@ -12,14 +12,16 @@
rospack(dense_laser_assembler)
-rospack_add_library(dense_laser_assembler src/build_dense_laser_snapshot.cpp)
+rospack_add_library(dense_laser_assembler src/build_dense_laser_snapshot.cpp
+ src/dense_laser_msg_filter.cpp)
#rospack_add_executable(dense_laser_assembler_srv src/dense_laser_assembler_srv.cpp)
#target_link_libraries(dense_laser_assembler_srv dense_laser_assembler)
-#rospack_add_executable(joint_extractor_display src/joint_extractor_display.cpp)
+rospack_add_executable(joint_extractor_display src/joint_extractor_display.cpp)
#rospack_add_executable(tagged_laser_cache_display src/tagged_laser_cache_display.cpp)
rospack_add_executable(dense_laser_snapshotter src/dense_laser_snapshotter.cpp)
+target_link_libraries(dense_laser_snapshotter dense_laser_assembler)
rospack_add_executable(laser_imager src/laser_imager.cpp)
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -42,16 +42,19 @@
#include <vector>
#include "dense_laser_assembler/laser_scan_tagger.h"
-#include "message_filters/msg_cache.h"
+#include "message_filters/cache.h"
+#include "dense_laser_assembler/JointPVArray.h"
+#include "dense_laser_assembler/tagged_laser_scan.h"
+
#include "calibration_msgs/DenseLaserSnapshot.h"
+
namespace dense_laser_assembler
{
-typedef LaserScanTagger<JointExtractor::JointArray> LaserJointTagger ;
-typedef LaserScanTagger<JointExtractor::JointArray>::TaggedLaserScan JointTaggedLaserScan ;
-typedef message_filters::MsgCache<JointTaggedLaserScan> JointTaggedLaserScanCache ;
+typedef LaserScanTagger<JointPVArray> LaserJointTagger ;
+typedef TaggedLaserScan<JointPVArray> JointTaggedLaserScan ;
/**
* \brief Builds a dense laser snapshot
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -0,0 +1,207 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+//! \author Vijay Pradeep / vpr...@wi...
+
+
+#ifndef DENSE_LASER_ASSEMBLER_DENSE_LASER_MSG_FILTER_H_
+#define DENSE_LASER_ASSEMBLER_DENSE_LASER_MSG_FILTER_H_
+
+#include "ros/ros.h"
+#include "dense_laser_assembler/joint_pv_msg_filter.h"
+#include "dense_laser_assembler/laser_scan_tagger.h"
+#include "message_filters/cache.h"
+
+
+// Messages
+#include "sensor_msgs/LaserScan.h"
+#include "mechanism_msgs/MechanismState.h"
+#include "calibration_msgs/DenseLaserSnapshot.h"
+
+#define CONSTRUCT_INT(param, default_val) \
+ int param ;\
+ if (!n_.getParam("~" #param, param) ) \
+ { \
+ ROS_WARN("[~" #param "] not set. Setting to default value of [%u]", default_val) ; \
+ param = default_val ; \
+ } \
+ else \
+ { \
+ ROS_INFO("[~" #param "] set to value of [%u]", param) ; \
+ }
+
+namespace dense_laser_assembler
+{
+
+/**
+ * \brief Listens to LaserScan and MechanismState messages, and builds DenseLaserSnapshots
+ */
+class DenseLaserMsgFilter
+{
+public:
+
+ typedef boost::shared_ptr<const TaggedLaserScan<JointPVArray> > MConstPtr ;
+ typedef boost::function<void(const MConstPtr&)> Callback;
+ typedef boost::signal<void(const MConstPtr&)> Signal;
+
+ //! \brief Not yet implemented
+ template<class A, class B>
+ DenseLaserMsgFilter(std::string name, A& a, B&b, unsigned int laser_queue_size,
+ unsigned int laser_cache_size, unsigned int mech_state_cache,
+ std::vector<std::string> joint_names) ;
+
+
+ /**
+ * \brief Construct assembler, and subscribe to the both LaserScan and MechanismState data providers
+ * \param name Namespace for the node. Filter will search for params in ~/[name]/
+ * \param laser_scan_provider MsgFilter that will provide LaserScan messages
+ * \param mech_state_provider MsgFilter that will provide MechanismState message
+ */
+ template<class A, class B>
+ DenseLaserMsgFilter(std::string name, A& laser_scan_provider, B& mech_state_provider)
+ {
+ subscribeLaserScan(laser_scan_provider) ;
+ subscribeMechState(mech_state_provider) ;
+
+ // Get all the different queue size parameters
+ CONSTRUCT_INT(laser_queue_size, 40) ;
+ CONSTRUCT_INT(laser_cache_size, 1000) ;
+ CONSTRUCT_INT(mech_state_cache_size, 100) ;
+
+ // Get the list of joints that we care about
+ joint_names_.clear() ;
+ bool found_joint = true ;
+ int joint_count = 0 ;
+
+ char param_buf[1024] ;
+ while(found_joint)
+ {
+ sprintf(param_buf, "~joint_name_%02u", joint_count) ;
+ std::string param_name = param_buf ;
+ std::string cur_joint_name ;
+ found_joint = n_.getParam(param_name, cur_joint_name) ;
+ if (found_joint)
+ {
+ ROS_INFO("[%s] -> %s", param_name.c_str(), cur_joint_name.c_str()) ;
+ joint_names_.push_back(cur_joint_name) ;
+ }
+ else
+ ROS_DEBUG("Did not find param [%s]", param_name.c_str()) ;
+ joint_count++ ;
+ }
+
+ // Configure the joint_pv_filter and associated cache
+ joint_pv_filter_.setJointNames(joint_names_) ;
+ joint_cache_.setCacheSize(mech_state_cache_size) ;
+ joint_cache_.subscribe(joint_pv_filter_) ;
+
+ // Set up the laser tagger and associated cache
+ laser_tagger_.setMaxQueueSize(laser_queue_size) ;
+ laser_tagger_.subscribeTagCache(joint_cache_) ;
+ tagged_laser_cache_.setCacheSize(laser_cache_size) ;
+ tagged_laser_cache_.subscribe(laser_tagger_) ;
+ }
+
+ /**
+ * \brief Used to connect this MessageFilter to a callback
+ * \param callback Function to be called whenever a new TaggedLaserScan message is available
+ */
+ boost::signals::connection connect(const Callback& callback) ;
+
+ //! \brief Not yet implemented
+ void processLaserScan(const sensor_msgs::LaserScanConstPtr& msg) ;
+
+ //! \brief Not yet implemented
+ void processMechState(const mechanism_msgs::MechanismState& msg) ;
+
+ //! \brief Get what the oldest time processed scan is
+ ros::Time getOldestScanTime() ;
+ //! \brief Get what the new time processed scan is
+ ros::Time getNewstScanTime() ;
+
+ /**
+ * \brief Builds a dense laser snapshot
+ * Grabs all the scans between the start and end time, and composes them into one larger snapshot. This call is non-blocking.
+ * Thus, if there are scans that haven't been cached yet, but occur before the end time, they won't be added to the snapshot.
+ * \param start The earliest scan time to be included in the snapshot
+ * \param end The latest scan time to be included in the snapshot
+ * \param output: A populated snapshot message
+ * \return True if successful. False if unsuccessful
+ */
+ bool buildSnapshotFromInterval(const ros::Time& start, const ros::Time& end, calibration_msgs::DenseLaserSnapshot& snapshot) ;
+private:
+ ros::NodeHandle n_ ;
+
+ /**
+ * \brief Subscribe to a message filter that outputs LaserScans.
+ * This is only useful if we're not subscribing to laser scans in the constructor.
+ * Thus, this will stay private until we have more flexible constructors
+ */
+ template<class A>
+ void subscribeLaserScan(A& a)
+ {
+ laser_tagger_.subscribeLaserScan(a) ;
+ }
+
+ /**
+ * \brief Subscribe to a message filter that outputs MechanismState.
+ * This is only useful if we're not subscribing to MechanismState in the constructor.
+ * Thus, this will stay private until we have more flexible constructors
+ */
+ template<class B>
+ void subscribeMechState(B& b)
+ {
+ joint_pv_filter_.subscribe(b) ;
+ }
+
+ //! Extracts positions data for a subset of joints in MechanismState
+ JointPVMsgFilter joint_pv_filter_ ;
+
+ //! Stores a time history of position data for a subset of joints in MechanismState
+ message_filters::Cache<JointPVArray> joint_cache_ ;
+
+ //! Combines a laser scan with the set of pertinent joint positions
+ LaserScanTagger< JointPVArray > laser_tagger_ ;
+
+ //! Stores a time history of laser scans that are annotated with joint positions.
+ message_filters::Cache< TaggedLaserScan<JointPVArray> > tagged_laser_cache_ ;
+
+ std::vector<std::string> joint_names_ ;
+} ;
+
+}
+
+#undef CONSTRUCT_INT
+
+#endif
Deleted: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_extractor.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_extractor.h 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_extractor.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -1,133 +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.
-*********************************************************************/
-
-/*! \mainpage
- * \htmlinclude manifest.html
- */
-
-#ifndef DENSE_LASER_ASSEMBLER_JOINT_EXTRACTOR
-#define DENSE_LASER_ASSEMBLER_JOINT_EXTRACTOR
-
-#include <vector>
-
-#include "mechanism_msgs/MechanismState.h"
-
-namespace dense_laser_assembler
-{
-
-/**
- * Streams an array of joint positions from MechanismState, given a list of requested joints
- */
-class JointExtractor
-{
-public:
-
- struct JointArray
- {
- roslib::Header header ;
- std::vector<double> positions ;
- } ;
-
- JointExtractor()
- {
- joint_names_.clear() ;
- }
-
- JointExtractor(std::vector<std::string> joint_names)
- {
- joint_names_ = joint_names ;
- }
-
- void setJointNames(std::vector<std::string> joint_names)
- {
- joint_names_ = joint_names ;
- }
-
- /**
- * Links Consumer's input to some provider's output.
- * \param provider The filter from which we want to receive data
- */
- template<class T>
- void subscribe(T& provider)
- {
- provider.addOutputCallback(boost::bind(&JointExtractor::processMechState, this, _1)) ;
- }
-
- void processMechState(const mechanism_msgs::MechanismStateConstPtr& msg)
- {
- //printf("Processing MechState\n") ;
- boost::shared_ptr<JointArray> joint_array_ptr(new JointArray) ;
- joint_array_ptr->positions.resize(joint_names_.size()) ;
-
- joint_array_ptr->header = msg->header ;
-
- //! \todo This can seriously be optimized using a map, or some sort of cached lookup
- for (unsigned int i=0; i<joint_names_.size(); i++)
- {
- for (unsigned int j=0; j<msg->joint_states.size(); j++)
- {
- if (joint_names_[i] == msg->joint_states[j].name )
- {
- joint_array_ptr->positions[i] = msg->joint_states[j].position ;
- break ;
- }
- }
- }
-
- // Sequentially call each registered call
- for (unsigned int i=0; i<output_callbacks_.size(); i++)
- output_callbacks_[i](joint_array_ptr) ;
- }
-
- void addOutputCallback(const boost::function<void(const boost::shared_ptr<JointArray const>&)>& callback)
- {
- output_callbacks_.push_back(callback) ;
- }
-
-private:
- std::vector<std::string> joint_names_ ;
- std::vector<boost::function<void(const boost::shared_ptr<JointArray const>&)> > output_callbacks_ ;
-
-} ;
-
-}
-
-
-
-
-
-
-
-
-#endif // DENSE_LASER_ASSEMBLER_JOINT_EXTRACTOR
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -0,0 +1,168 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/*! \mainpage
+ * \htmlinclude manifest.html
+ */
+
+#ifndef DENSE_LASER_ASSEMBLER_JOINT_PV_MSG_FILTER_H_
+#define DENSE_LASER_ASSEMBLER_JOINT_PV_MSG_FILTER_H_
+
+#include <vector>
+
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/signals.hpp>
+
+// Messages
+#include "dense_laser_assembler/JointPVArray.h"
+#include "mechanism_msgs/MechanismState.h"
+
+namespace dense_laser_assembler
+{
+
+/**
+ * Streams an array of joint positions and velocities from MechanismState,
+ * given a mapping from joint_names to array indices
+ */
+class JointPVMsgFilter
+{
+public:
+ typedef boost::function<void(const JointPVArrayConstPtr&)> Callback;
+ typedef boost::signal<void(const JointPVArrayConstPtr&)> Signal;
+
+ /**
+ * \brief Subscribe to another MessageFilter at construction time
+ * \param a The parent message filter
+ * \param joint_names Vector of joint names that we want to output. Must match
+ * joint names in mechanism state
+ */
+ template<class A>
+ JointPVMsgFilter(A& a, std::vector<std::string> joint_names = std::vector<std::string>())
+ {
+ setJointNames(joint_names);
+ subscribe(a);
+ }
+
+ /**
+ * \brief Construct without subcribing to another MsgFilter at construction time
+ * \param joint_names Vector of joint names that we want to output. Must match
+ * joint names in mechanism state
+ */
+ JointPVMsgFilter(std::vector<std::string> joint_names = std::vector<std::string>())
+ {
+ setJointNames(joint_names);
+ }
+
+ /**
+ * \brief Subcribes this MsgFilter to another MsgFilter
+ * \param a The MsgFilter that this MsgFilter should get data from
+ */
+ template<class A>
+ void subscribe(A& a)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ incoming_connection_ = a.connect(boost::bind(&JointPVMsgFilter::processMechState, this, _1));
+ }
+
+ /**
+ * \brief Called by user wants they want to connect this MessageFilter to their own code
+ * \param callback Function to be called whenever a JointPVArray message is available
+ */
+ boost::signals::connection connect(const Callback& callback)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ return signal_.connect(callback);
+ }
+
+ /**
+ * \brief Define the mapping from MechanismState to JointPVArray
+ */
+ void setJointNames(std::vector<std::string> joint_names)
+ {
+ boost::mutex::scoped_lock lock(joint_mapping_mutex_);
+ joint_names_ = joint_names;
+ }
+
+ /**
+ * \brief Extracts joint positions from MechState, using the joint_names mapping.
+ * Also calls all the callback functions as set by connect()
+ * \param msg The MechanismState message from which we want to extract positions
+ */
+ void processMechState(const mechanism_msgs::MechanismStateConstPtr& msg)
+ {
+ // Allocate mem to store out output data
+ boost::shared_ptr<JointPVArray> joint_array_ptr(new JointPVArray) ;
+
+ // Copy MechState data into a JointPVArray
+ {
+ boost::mutex::scoped_lock lock(joint_mapping_mutex_);
+
+ joint_array_ptr->pos.resize(joint_names_.size()) ;
+ joint_array_ptr->vel.resize(joint_names_.size()) ;
+
+ joint_array_ptr->header = msg->header ;
+
+ //! \todo This can seriously be optimized using a map, or some sort of cached lookup
+ for (unsigned int i=0; i<joint_names_.size(); i++)
+ {
+ for (unsigned int j=0; j<msg->joint_states.size(); j++)
+ {
+ if (joint_names_[i] == msg->joint_states[j].name )
+ {
+ joint_array_ptr->pos[i] = msg->joint_states[j].position ;
+ joint_array_ptr->vel[i] = msg->joint_states[j].velocity ;
+ break ;
+ }
+ }
+ }
+ }
+
+ // Call all connected callbacks
+ signal_(joint_array_ptr) ;
+ }
+
+protected:
+ boost::mutex joint_mapping_mutex_ ;
+ std::vector<std::string> joint_names_ ;
+
+ // Filter Connection Stuff
+ boost::signals::connection incoming_connection_;
+ Signal signal_;
+ boost::mutex signal_mutex_;
+} ;
+
+}
+
+#endif // DENSE_LASER_ASSEMBLER_JOINT_PV_MSG_FILTER_H_
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -41,87 +41,138 @@
#include <deque>
#include "sensor_msgs/LaserScan.h"
-#include "message_filters/msg_cache.h"
-#include "dense_laser_assembler/joint_extractor.h"
+#include "message_filters/cache.h"
#include "boost/shared_ptr.hpp"
+#include "dense_laser_assembler/tagged_laser_scan.h"
namespace dense_laser_assembler
{
+/**
+ * Listens to LaserScans and a cache of 'tag' messages. Outputs a laserScan
+ * whenever there is a tag msg that occurs before and after the scan.
+ */
template <class T>
class LaserScanTagger
{
public:
- struct TaggedLaserScan
- {
- roslib::Header header ;
- sensor_msgs::LaserScanConstPtr scan ;
- boost::shared_ptr<const T> before ;
- boost::shared_ptr<const T> after ;
- } ;
+ typedef boost::shared_ptr<const T> TConstPtr ;
+ typedef boost::shared_ptr<const TaggedLaserScan<T> > MConstPtr ;
+ typedef boost::function<void(const MConstPtr&)> Callback;
+ typedef boost::signal<void(const MConstPtr&)> Signal;
- typedef boost::shared_ptr<const TaggedLaserScan> TaggedLaserScanConstPtr ;
- LaserScanTagger(message_filters::MsgCache<T>& tag_cache, unsigned int max_queue_size)
+ template<class A>
+ /**
+ * \brief Construct object, and also subscribe to relevant data sources
+ */
+ LaserScanTagger(A& a, message_filters::Cache<T>& tag_cache, unsigned int max_queue_size)
{
+ subscribeLaserScan(a);
+ subscribeTagCache(tag_cache);
+ setMaxQueueSize(max_queue_size);
tag_cache_ = &tag_cache ;
max_queue_size_ = max_queue_size ;
}
LaserScanTagger()
{
- tag_cache_ = NULL ;
- max_queue_size_ = 1 ;
+ tag_cache_ = NULL;
+ setMaxQueueSize(1);
}
- void setQueueSize(unsigned int max_queue_size)
+ ~LaserScanTagger()
{
- max_queue_size_ = max_queue_size ;
+ incoming_laser_scan_connection_.disconnect() ;
+ incoming_tag_connection_.disconnect() ;
}
- void setTagCache(message_filters::MsgCache<T>& tag_cache)
+ template<class A>
+ void subscribeLaserScan(A& a)
{
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ incoming_laser_scan_connection_ = a.connect(boost::bind(&LaserScanTagger<T>::processLaserScan, this, _1));
+ }
+
+ void subscribeTagCache(message_filters::Cache<T>& tag_cache)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
tag_cache_ = &tag_cache ;
+ incoming_tag_connection_ = tag_cache.connect(boost::bind(&LaserScanTagger<T>::processTag, this, _1));
}
+ void setMaxQueueSize(unsigned int max_queue_size)
+ {
+ boost::mutex::scoped_lock lock(queue_mutex_);
+ max_queue_size_ = max_queue_size;
+ }
+
/**
- * Adds the laser scan onto the queue of scans that need to be matched with the stamped data
+ * Adds the laser scan onto the queue of scans that need to be matched with the tags
*/
void processLaserScan(const sensor_msgs::LaserScanConstPtr& msg)
{
+ {
+ boost::mutex::scoped_lock lock(queue_mutex_);
+ //! \todo need to more carefully decide on overflow logic
+ if (queue_.size() < max_queue_size_)
+ queue_.push_back(msg) ;
+ else
+ ROS_WARN("Queue full, not pushing new data onto queue until queue is serviced") ;
+ }
- queue_lock_.lock() ;
+ update() ;
+ }
- //! \todo need to decide on overflow logic
- if (queue_.size() < max_queue_size_)
- queue_.push_back(msg) ;
- else
- ROS_WARN("Queue full, not pushing new data onto queue until queue is serviced") ;
- queue_lock_.unlock() ;
-
+ /**
+ * Since we just received a new tag message, we might have laser scans that we need to process
+ */
+ void processTag(const TConstPtr& msg)
+ {
update() ;
}
/**
+ * \brief Connect this message filter's output to some callback
+ * \param callback Function to call after we've tagged a LaserScan
+ */
+ boost::signals::connection connect(const Callback& callback)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ return signal_.connect(callback);
+ }
+
+ /**
* Triggers the module to try to service the queue
*/
void update()
{
+ //! \todo This is not a good enough check. We need to somehow make sure the cache hasn't been destructed
if (!tag_cache_)
{
- ROS_WARN("Have a NULL pointer to TagCache. Skipping update") ;
- return ;
+ ROS_WARN("Have a NULL pointer to TagCache. Skipping update");
+ return;
}
- queue_lock_.lock() ;
-
bool did_something = true ;
- while (did_something && queue_.size() > 0)
+ // It's possible we need to process multiple laser scans. Therefore, keep
+ // looping until we reach a scan that's a no-op
+ while (true)
{
+ //! \todo Come up with better locking/unlocking in this loop
+ queue_mutex_.lock();
+
+ // Exit condition
+ if (!did_something || queue_.size() == 0)
+ {
+ queue_mutex_.unlock();
+ break;
+ }
+
did_something = false ; // Haven't done anything yet
const sensor_msgs::LaserScanConstPtr& elem = queue_.front() ;
@@ -132,51 +183,45 @@
boost::shared_ptr<const T> tag_before = tag_cache_->getElemBeforeTime(s...
[truncated message content] |
Thanks for helping keep SourceForge clean.
X