|
From: <vij...@us...> - 2009-01-15 02:31:26
|
Revision: 9471
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9471&view=rev
Author: vijaypradeep
Date: 2009-01-15 01:32:18 +0000 (Thu, 15 Jan 2009)
Log Message:
-----------
Started prototyping a realtime scheduling framework
Added Paths:
-----------
pkg/trunk/sandbox/
pkg/trunk/sandbox/rt_scheduler/
pkg/trunk/sandbox/rt_scheduler/CMakeLists.txt
pkg/trunk/sandbox/rt_scheduler/Makefile
pkg/trunk/sandbox/rt_scheduler/include/
pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/
pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/oracle.h
pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/ports.h
pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/rt_node.h
pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/scheduler.h
pkg/trunk/sandbox/rt_scheduler/manifest.xml
pkg/trunk/sandbox/rt_scheduler/src/
pkg/trunk/sandbox/rt_scheduler/src/run_test.cpp
Added: pkg/trunk/sandbox/rt_scheduler/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/rt_scheduler/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/rt_scheduler/CMakeLists.txt 2009-01-15 01:32:18 UTC (rev 9471)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(rt_scheduler)
+
+#rospack_add_library(rt_scheduler )
+
+rospack_add_executable(run_test src/run_test.cpp)
+#target_link_libraries(run_test rt_scheduler)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
\ No newline at end of file
Added: pkg/trunk/sandbox/rt_scheduler/Makefile
===================================================================
--- pkg/trunk/sandbox/rt_scheduler/Makefile (rev 0)
+++ pkg/trunk/sandbox/rt_scheduler/Makefile 2009-01-15 01:32:18 UTC (rev 9471)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/oracle.h
===================================================================
--- pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/oracle.h (rev 0)
+++ pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/oracle.h 2009-01-15 01:32:18 UTC (rev 9471)
@@ -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 Vijay Pradeep
+
+#ifndef RT_SCHEDULER_ORACLE_H_
+#define RT_SCHEDULER_ORACLE_H_
+
+#include <vector>
+#include <utility>
+#include <string>
+
+#include "rt_scheduler/rt_node.h"
+#include "rt_scheduler/ports.h"
+
+namespace rt_scheduler
+{
+
+class Oracle
+{
+public:
+ Oracle()
+ { }
+
+ ~Oracle()
+ { }
+
+ void registerPort(OutputPort<int>* port, const std::string& name)
+ {
+ std::pair< std::string, OutputPort<int>* > entry(name, port) ;
+ ports_.push_back(entry) ;
+ }
+
+ void find(OutputPort<int>* &port, const std::string& name)
+ {
+ port = NULL ;
+ for (unsigned int i=0; i < ports_.size(); i++)
+ {
+ if (ports_[i].first == name)
+ port = ports_[i].second ;
+ }
+ }
+
+private:
+ std::vector< std::pair<std::string, OutputPort<int>* > > ports_ ;
+} ;
+
+}
+
+#endif /* RT_SCHEDULER_ORACLE_H_ */
Added: pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/ports.h
===================================================================
--- pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/ports.h (rev 0)
+++ pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/ports.h 2009-01-15 01:32:18 UTC (rev 9471)
@@ -0,0 +1,94 @@
+/*********************************************************************
+* 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
+
+#ifndef RT_SCHEDULER_PORTS_H_
+#define RT_SCHEDULER_PORTS_H_
+
+#include "rt_scheduler/rt_node.h"
+
+namespace rt_scheduler
+{
+
+template<class T>
+class OutputPort
+{
+public:
+ OutputPort(RtNode* owner) { owner_ = owner ; }
+
+//private
+ T data_ ;
+ RtNode* owner_ ;
+} ;
+
+template<class T>
+class InputPort
+{
+public:
+ InputPort(RtNode* owner)
+ {
+ owner_ = owner ;
+ output_port_ = NULL ;
+ id_ = owner_->generateID() ;
+ }
+
+ bool getData(T& data)
+ {
+ if (output_port_)
+ {
+ data = output_port_->data_ ;
+ return true ;
+ }
+ else
+ return false ;
+ }
+
+ void Link(OutputPort<T>* output_port)
+ {
+ output_port_ = output_port ;
+ owner_->updateDep(id_, output_port_->owner_) ;
+ }
+
+//private
+ OutputPort<T>* output_port_ ;
+ RtNode* owner_ ;
+ unsigned int id_ ;
+} ;
+
+}
+
+
+
+#endif /* RT_SCHEDULER_PORTS_H_ */
Added: pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/rt_node.h
===================================================================
--- pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/rt_node.h (rev 0)
+++ pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/rt_node.h 2009-01-15 01:32:18 UTC (rev 9471)
@@ -0,0 +1,80 @@
+/*********************************************************************
+* 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
+
+#ifndef RT_SCHEDULER_RT_NODE_H_
+#define RT_SCHEDULER_RT_NODE_H_
+
+#include <string>
+#include <vector>
+#include <utility>
+
+namespace rt_scheduler
+{
+
+class RtNode
+{
+public:
+ RtNode() { }
+ RtNode(std::string name) : name_(name) { } ;
+ virtual ~RtNode() { } ;
+ virtual bool update() = 0 ;
+
+ unsigned int generateID()
+ {
+ unsigned int next_id = deps_.size() ;
+ deps_.push_back(NULL) ;
+ printf("%s: Adding port ID #%u\n", name_.c_str(), next_id) ;
+ return next_id ;
+ }
+
+ void updateDep(unsigned int id, RtNode* dep_node)
+ {
+ deps_[id] = dep_node ;
+ }
+
+ std::string name_ ;
+
+//private:
+ std::vector< RtNode* > deps_ ;
+ std::vector<unsigned int> exec_order_ ;
+
+} ;
+
+}
+
+
+
+#endif /* RT_SCHEDULER_RT_NODE_H_ */
Added: pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/scheduler.h
===================================================================
--- pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/scheduler.h (rev 0)
+++ pkg/trunk/sandbox/rt_scheduler/include/rt_scheduler/scheduler.h 2009-01-15 01:32:18 UTC (rev 9471)
@@ -0,0 +1,101 @@
+/*********************************************************************
+* 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
+
+#ifndef RT_SCHEDULER_SCHEDULER_H_
+#define RT_SCHEDULER_SCHEDULER_H_
+
+
+
+#include <vector>
+
+#include "rt_scheduler/rt_node.h"
+
+
+
+namespace rt_scheduler
+{
+
+class Scheduler : public rt_scheduler::RtNode
+{
+public:
+ Scheduler()
+ { }
+
+ ~Scheduler()
+ { }
+
+ void add(RtNode* node)
+ {
+ node_list_.push_back(node) ;
+ printf("Adding %s to scheduler\n", node->name_.c_str()) ;
+ }
+
+ bool update()
+ {
+ bool success = true ;
+ for (unsigned int i=0; i < exec_order_.size(); i++)
+ {
+ printf("Updating Node #%u: %s\n", exec_order_[i], node_list_[exec_order_[i]]->name_.c_str()) ;
+ success = success && node_list_[exec_order_[i]]->update() ;
+ }
+ return success ;
+ }
+
+ // A super dumb, dangerous scheduler
+ void updateExecutionOrder()
+ {
+ printf("Dependencies:\n") ;
+ for (unsigned int i=0; i<node_list_.size(); i++)
+ {
+ for (unsigned int j=0; j<node_list_[i]->deps_.size(); j++)
+ printf(" %s -> %s\n", node_list_[i]->name_.c_str(), node_list_[i]->deps_[j]->name_.c_str()) ;
+ }
+
+
+ exec_order_.resize(node_list_.size()) ;
+ for (unsigned int i=0; i<exec_order_.size(); i++)
+ exec_order_[i] = i ;
+ }
+
+private:
+ std::vector<RtNode*> node_list_ ;
+ std::vector<unsigned int> exec_order_ ;
+
+} ;
+
+}
+
+#endif /* RT_SCHEDULER_SCHEDULER_H_ */
Added: pkg/trunk/sandbox/rt_scheduler/manifest.xml
===================================================================
--- pkg/trunk/sandbox/rt_scheduler/manifest.xml (rev 0)
+++ pkg/trunk/sandbox/rt_scheduler/manifest.xml 2009-01-15 01:32:18 UTC (rev 9471)
@@ -0,0 +1,6 @@
+<package>
+ <description>Vijay's prototype idea of a reatime (single thread) scheduler</description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="Experimental" notes=""/>
+</package>
Added: pkg/trunk/sandbox/rt_scheduler/src/run_test.cpp
===================================================================
--- pkg/trunk/sandbox/rt_scheduler/src/run_test.cpp (rev 0)
+++ pkg/trunk/sandbox/rt_scheduler/src/run_test.cpp 2009-01-15 01:32:18 UTC (rev 9471)
@@ -0,0 +1,188 @@
+/*********************************************************************
+* 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
+
+#include <cstdio>
+
+#include "rt_scheduler/rt_node.h"
+#include "rt_scheduler/ports.h"
+#include "rt_scheduler/scheduler.h"
+#include "rt_scheduler/oracle.h"
+using namespace rt_scheduler ;
+
+class Talker : public rt_scheduler::RtNode
+{
+public:
+ Talker(int start, int inc) : out_(this)
+ {
+ state_ = start ;
+ inc_ = inc ;
+ }
+
+ ~Talker()
+ { }
+
+ bool update()
+ {
+ state_ += inc_ ;
+ out_.data_ = state_ ;
+ printf("%s: out = %i\n", name_.c_str(), out_.data_) ;
+ return true ;
+ }
+
+ OutputPort<int> out_ ;
+ int state_ ;
+ int inc_ ;
+} ;
+
+class Adder : public rt_scheduler::RtNode
+{
+public:
+ Adder(Oracle& oracle, std::string name) : rt_scheduler::RtNode(name), in_A_(this),
+ in_B_(this), out_(this)
+ {
+ oracle.registerPort(&out_, name+"/out") ;
+ }
+
+ ~Adder()
+ { }
+
+ bool update()
+ {
+ int a,b ;
+ if (in_A_.getData(a))
+ printf(" Got data from Port A: %i\n", a) ;
+ else
+ {
+ printf(" Error accessing Port A\n") ;
+ return false ;
+ }
+ if (in_B_.getData(b))
+ printf(" Got data from Port B: %i\n", b) ;
+ else
+ {
+ printf(" Error accessing Port B\n") ;
+ return false ;
+ }
+
+ int result = a+b ;
+
+ out_.data_ = result ;
+
+ printf(" Result: %i\n", result) ;
+
+ return true ;
+ }
+
+ InputPort<int> in_A_ ;
+ InputPort<int> in_B_ ;
+ OutputPort<int> out_ ;
+} ;
+
+class Doubler : public rt_scheduler::RtNode
+{
+public:
+ Doubler(std::string name) : rt_scheduler::RtNode(name), in_(this)
+ {
+
+ }
+
+ ~Doubler()
+ { }
+
+ bool update()
+ {
+ int a ;
+ if (in_.getData(a))
+ printf(" Got data from Port: %i\n", a) ;
+ else
+ {
+ printf(" Error accessing Port\n") ;
+ return false ;
+ }
+
+ int result = 2*a ;
+
+
+ printf(" Result: %i\n", result) ;
+
+ return true ;
+ }
+
+ InputPort<int> in_ ;
+} ;
+
+
+int main(int argc, char** argv)
+{
+ // Initialize
+ Oracle oracle ;
+
+ Talker talker_A(5,2) ;
+ Talker talker_B(10,-1) ;
+ Adder adder(oracle, "Adder") ;
+ Doubler doubler("Doubler") ;
+
+ talker_A.name_ = "TalkerA" ;
+ talker_B.name_ = "TalkerB" ;
+
+ // Connect ports together
+ adder.in_A_.Link(&talker_A.out_) ;
+ adder.in_B_.Link(&talker_B.out_) ;
+
+ OutputPort<int>* port ;
+ oracle.find(port, "Adder/out") ;
+ if(port != NULL)
+ doubler.in_.Link(port) ;
+ else
+ printf("Oracle could not find port") ;
+
+ // Add to the scheduler
+ Scheduler scheduler ;
+ scheduler.add(&talker_A) ;
+ scheduler.add(&talker_B) ;
+ scheduler.add(&adder) ;
+ scheduler.add(&doubler) ;
+
+ scheduler.updateExecutionOrder() ;
+
+ while(true)
+ {
+ scheduler.update() ;
+ getchar() ;
+ }
+
+ return 0 ;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-15 19:26:02
|
Revision: 9502
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9502&view=rev
Author: sfkwc
Date: 2009-01-15 19:25:57 +0000 (Thu, 15 Jan 2009)
Log Message:
-----------
#839: deprecating in favor of mechanism_model and xacro
Added Paths:
-----------
pkg/trunk/deprecated/wg_robot_description_parser/
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description_parser/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-15 19:31:26
|
Revision: 9504
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9504&view=rev
Author: sfkwc
Date: 2009-01-15 19:31:22 +0000 (Thu, 15 Jan 2009)
Log Message:
-----------
#839: deprecating in favor of mechanism_model and xacro
Added Paths:
-----------
pkg/trunk/deprecated/resource_locator/
Removed Paths:
-------------
pkg/trunk/util/resource_locator/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-16 09:10:30
|
Revision: 9536
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9536&view=rev
Author: rdiankov
Date: 2009-01-16 09:10:25 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
moved PhaseSpace messages to robot_msgs and prefixed them with Mocap, made several x86-64 compat fixes, added better detection for optimization options in collision_map, fixed bug in rosarmik
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/drivers/cam/dc1394_cam/CMakeLists.txt
pkg/trunk/drivers/phase_space/CMakeLists.txt
pkg/trunk/drivers/phase_space/manifest.xml
pkg/trunk/drivers/phase_space/phase_space_node.cpp
pkg/trunk/drivers/phase_space/phase_space_node.h
pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml
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_odometry.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp
pkg/trunk/mapping/collision_map/CMakeLists.txt
pkg/trunk/mapping/collision_map/src/collision_map.cpp
pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m
pkg/trunk/openrave_planning/ormanipulation/octave/runmanip_sim.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp
pkg/trunk/openrave_planning/orrosplanning/src/rosplanningproblem.h
pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
Added Paths:
-----------
pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h
pkg/trunk/robot_msgs/msg/MocapBody.msg
pkg/trunk/robot_msgs/msg/MocapMarker.msg
pkg/trunk/robot_msgs/msg/MocapSnapshot.msg
Removed Paths:
-------------
pkg/trunk/drivers/phase_space/msg/
pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-16 09:10:25 UTC (rev 9536)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 606
+SVN_REVISION = -r 609
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-01-16 09:10:25 UTC (rev 9536)
@@ -19,4 +19,4 @@
std_msgs/PointCloud laser_cloud
# Data that we captured from phasespace
-phase_space/PhaseSpaceSnapshot phase_space_snapshot
+robot_msgs/MocapSnapshot phase_space_snapshot
Modified: pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -37,7 +37,7 @@
#include "ros/node.h"
#include "rosthread/mutex.h"
#include "robot_msgs/MechanismState.h"
-#include "phase_space/PhaseSpaceSnapshot.h"
+#include "robot_msgs/MocapSnapshot.h"
#include "robot_kinematics/robot_kinematics.h"
#include "kdl/chain.hpp"
@@ -111,7 +111,7 @@
case ' ':
{
printf("Capturing...\n") ;
- phase_space::PhaseSpaceMarker cur_marker ;
+ robot_msgs::MocapMarker cur_marker ;
//GetMarker(cur_marker, marker_id_) ;
@@ -138,7 +138,7 @@
robot_kinematics::RobotKinematics robot_kinematics ;
string robot_desc ;
param("robotdesc/pr2", robot_desc, string("")) ;
- printf("RobotDesc.length() = %u\n", robot_desc.length()) ;
+ printf("RobotDesc.length() = %u\n", (unsigned int)robot_desc.length()) ;
robot_kinematics.loadString(robot_desc.c_str()) ;
@@ -238,7 +238,7 @@
return true ;
}
- void GetMarker(phase_space::PhaseSpaceMarker& marker, int id)
+ void GetMarker(robot_msgs::MocapMarker& marker, int id)
{
bool marker_found = false ;
@@ -304,10 +304,10 @@
}
private:
- phase_space::PhaseSpaceSnapshot snapshot_ ;
+ robot_msgs::MocapSnapshot snapshot_ ;
robot_msgs::MechanismState mech_state_ ;
- phase_space::PhaseSpaceSnapshot safe_snapshot_ ;
+ robot_msgs::MocapSnapshot safe_snapshot_ ;
int marker_id_ ;
robot_msgs::MechanismState safe_mech_state_ ;
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -37,7 +37,7 @@
#include "ros/node.h"
#include "rosthread/mutex.h"
#include "robot_msgs/MechanismState.h"
-#include "phase_space/PhaseSpaceSnapshot.h"
+#include "robot_msgs/MocapSnapshot.h"
#include "std_msgs/PointCloud.h"
Modified: pkg/trunk/drivers/cam/dc1394_cam/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/cam/dc1394_cam/CMakeLists.txt 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/cam/dc1394_cam/CMakeLists.txt 2009-01-16 09:10:25 UTC (rev 9536)
@@ -13,6 +13,8 @@
link_directories(${SDL_LIBRARY_DIRS})
rospack_add_library(dc1394cam src/dc1394_cam/dc1394_cam.cpp)
+rospack_add_compile_flags(dc1394cam "-fPIC")
+
# librt is needed for clock_gettime(); not sure how this interacts with
# real-time....
target_link_libraries(dc1394cam rt)
Modified: pkg/trunk/drivers/phase_space/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/phase_space/CMakeLists.txt 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/phase_space/CMakeLists.txt 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,7 +1,6 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(phase_space)
-genmsg()
link_directories(${PROJECT_SOURCE_DIR}/lib)
rospack_add_executable(phase_space_node phase_space_node.cpp)
#rospack_add_executable(phase_space_benchmark phase_space_benchmark.cpp)
Modified: pkg/trunk/drivers/phase_space/manifest.xml
===================================================================
--- pkg/trunk/drivers/phase_space/manifest.xml 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/phase_space/manifest.xml 2009-01-16 09:10:25 UTC (rev 9536)
@@ -5,6 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
<depend package="std_msgs" />
+ <depend package="robot_msgs" />
<depend package="rospy" />
<sysdepend os="ubuntu" version="8.04-hardy" package="libstdc++5"/>
Modified: pkg/trunk/drivers/phase_space/phase_space_node.cpp
===================================================================
--- pkg/trunk/drivers/phase_space/phase_space_node.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/phase_space/phase_space_node.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -57,7 +57,7 @@
PhaseSpaceNode::PhaseSpaceNode() : ros::node("phase_space")
{
- advertise<PhaseSpaceSnapshot>("phase_space_snapshot", 48) ;
+ advertise<robot_msgs::MocapSnapshot>("phase_space_snapshot", 48) ;
}
PhaseSpaceNode::~PhaseSpaceNode()
@@ -88,7 +88,7 @@
while (ok()) // While the node has not been shutdown
{
usleep(1) ;
- PhaseSpaceSnapshot snapshot ;
+ robot_msgs::MocapSnapshot snapshot ;
snapshot.header.frame_id = "phase_space" ;
@@ -112,7 +112,7 @@
return true ;
}
-int PhaseSpaceNode::grabMarkers(PhaseSpaceSnapshot& snapshot)
+int PhaseSpaceNode::grabMarkers(robot_msgs::MocapSnapshot& snapshot)
{
OWLMarker markers[MAX_NUM_MARKERS] ;
@@ -147,7 +147,7 @@
return n ;
}
-int PhaseSpaceNode::grabBodies(PhaseSpaceSnapshot& snapshot)
+int PhaseSpaceNode::grabBodies(robot_msgs::MocapSnapshot& snapshot)
{
OWLRigid bodies[MAX_NUM_BODIES] ;
int n = owlGetRigids(bodies, MAX_NUM_BODIES) ;
@@ -173,7 +173,7 @@
return n ;
}
-void PhaseSpaceNode::grabTime(PhaseSpaceSnapshot& snapshot)
+void PhaseSpaceNode::grabTime(robot_msgs::MocapSnapshot& snapshot)
{
int timeVal[3] ;
timeVal[0] = 0 ;
@@ -193,7 +193,7 @@
owlDone() ; // OWL API-call to perform system cleanup before client termination
}
-void PhaseSpaceNode::copyMarkerToMessage(const OWLMarker& owl_marker, PhaseSpaceMarker& msg_marker)
+void PhaseSpaceNode::copyMarkerToMessage(const OWLMarker& owl_marker, robot_msgs::MocapMarker& msg_marker)
{
msg_marker.id = owl_marker.id ;
@@ -204,7 +204,7 @@
msg_marker.condition = owl_marker.cond ;
}
-void PhaseSpaceNode::copyBodyToMessage(const OWLRigid& owl_body, PhaseSpaceBody& msg_body)
+void PhaseSpaceNode::copyBodyToMessage(const OWLRigid& owl_body, robot_msgs::MocapBody& msg_body)
{
msg_body.id = owl_body.id ;
@@ -236,7 +236,7 @@
printf("%s: 0x%x\n", s, n);
}
-void PhaseSpaceNode::dispSnapshot(const PhaseSpaceSnapshot& s)
+void PhaseSpaceNode::dispSnapshot(const robot_msgs::MocapSnapshot& s)
{
printf("rosTF_frame: %s Frame #%u\n", s.header.frame_id.c_str(), s.frameNum) ;
printf(" Time: %lf\n", s.header.stamp.to_double()) ;
Modified: pkg/trunk/drivers/phase_space/phase_space_node.h
===================================================================
--- pkg/trunk/drivers/phase_space/phase_space_node.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/phase_space/phase_space_node.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -48,9 +48,9 @@
#include "owl.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
namespace phase_space
{
@@ -68,13 +68,13 @@
bool spin() ;
private :
- int grabMarkers(PhaseSpaceSnapshot& snapshot) ;
- int grabBodies (PhaseSpaceSnapshot& snapshot) ;
- void grabTime (PhaseSpaceSnapshot& snapshot) ;
- void copyMarkerToMessage(const OWLMarker& owl_marker, PhaseSpaceMarker& msg_marker) ;
- void copyBodyToMessage(const OWLRigid& owl_body, PhaseSpaceBody& msg_body) ;
+ int grabMarkers(robot_msgs::MocapSnapshot& snapshot) ;
+ int grabBodies (robot_msgs::MocapSnapshot& snapshot) ;
+ void grabTime (robot_msgs::MocapSnapshot& snapshot) ;
+ void copyMarkerToMessage(const OWLMarker& owl_marker, robot_msgs::MocapMarker& msg_marker) ;
+ void copyBodyToMessage(const OWLRigid& owl_body, robot_msgs::MocapBody& msg_body) ;
void owlPrintError(const char *s, int n) ;
- void dispSnapshot(const PhaseSpaceSnapshot& snapshot) ;
+ void dispSnapshot(const robot_msgs::MocapSnapshot& snapshot) ;
} ;
}
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml 2009-01-16 09:10:25 UTC (rev 9536)
@@ -5,6 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
<depend package="std_msgs" />
+ <depend package="robot_msgs" />
<depend package="phase_space" />
<depend package="tf" />
<depend package="pr2_mechanism_controllers" />
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-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -35,15 +35,15 @@
//! \author Sachin Chitta
/****
- * This node takes the PhaseSpaceSnapshot packet and repackages into a form that can be used with the odometry
+ * This node takes the MocapSnapshot packet and repackages into a form that can be used with the odometry
*/
#include "ros/node.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
#include "std_msgs/Transform.h"
#include "std_msgs/RobotBase2DOdom.h"
@@ -101,7 +101,7 @@
{
if (snapshot_.bodies[i].id == base_id_) // Check if we found the robot base in the list of rigid bodies
{
- const phase_space::PhaseSpaceBody& body = snapshot_.bodies[0] ;
+ const robot_msgs::MocapBody& body = snapshot_.bodies[0] ;
// Build Transform Message
tf::Transform mytf;
@@ -150,7 +150,7 @@
private:
- phase_space::PhaseSpaceSnapshot snapshot_;
+ robot_msgs::MocapSnapshot snapshot_;
std_msgs::RobotBase2DOdom m_currentPos;
tf::TransformBroadcaster *m_tfServer;
unsigned int publish_success_count_ ;
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -35,15 +35,15 @@
//! \author Vijay Pradeep
/****
- * This node takes the PhaseSpaceSnapshot packet and repackages into a form that can be used with the odometry
+ * This node takes the MocapSnapshot packet and repackages into a form that can be used with the odometry
*/
#include "ros/node.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
#include "std_msgs/TransformWithRateStamped.h"
@@ -84,7 +84,7 @@
private :
- phase_space::PhaseSpaceSnapshot snapshot_ ;
+ robot_msgs::MocapSnapshot snapshot_ ;
int publish_count_ ;
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -35,7 +35,7 @@
//! \author Vijay Pradeep
/****
- * This node takes the PhaseSpaceSnapshot packet and repackages to work with the pan_tilt tracker
+ * This node takes the MocapSnapshot packet and repackages to work with the pan_tilt tracker
*/
#include <string>
@@ -43,9 +43,9 @@
#include "ros/node.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
#include "std_msgs/PointStamped.h"
@@ -79,7 +79,7 @@
if (snapshot_.get_markers_size() == 0)
return ;
- phase_space::PhaseSpaceMarker& cur_marker = snapshot_.markers[0] ;
+ robot_msgs::MocapMarker& cur_marker = snapshot_.markers[0] ;
std_msgs::PointStamped target_point ;
target_point.header = snapshot_.header ;
@@ -93,7 +93,7 @@
private :
const string topic ;
- phase_space::PhaseSpaceSnapshot snapshot_ ;
+ robot_msgs::MocapSnapshot snapshot_ ;
int publish_count_ ;
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-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -35,7 +35,7 @@
//! \author Vijay Pradeep
/****
- * This node takes the PhaseSpaceSnapshot packet and repackages a marker as a point_stamped message
+ * This node takes the MocapSnapshot packet and repackages a marker as a point_stamped message
*/
#include <string>
@@ -43,9 +43,9 @@
#include "ros/node.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
#include "std_msgs/PoseStamped.h"
@@ -57,12 +57,12 @@
{
/**
- * \brief Listens for a PhaseSpaceSnapshot message and then publishes it as a PoseStamped message, based on a
+ * \brief Listens for a MocapSnapshot message and then publishes it as a PoseStamped message, based on a
* series a parameters.
* @section topic ROS topics
* Subscribes to (name [type]):
- * - @b "phase_space_snapshot" [phase_space/PhaseSpaceSnapshot] : The current state of the phasespace system,
- * which is normally published by phase_space::PhaseSpaceNode
+ * - @b "phase_space_snapshot" [phase_space/MocapSnapshot] : The current state of the phasespace system,
+ * which is normally published by robot_msgs::MocapSnapshot
*
* Publishes to (name [type]):
* - @b "cmd" [std_msgs/PoseStamped] : The commanded pose, with an associated timestamp and frame_id. You will
@@ -134,7 +134,7 @@
{
if (snapshot_.bodies[i].id == body_id_) // Did we find our body?
{
- const phase_space::PhaseSpaceBody& cur_body = snapshot_.bodies[i] ;
+ const robot_msgs::MocapBody& cur_body = snapshot_.bodies[i] ;
// Define our starting frame
tf::Quaternion rot_phasespace ;
@@ -166,7 +166,7 @@
}
private :
- phase_space::PhaseSpaceSnapshot snapshot_ ;
+ robot_msgs::MocapSnapshot snapshot_ ;
tf::Transform transform_ ;
Modified: pkg/trunk/mapping/collision_map/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,9 +1,83 @@
cmake_minimum_required(VERSION 2.4.6)
+set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROS_BUILD_TYPE RelWithDebInfo)
rospack(collision_map)
-add_definitions(-Wall -O3 -DNDEBUG -march=pentium4 -mfpmath=sse -mmmx -msse -msse2 -msse3)
+#add_definitions(-Wall -O3 -DNDEBUG -march=pentium4 -mfpmath=sse -mmmx -msse -msse2 -msse3)
+add_definitions(-Wall -O3 -DNDEBUG)
+
+include(CheckIncludeFile)
+include(CheckLibraryExists)
+include(CheckCXXSourceRuns)
+include(CheckCXXCompilerFlag)
+
+# check for SSE extensions
+if( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX )
+ set(SSE_FLAGS)
+
+ set(CMAKE_REQUIRED_FLAGS "-msse2")
+ check_cxx_source_runs("
+ #include <emmintrin.h>
+
+ int main()
+ {
+ __m128d a, b;
+ double vals[2] = {0};
+ a = _mm_loadu_pd(vals);
+ b = _mm_add_pd(a,a);
+ _mm_storeu_pd(vals,b);
+ return 0;
+ }"
+ HAS_SSE2_EXTENSIONS)
+
+ set(CMAKE_REQUIRED_FLAGS "-msse")
+ check_cxx_source_runs("
+ #include <xmmintrin.h>
+ int main()
+ {
+ __m128 a, b;
+ float vals[4] = {0};
+ a = _mm_loadu_ps(vals);
+ b = a;
+ b = _mm_add_ps(a,b);
+ _mm_storeu_ps(vals,b);
+ return 0;
+ }"
+ HAS_SSE_EXTENSIONS)
+
+ set(CMAKE_REQUIRED_FLAGS)
+
+ if(HAS_SSE2_EXTENSIONS)
+ message(STATUS "Using SSE2 extensions")
+ set(SSE_FLAGS "-msse2 -mfpmath=sse")
+ elseif(HAS_SSE_EXTENSIONS)
+ message(STATUS "Using SSE extensions")
+ set(SSE_FLAGS "-msse -mfpmath=sse")
+ endif()
+
+ add_definitions(${SSE_FLAGS})
+elseif(MSVC)
+ check_cxx_source_runs("
+ #include <emmintrin.h>
+
+ int main()
+ {
+ __m128d a, b;
+ double vals[2] = {0};
+ a = _mm_loadu_pd(vals);
+ b = _mm_add_pd(a,a);
+ _mm_storeu_pd(vals,b);
+ return 0;
+ }"
+ HAS_SSE2_EXTENSIONS)
+ if( HAS_SSE2_EXTENSIONS )
+ message(STATUS "Using SSE2 extensions")
+ add_definitions( "/arch:SSE2 /fp:fast -D__SSE__ -D__SSE2__" )
+ endif()
+endif()
+
+
genmsg()
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
Modified: pkg/trunk/mapping/collision_map/src/collision_map.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/mapping/collision_map/src/collision_map.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -172,7 +172,7 @@
ROS_ERROR ("TF not running or wrong TF frame specified! Defaulting to 0,0,0.");
torso_lift_origin = base_origin;
}
- ROS_INFO ("Received %d data points.", cloud_.pts.size ());
+ ROS_INFO ("Received %u data points.", (unsigned int)cloud_.pts.size ());
timeval t1, t2;
gettimeofday (&t1, NULL);
@@ -271,7 +271,7 @@
gettimeofday (&t2, NULL);
double time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
- ROS_INFO ("Collision map computed in %g seconds. Number of boxes: %d.", time_spent, c_map_.boxes.size ());
+ ROS_INFO ("Collision map computed in %g seconds. Number of boxes: %u.", time_spent, (unsigned int)c_map_.boxes.size ());
publish ("collision_map", c_map_);
}
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -146,6 +146,6 @@
void sigint_handler(int)
{
- s_sessionserver->selfDestruct();
- s_pmasternode->selfDestruct();
+ s_sessionserver->shutdown();
+ s_pmasternode->shutdown();
}
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -175,7 +175,7 @@
void Destroy()
{
- selfDestruct();
+ shutdown();
node* pnode = node::instance();
if( pnode == NULL )
@@ -228,7 +228,7 @@
};
}
- virtual void selfDestruct()
+ virtual void shutdown()
{
_ok = false;
boost::mutex::scoped_lock lockcreate(_mutexViewer);
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2009-01-16 09:10:25 UTC (rev 9536)
@@ -18,9 +18,7 @@
</Body>
</Kinbody>
- <Kinbody file="willow_table.kinbody.xml">
- <translation>2 0 0</translation>
- </Kinbody>
+ <Kinbody file="willow_table.kinbody.xml"/>
<Kinbody name="ricebox0" file="ricebox.kinbody.xml">
<rotationmat>0 0 -1 0 1 0 1 0 0</rotationmat>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,4 +1,7 @@
#!/usr/bin/env octave
+[status,rosoctpath] = system(['rospack find rosoct']);
+rosoctpath = strtrim(rosoctpath);
+addpath(fullfile(rosoctpath, 'octave'));
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
global updir probs
@@ -26,7 +29,7 @@
end
%% phase space system
-out = orProblemSendCommand('createsystem PhaseSpace phase_space_snapshot',probs.task);
+out = orProblemSendCommand('createsystem ROSMocap phase_space_snapshot',probs.task);
if( isempty(out) )
error('failed to create phasespace');
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runmanip_sim.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runmanip_sim.m 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runmanip_sim.m 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,5 +1,8 @@
#!/usr/bin/env octave
global updir probs
+[status,rosoctpath] = system(['rospack find rosoct']);
+rosoctpath = strtrim(rosoctpath);
+addpath(fullfile(rosoctpath, 'octave'));
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
startup;
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,4 +1,7 @@
#!/usr/bin/env octave
+[status,rosoctpath] = system(['rospack find rosoct']);
+rosoctpath = strtrim(rosoctpath);
+addpath(fullfile(rosoctpath, 'octave'));
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
global updir probs
@@ -26,7 +29,7 @@
end
%% phase space system
-out = orProblemSendCommand('createsystem PhaseSpace phase_space_snapshot',probs.task);
+out = orProblemSendCommand('createsystem ROSMocap phase_space_snapshot',probs.task);
if( isempty(out) )
error('failed to create phasespace');
end
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-16 09:10:25 UTC (rev 9536)
@@ -6,7 +6,6 @@
<license>BSD</license>
<depend package="roscpp"/>
<depend package="openrave"/>
- <depend package="phase_space"/>
<depend package="openrave_robot_description"/>
<depend package="libKinematics"/>
<depend package="boost"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -207,7 +207,7 @@
GetEnv()->LockPhysics(false);
- MocapData* pdata = new MocapData();
+ XMLData* pdata = new XMLData();
pdata->strOffsetLink = pbody->GetLinks().front()->GetName();
BODY* b = AddKinBody(pbody, pdata);
if( b == NULL ) {
Copied: pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h (from rev 9530, pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h)
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -0,0 +1,98 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Rosen Diankov
+// 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.
+// * The name of the author may not 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 PROF...
[truncated message content] |
|
From: <tpr...@us...> - 2009-01-16 17:27:29
|
Revision: 9539
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9539&view=rev
Author: tpratkanis
Date: 2009-01-16 17:27:18 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
Change the locking in highlevel controllers to make it less restrictive. Add map accessors.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.cpp
pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2009-01-16 17:27:18 UTC (rev 9539)
@@ -466,7 +466,7 @@
State state; /*!< Tracks the overall state of the controller */
double controllerCycleTime_; /*!< The duration for each control cycle */
double plannerCycleTime_; /*!< The duration for each planner cycle. */
- boost::mutex lock_; /*!< Lock for access to class members in callbacks */
+ boost::recursive_mutex lock_; /*!< Lock for access to class members in callbacks */
boost::thread* plannerThread_; /*!< Thread running the planner loop */
highlevel_controllers::Ping shutdownMsg_; /*!< For receiving shutdown from executive */
ros::Duration timeout; /*< The time limit for planning failure. */
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2009-01-16 17:27:18 UTC (rev 9539)
@@ -100,7 +100,7 @@
* @brief Accessor for the cost map. Use mainly for initialization
* of specialized map strunture for planning
*/
- const CostMap2D& getCostMap() const {return *costMap_;}
+ const CostMapAccessor& getCostMap() const {return *global_map_accessor_;}
/**
* @brief A handler to be over-ridden in the derived class to handle a diff stream from the
@@ -128,7 +128,7 @@
*/
void updatePlan(ompl::waypoint_plan_t const & newPlan);
- void updateCostMap(bool static_map_reset);
+ void updateCostMap();
/**
* @brief test the current plan for collisions with obstacles
@@ -239,9 +239,9 @@
VelocityController* controller_;
CostMap2D* costMap_; /**< The cost map mainatined incrementally from laser scans */
+ CostMapAccessor* global_map_accessor_; /**< Read-only access to global cost map */
+ CostMapAccessor* local_map_accessor_; /**< Read-only access to a window on the cost map */
- CostMapAccessor* ma_; /**< Sliding read-only window on the cost map */
-
tf::Stamped<tf::Pose> global_pose_; /**< The global pose in the map frame */
std_msgs::RobotBase2DOdom base_odom_; /**< Odometry in the base frame */
@@ -271,6 +271,8 @@
robot_filter::RobotFilter* filter_;
tf::MessageNotifier<std_msgs::PointCloud>* tiltLaserNotifier_;
+ //flag for reseting the costmap.
+ bool reset_cost_map_;
//ground plane extraction
ransac_ground_plane_extraction::RansacGroundPlaneExtraction ground_plane_extractor_;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2009-01-16 17:27:18 UTC (rev 9539)
@@ -55,12 +55,13 @@
tf_(*this, true, 10000000000ULL), // cache for 10 sec, no extrapolation
controller_(NULL),
costMap_(NULL),
- ma_(NULL),
+ local_map_accessor_(NULL),
+ global_map_accessor_(NULL),
baseLaserMaxRange_(10.0),
tiltLaserMaxRange_(10.0),
minZ_(0.02), maxZ_(2.0), robotWidth_(0.0), active_(true) , map_update_frequency_(10.0),
yaw_goal_tolerance_(0.1),
- xy_goal_tolerance_(robotWidth_ / 2)
+ xy_goal_tolerance_(robotWidth_ / 2), reset_cost_map_(false)
{
// Initialize global pose. Will be set in control loop based on actual data.
global_pose_.setIdentity();
@@ -244,7 +245,8 @@
ROS_ASSERT(mapSize <= costMap_->getWidth());
ROS_ASSERT(mapSize <= costMap_->getHeight());
- ma_ = new CostMapAccessor(*costMap_, mapSize, 0.0, 0.0);
+ global_map_accessor_ = new CostMapAccessor(*costMap_);
+ local_map_accessor_ = new CostMapAccessor(*costMap_, mapSize, 0.0, 0.0);
std_msgs::Point2DFloat32 pt;
//create a square footprint
@@ -266,7 +268,7 @@
pt.y = 0;
footprint_.push_back(pt);
- controller_ = new ros::highlevel_controllers::TrajectoryRolloutController(&tf_, *ma_,
+ controller_ = new ros::highlevel_controllers::TrajectoryRolloutController(&tf_, *local_map_accessor_,
sim_time,
sim_steps,
samples_per_dim,
@@ -327,9 +329,13 @@
if(controller_ != NULL)
delete controller_;
- if(ma_ != NULL)
- delete ma_;
+ if(local_map_accessor_ != NULL)
+ delete local_map_accessor_;
+
+ if(global_map_accessor_ != NULL)
+ delete global_map_accessor_;
+
if(costMap_ != NULL)
delete costMap_;
@@ -367,7 +373,7 @@
global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
ROS_DEBUG("Received new position (x=%f, y=%f, th=%f)", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw);
- ma_->updateForRobotPosition(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
+ local_map_accessor_->updateForRobotPosition(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
}
@@ -376,7 +382,7 @@
*/
void MoveBase::updateGoalMsg(){
// Revert to static map on new goal. May result in oscillation, but requested by Eitan for the milestone
- updateCostMap(true);
+ reset_cost_map_ = true;
tf::Stamped<tf::Pose> goalPose, transformedGoalPose;
btQuaternion qt;
@@ -513,7 +519,7 @@
*/
void MoveBase::handlePlanningFailure(){
ROS_DEBUG("No plan found. Handling planning failure");
- updateCostMap(true);
+ reset_cost_map_ = true;
stopRobot();
}
@@ -665,9 +671,6 @@
bool planOk = checkWatchDog() && isValid();
std_msgs::BaseVel cmdVel; // Commanded velocities
- // Update the cost map window
- ma_->updateForRobotPosition(global_pose_.getOrigin().getX(), global_pose_.getOrigin().getY());
-
// if we have achieved all our waypoints but have yet to achieve the goal, then we know that we wish to accomplish our desired
// orientation
if(planOk && plan_.empty()){
@@ -758,12 +761,9 @@
/**
* @brief Utility to output local obstacles. Make the local cost map accessor. It is very cheap :-) Then
- * render the obstacles.
+ * render the obstacles. Note that the rendered window is typically larger than the local map for control
*/
void MoveBase::publishLocalCostMap() {
-
-
-
double mapSize = std::min(costMap_->getWidth()/2, costMap_->getHeight()/2);
CostMapAccessor cm(*costMap_, std::min(10.0, mapSize), global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
@@ -898,26 +898,40 @@
MoveBase::footprint_t const & MoveBase::getFootprint() const{
return footprint_;
}
+
+ void MoveBase::updateCostMap() {
+ if (reset_cost_map_) {
+ costMap_->revertToStaticMap(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
+ }
- void MoveBase::updateCostMap(bool static_map_reset){
ROS_DEBUG("Starting cost map update/n");
- if(static_map_reset)
- costMap_->revertToStaticMap(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
-
- // Aggregate buffered observations across 3 sources
+
+ lock();
+ // Aggregate buffered observations across all sources. Must be thread safe
std::vector<costmap_2d::Observation> observations;
baseScanBuffer_->get_observations(observations);
tiltScanBuffer_->get_observations(observations);
lowObstacleBuffer_->get_observations(observations);
stereoCloudBuffer_->get_observations(observations);
-
+ unlock();
+
ROS_DEBUG("Applying update with %d observations/n", observations.size());
// Apply to cost map
ros::Time start = ros::Time::now();
costMap_->updateDynamicObstacles(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), observations);
double t_diff = (ros::Time::now() - start).toSec();
+ ROS_DEBUG("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
+
+ // Finally, we must extract the cost data that we have computed and:
+ // 1. Refresh the local_map_accessor for the controller
+ // 2. Refresh the global_map accessor for the planner
+ // 3. Publish the local cost map window
+ lock();
+ local_map_accessor_->refresh();
+ global_map_accessor_->refresh();
publishLocalCostMap();
- ROS_DEBUG("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
+ reset_cost_map_ = false;
+ unlock();
}
/**
@@ -932,10 +946,7 @@
while (active_){
//Avoids laser race conditions.
if (isInitialized()) {
- //update the cost map without resetting to static map
- lock();
- updateCostMap(false);
- unlock();
+ updateCostMap();
}
d->sleep();
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-01-16 17:27:18 UTC (rev 9539)
@@ -167,7 +167,7 @@
ompl::createIndexTransformWrap(&getCostMap()), true,
0, 0, 0, 0,
CostMap2D::INSCRIBED_INFLATED_OBSTACLE);
- }
+ }
else if ("3DKIN" == environmentType) {
string const prefix("env3d/");
string obst_cost_thresh_str;
@@ -248,7 +248,7 @@
}
bool MoveBaseSBPL::isMapDataOK() {
- const CostMap2D& cm = getCostMap();
+ const CostMapAccessor& cm = getCostMap();
for(unsigned int i = 0; i<cm.getWidth(); i++){
for(unsigned int j = 0; j < cm.getHeight(); j++){
@@ -273,7 +273,7 @@
try {
// Update costs
lock();
- const CostMap2D& cm = getCostMap();
+ const CostMapAccessor& cm = getCostMap();
unsigned int x = cm.getWidth();
while(x > 0){
x--;
Modified: pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.cpp 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.cpp 2009-01-16 17:27:18 UTC (rev 9539)
@@ -43,7 +43,7 @@
class cm2dCostmapWrap: public ompl::CostmapWrap {
public:
- cm2dCostmapWrap(costmap_2d::CostMap2D const * cm): cm_(cm) {}
+ cm2dCostmapWrap(costmap_2d::CostMapAccessor const * cm): cm_(cm) {}
virtual int getWSpaceObstacleCost() const { return costmap_2d::CostMap2D::INSCRIBED_INFLATED_OBSTACLE; }
virtual int getCSpaceObstacleCost() const { return costmap_2d::CostMap2D::LETHAL_OBSTACLE; }
@@ -82,13 +82,13 @@
return true;
}
- costmap_2d::CostMap2D const * cm_;
+ costmap_2d::CostMapAccessor const * cm_;
};
class cm2dTransformWrap: public ompl::IndexTransformWrap {
public:
- cm2dTransformWrap(costmap_2d::CostMap2D const * cm): cm_(cm) {}
+ cm2dTransformWrap(costmap_2d::CostMapAccessor const * cm): cm_(cm) {}
virtual void globalToIndex(double global_x, double global_y, ssize_t * index_x, ssize_t * index_y) const {
unsigned int ix, iy;
@@ -102,7 +102,7 @@
virtual double getResolution() const { return cm_->getResolution(); }
- costmap_2d::CostMap2D const * cm_;
+ costmap_2d::CostMapAccessor const * cm_;
};
@@ -173,14 +173,14 @@
namespace ompl {
- CostmapWrap * createCostmapWrap(costmap_2d::CostMap2D const * cm)
+ CostmapWrap * createCostmapWrap(costmap_2d::CostMapAccessor const * cm)
{ return new cm2dCostmapWrap(cm); }
#warning 'Using RDTravmap instead of a raw TraversabilityMap is a big performance hit!'
CostmapWrap * createCostmapWrap(sfl::RDTravmap const * rdt)
{ return new sflCostmapWrap(rdt); }
- IndexTransformWrap * createIndexTransformWrap(costmap_2d::CostMap2D const * cm)
+ IndexTransformWrap * createIndexTransformWrap(costmap_2d::CostMapAccessor const * cm)
{ return new cm2dTransformWrap(cm); }
IndexTransformWrap * createIndexTransformWrap(sfl::GridFrame const * gf)
Modified: pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.h
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.h 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/motion_planning/sbpl_util/src/costmap_wrap.h 2009-01-16 17:27:18 UTC (rev 9539)
@@ -41,6 +41,7 @@
namespace costmap_2d {
class CostMap2D;
+ class CostMapAccessor;
}
namespace sfl {
@@ -105,10 +106,10 @@
typedef GenericIndexTransformWrap<ssize_t> IndexTransformWrap;
- CostmapWrap * createCostmapWrap(costmap_2d::CostMap2D const * cm);
+ CostmapWrap * createCostmapWrap(costmap_2d::CostMapAccessor const * cm);
CostmapWrap * createCostmapWrap(sfl::RDTravmap const * rdt);
- IndexTransformWrap * createIndexTransformWrap(costmap_2d::CostMap2D const * cm);
+ IndexTransformWrap * createIndexTransformWrap(costmap_2d::CostMapAccessor const * cm);
IndexTransformWrap * createIndexTransformWrap(sfl::GridFrame const * gf);
}
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2009-01-16 17:27:18 UTC (rev 9539)
@@ -316,10 +316,21 @@
CostMapAccessor(const CostMap2D& costMap, double maxSize, double pose_x, double pose_y);
/**
+ * @broef Constructor
+ * Use when we want to access the whole cost map rather than a window
+ */
+ CostMapAccessor(const CostMap2D& costMap);
+
+ /**
* @brief Set the pose for the robot. Will adjust other parameters accordingly.
*/
void updateForRobotPosition(double wx, double wy);
+ /**
+ * @brief Refresh the copied cost data structure from the source cost map
+ */
+ void refresh();
+
private:
static unsigned int computeSize(double maxSize, double resolution);
Modified: pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-16 17:27:18 UTC (rev 9539)
@@ -99,6 +99,7 @@
Observation obs(o, map_cloud);
buffer_observation(obs);
map_cloud = NULL;
+ newData = NULL;
}
// In case we get thrown out on the second transform - clean up
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2009-01-16 17:27:18 UTC (rev 9539)
@@ -536,20 +536,32 @@
CostMapAccessor::CostMapAccessor(const CostMap2D& costMap, double maxSize, double poseX, double poseY)
: ObstacleMapAccessor(computeWX(costMap, maxSize, poseX, poseY),
- computeWY(costMap, maxSize, poseX, poseY),
- computeSize(maxSize, costMap.getResolution()),
- computeSize(maxSize, costMap.getResolution()),
- costMap.getResolution()), costMap_(costMap),
- maxSize_(maxSize){
+ computeWY(costMap, maxSize, poseX, poseY),
+ computeSize(maxSize, costMap.getResolution()),
+ computeSize(maxSize, costMap.getResolution()),
+ costMap.getResolution()),
+ costMap_(costMap), maxSize_(maxSize){
- setCircumscribedCostLowerBound(costMap.getCircumscribedCostLowerBound());
+ setCircumscribedCostLowerBound(costMap.getCircumscribedCostLowerBound());
- // Set robot position
- updateForRobotPosition(poseX, poseY);
+ // Set robot position
+ updateForRobotPosition(poseX, poseY);
- ROS_DEBUG_NAMED("costmap_2d", "Creating Local %d X %d Map\n", getWidth(), getHeight());
- }
+ ROS_DEBUG_NAMED("costmap_2d", "Creating Local %d X %d Map\n", getWidth(), getHeight());
+ }
+ CostMapAccessor::CostMapAccessor(const CostMap2D& costMap)
+ : ObstacleMapAccessor(0.0, 0.0, costMap.getWidth(), costMap.getHeight(), costMap.getResolution()),
+ costMap_(costMap), maxSize_(0.0), mx_0_(0), my_0_(0){
+
+ setCircumscribedCostLowerBound(costMap.getCircumscribedCostLowerBound());
+
+ // Set values from source
+ refresh();
+
+ ROS_DEBUG_NAMED("costmap_2d", "Creating Local %d X %d Map\n", getWidth(), getHeight());
+ }
+
void CostMapAccessor::updateForRobotPosition(double wx, double wy){
if(wx < 0 || wx > costMap_.getResolution() * costMap_.getWidth())
return;
@@ -559,11 +571,22 @@
origin_x_ = computeWX(costMap_, maxSize_, wx, wy);
origin_y_ = computeWY(costMap_, maxSize_, wx, wy);
- costMap_.WC_MC(origin_x_, origin_y_, mx_0_, my_0_);
+ costMap_.WC_MC(origin_x_, origin_y_, mx_0_, my_0_);
ROS_DEBUG( "Moving map to locate at <%f, %f> and size of %f meters for position <%f, %f>\n",
origin_x_, origin_y_, maxSize_, wx, wy);
+ refresh();
+ }
+
+
+ unsigned int CostMapAccessor::computeSize(double maxSize, double resolution){
+ unsigned int cellWidth = (unsigned int) ceil(maxSize/resolution);
+ ROS_DEBUG("Given a size of %f and a resolution of %f, we have a cell width of %d\n", maxSize, resolution, cellWidth);
+ return cellWidth;
+ }
+
+ void CostMapAccessor::refresh(){
// Now update all the cells from the cost map
for(unsigned int x = 0; x < width_; x++){
for (unsigned int y = 0; y < height_; y++){
@@ -574,14 +597,4 @@
}
}
-
- unsigned int CostMapAccessor::computeSize(double maxSize, double resolution){
- unsigned int cellWidth = (unsigned int) ceil(maxSize/resolution);
- ROS_DEBUG("Given a size of %f and a resolution of %f, we have a cell width of %d\n", maxSize, resolution, cellWidth);
- return cellWidth;
- }
-
-
-
-
}
Modified: pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-16 09:24:35 UTC (rev 9538)
+++ pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-16 17:27:18 UTC (rev 9539)
@@ -77,8 +77,17 @@
// observation.cloud_->header.stamp.toSec(), last_updated_.toSec());
}
- // If the duration is 0, then we just keep the latest one, so we clear out all existing observations
- while(!buffer_.empty()){
+ // Otherwise just store it and indicate success
+ buffer_.push_back(observation);
+ return true;
+ }
+
+ void ObservationBuffer::get_observations(std::vector<Observation>& observations){
+ // If the duration is 0, then we just keep the latest one, so we must have a limit of one.
+ // If the duration is non-zero, we want to delete all the observations.
+ unsigned int min_observations = (keep_alive_ == 0) ? 1 : 0;
+
+ while(buffer_.size() > min_observations){
std::list<Observation>::iterator it = buffer_.begin();
// Get the current one, and check if still alive. if so
Observation& obs = *it;
@@ -90,12 +99,6 @@
break;
}
- // Otherwise just store it and indicate success
- buffer_.push_back(observation);
- return true;
- }
-
- void ObservationBuffer::get_observations(std::vector<Observation>& observations){
// Add all remaining observations to the output
for(std::list<Observation>::const_iterator it = buffer_.begin(); it != buffer_.end(); ++it){
observations.push_back(*it);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2009-01-16 19:09:44
|
Revision: 9547
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9547&view=rev
Author: jleibs
Date: 2009-01-16 19:09:38 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
Moving topic_synchronizer over to boost threads.
Modified Paths:
--------------
pkg/trunk/util/topic_synchronizer/manifest.xml
pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
pkg/trunk/vision/stereo_view/stereo_view.cpp
pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
Modified: pkg/trunk/util/topic_synchronizer/manifest.xml
===================================================================
--- pkg/trunk/util/topic_synchronizer/manifest.xml 2009-01-16 18:37:07 UTC (rev 9546)
+++ pkg/trunk/util/topic_synchronizer/manifest.xml 2009-01-16 19:09:38 UTC (rev 9547)
@@ -7,7 +7,7 @@
<depend package="std_msgs" />
<depend package="rosthread" />
<export>
- <cpp cflags="-I${prefix}"/>
+ <cpp cflags="-I${prefix} -D__STDC_LIMIT_MACROS -DBOOST_DATE_TIME_POSIX_TIME_STD_CONFIG"/>
</export>
<url>http://pr.willowgarage.com/wiki/topic_synchronizer</url>
</package>
Modified: pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
===================================================================
--- pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-01-16 18:37:07 UTC (rev 9546)
+++ pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-01-16 19:09:38 UTC (rev 9547)
@@ -39,8 +39,13 @@
#ifndef TOPIC_SYNCHRONIZER_HH
#define TOPIC_SYNCHRONIZER_HH
-#include "rosthread/mutex.h"
-#include "rosthread/condition.h"
+
+#include <boost/thread.hpp>
+#include "boost/date_time/posix_time/posix_time.hpp"
+
+//#include "rosthread/mutex.h"
+//#include "rosthread/condition.h"
+
#include "ros/time.h"
//! A templated class for synchronizing incoming topics
@@ -56,7 +61,8 @@
class UnsubscribeList
{
std::list<std::string> list_;
- ros::thread::mutex list_mutex_;
+ boost::mutex list_mutex_;
+ // ros::thread::mutex list_mutex_;
public:
UnsubscribeList(std::list<std::string>& l) : list_(l) { }
@@ -106,13 +112,16 @@
void (N::*callback_)(ros::Time);
//! Timeout duration
- ros::Duration timeout_;
+ boost::posix_time::time_duration timeout_;
+ // ros::Duration timeout_;
//! The callback to be called if timed out
void (N::*timeout_callback_)(ros::Time);
- //! The condition variable used for synchronization
- ros::thread::condition cond_all_;
+ //! The condition variable and mutex used for synchronization
+ boost::condition_variable cond_all_;
+ boost::mutex cond_all_mutex_;
+ // ros::thread::condition cond_all_;
//! The number of expected incoming messages
int expected_count_;
@@ -144,19 +153,21 @@
ros::Time* time = (ros::Time*)(p);
- cond_all_.lock();
+ boost::unique_lock<boost::mutex> lock(cond_all_mutex_);
+ // cond_all_mutex_.lock();
+
// If first to get message, wait for others
if (count_ == 0)
{
- wait_for_others(time);
+ wait_for_others(time, lock);
return;
}
// If behind, skip
if (*time < waiting_time_)
{
- cond_all_.unlock();
+ // cond_all_mutex_.unlock();
return;
}
@@ -166,21 +177,21 @@
count_++;
if (count_ == expected_count_)
{
- cond_all_.broadcast();
+ cond_all_.notify_all();
}
while (!done_ && *time == waiting_time_)
- cond_all_.wait();
+ cond_all_.wait(lock);
- cond_all_.unlock();
+ // cond_all_mutex_.unlock();
return;
}
// If ahead, wakeup others, and ten wait for others
if (*time > waiting_time_)
{
- cond_all_.broadcast();
- wait_for_others(time);
+ cond_all_.notify_all();
+ wait_for_others(time, lock);
}
}
@@ -200,7 +211,7 @@
/*!
* \param time The time that is being waited for
*/
- void wait_for_others(ros::Time* time)
+ void wait_for_others(ros::Time* time, boost::unique_lock<boost::mutex>& lock)
{
count_ = 1;
done_ = false;
@@ -209,7 +220,7 @@
bool timed_out = false;
while (count_ < expected_count_ && *time == waiting_time_ && !timed_out)
- if (!cond_all_.timed_wait(timeout_))
+ if (!cond_all_.timed_wait(lock, timeout_))
{
timed_out = true;
if (timeout_callback_)
@@ -223,9 +234,9 @@
{
done_ = true;
count_ = 0;
- cond_all_.broadcast();
+ cond_all_.notify_all();
}
- cond_all_.unlock();
+ // cond_all_mutex_.unlock();
}
public:
@@ -239,8 +250,10 @@
* \param timeout The duration
* \param timeout_callback A callback which is triggered when the timeout expires
*/
- TopicSynchronizer(N* node, void (N::*callback)(ros::Time), ros::Duration timeout = ros::Duration(1.0), void (N::*timeout_callback)(ros::Time) = NULL) : node_(node), callback_(callback), timeout_(timeout), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
- { }
+ TopicSynchronizer(N* node, void (N::*callback)(ros::Time), ros::Duration timeout = ros::Duration(1.0), void (N::*timeout_callback)(ros::Time) = NULL) : node_(node), callback_(callback), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
+ {
+ timeout_ = boost::posix_time::nanosec(timeout.toNSec());
+ }
//! Destructor
~TopicSynchronizer()
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-01-16 18:37:07 UTC (rev 9546)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-01-16 19:09:38 UTC (rev 9547)
@@ -50,6 +50,8 @@
#include "topic_synchronizer.h"
+#include <boost/thread.hpp>
+
using namespace std;
class StereoView : public ros::node
@@ -78,7 +80,7 @@
bool calib_color_;
bool recompand_;
- ros::thread::mutex cv_mutex;
+ boost::mutex cv_mutex;
StereoView() : ros::node("stereo_view"),
lcal(this), rcal(this), lcalimage(NULL), rcalimage(NULL),
@@ -90,20 +92,20 @@
cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
std::list<std::string> left_list;
- left_list.push_back(map_name("stereo") + std::string("/left/image_rect_color"));
- left_list.push_back(map_name("stereo") + std::string("/left/image_rect"));
+ left_list.push_back(mapName("stereo") + std::string("/left/image_rect_color"));
+ left_list.push_back(mapName("stereo") + std::string("/left/image_rect"));
std::list<std::string> right_list;
- right_list.push_back(map_name("stereo") + std::string("/right/image_rect_color"));
- right_list.push_back(map_name("stereo") + std::string("/right/image_rect"));
+ right_list.push_back(mapName("stereo") + std::string("/right/image_rect_color"));
+ right_list.push_back(mapName("stereo") + std::string("/right/image_rect"));
sync.subscribe(left_list, limage, 1);
sync.subscribe(right_list, rimage, 1);
- sync.subscribe(map_name("stereo") + "/disparity", dimage, 1);
- sync.subscribe(map_name("stereo") + "/stereo_info", stinfo, 1);
- sync.subscribe(map_name("stereo") + "/disparity_info", dinfo, 1);
+ sync.subscribe(mapName("stereo") + "/disparity", dimage, 1);
+ sync.subscribe(mapName("stereo") + "/stereo_info", stinfo, 1);
+ sync.subscribe(mapName("stereo") + "/disparity_info", dinfo, 1);
sync.ready();
}
@@ -192,8 +194,8 @@
// Fetch color calibration parameters as necessary
if (calib_color_)
{
- lcal.getFromParam(map_name("stereo") + "/left/image_rect_color");
- rcal.getFromParam(map_name("stereo") + "/right/image_rect_color");
+ lcal.getFromParam(mapName("stereo") + "/left/image_rect_color");
+ rcal.getFromParam(mapName("stereo") + "/right/image_rect_color");
}
cv_mutex.unlock();
Modified: pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-01-16 18:37:07 UTC (rev 9546)
+++ pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-01-16 19:09:38 UTC (rev 9547)
@@ -61,6 +61,8 @@
#include "topic_synchronizer.h"
+#include <boost/thread.hpp>
+
using namespace std;
struct MouseCallbackParams {
@@ -72,7 +74,7 @@
};
//ros::thread::mutex cv_mutex;
-ros::thread::mutex g_cv_mutex;
+boost::mutex g_cv_mutex;
/*!
* \brief Click on a point in the left image to get 2d, color and 3d information.
@@ -170,7 +172,7 @@
MouseCallbackParams mcbparams_; /**< Parameters for the mouse callback. */
- ros::thread::mutex cv_mutex;
+ boost::mutex cv_mutex;
StereoView() : ros::node("stereo_view"),
lcal(this), rcal(this), lcalimage(NULL), rcalimage(NULL),
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-16 20:31:53
|
Revision: 9552
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9552&view=rev
Author: sfkwc
Date: 2009-01-16 20:31:47 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
#839: deprecating in favor of dcam
Added Paths:
-----------
pkg/trunk/deprecated/videre_cam/
Removed Paths:
-------------
pkg/trunk/drivers/cam/videre_cam/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-16 20:33:19
|
Revision: 9554
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9554&view=rev
Author: sfkwc
Date: 2009-01-16 20:33:15 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
#839: deprecating in favor of dcam
Added Paths:
-----------
pkg/trunk/deprecated/dc1394_cam/
Removed Paths:
-------------
pkg/trunk/drivers/cam/dc1394_cam/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-16 20:34:09
|
Revision: 9555
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9555&view=rev
Author: sfkwc
Date: 2009-01-16 20:34:04 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
#839: deprecating in favor of dcam
Added Paths:
-----------
pkg/trunk/deprecated/dc1394_cam_server/
Removed Paths:
-------------
pkg/trunk/drivers/cam/dc1394_cam_server/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-16 22:47:16
|
Revision: 9569
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9569&view=rev
Author: sfkwc
Date: 2009-01-16 22:19:47 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
blacklisting non-cmake packages due to incremental update issues that make cannot handle
Added Paths:
-----------
pkg/trunk/drivers/input/joy_view/ROS_BUILD_BLACKLIST
pkg/trunk/manip/arm_trajectory/ROS_BUILD_BLACKLIST
pkg/trunk/vision/cv_movie_streamer/ROS_BUILD_BLACKLIST
pkg/trunk/visualization/cloud_viewer/ROS_BUILD_BLACKLIST
Added: pkg/trunk/drivers/input/joy_view/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/input/joy_view/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/drivers/input/joy_view/ROS_BUILD_BLACKLIST 2009-01-16 22:19:47 UTC (rev 9569)
@@ -0,0 +1 @@
+Blacklisting until we move it to cmake.
Added: pkg/trunk/manip/arm_trajectory/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/manip/arm_trajectory/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/manip/arm_trajectory/ROS_BUILD_BLACKLIST 2009-01-16 22:19:47 UTC (rev 9569)
@@ -0,0 +1 @@
+Blacklisting until we migrate to cmake
Added: pkg/trunk/vision/cv_movie_streamer/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/cv_movie_streamer/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/cv_movie_streamer/ROS_BUILD_BLACKLIST 2009-01-16 22:19:47 UTC (rev 9569)
@@ -0,0 +1 @@
+Blacklisting until we migrate it to CMake. make does not handle the removing of header file dependencies well.
Added: pkg/trunk/visualization/cloud_viewer/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/visualization/cloud_viewer/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/visualization/cloud_viewer/ROS_BUILD_BLACKLIST 2009-01-16 22:19:47 UTC (rev 9569)
@@ -0,0 +1 @@
+Blacklisting until we migrate to cmake
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-01-16 22:47:57
|
Revision: 9567
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9567&view=rev
Author: vijaypradeep
Date: 2009-01-16 22:04:43 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
Working on python arm_commander scripts for calibration
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
Added Paths:
-----------
pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py
pkg/trunk/calibration/kinematic_calibration/src/goto_next.py
Modified: pkg/trunk/calibration/kinematic_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-01-16 21:53:38 UTC (rev 9566)
+++ pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-01-16 22:04:43 UTC (rev 9567)
@@ -12,6 +12,7 @@
<depend package="image_msgs" />
<depend package="newmat10"/>
<depend package="rosthread"/>
+ <depend package="pr2_mechanism_controllers" />
<export>
<cpp cflags="-I${prefix}/msg/cpp"/>
</export>
Added: pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py 2009-01-16 22:04:43 UTC (rev 9567)
@@ -0,0 +1,105 @@
+#!/usr/bin/env python
+import rostools
+rostools.update_path('kinematic_calibration')
+
+import rospy
+import sys
+from roscpp.msg import Empty
+from pr2_mechanism_controllers.srv import *
+from pr2_mechanism_controllers.msg import *
+from robot_msgs.msg import JointCmd
+
+cmd_count = 0
+
+def goto_next_callback(data, callback_args):
+ arm_cmd = callback_args[0]
+ start_topic = callback_args[1]
+ head_cmd = callback_args[2]
+ head_pub = callback_args[3]
+
+ global cmd_count
+ print "Callback Called"
+ print " Waiting for Service:\n (%s)..." % start_topic
+ rospy.wait_for_service(start_topic)
+ print " Service Found!"
+ print " Sending Cmd #%u" % cmd_count
+ if cmd_count < len(arm_cmd) :
+ print " *** Arm Command ***"
+ print arm_cmd[cmd_count]
+ start_srv = rospy.ServiceProxy(start_topic, TrajectoryStart)
+ joint_traj = JointTraj([JointTrajPoint(arm_cmd[cmd_count],0)])
+ start_resp = start_srv.call(TrajectoryStartRequest(joint_traj,0,1))
+ print "Response:"
+ print " Traj ID: %u" % start_resp.trajectoryid
+ print " Timestamps:"
+ print start_resp.timestamps
+
+ print " *** Head Command ***"
+ print head_cmd[cmd_count]
+ head_pub.publish(JointCmd(['head_pan_joint', 'head_tilt_joint'],[0.0,0.0],head_cmd[cmd_count],[0.0, 0.0],[0.0, 0.0]))
+
+ else :
+ print "No More Commands left in queue!"
+
+
+ print "************"
+ cmd_count = cmd_count + 1
+
+
+if __name__ == '__main__':
+ print "Running python code"
+ rospy.init_node('arm_commander', sys.argv, anonymous=False)
+
+ headers_string = rospy.get_param('~joint_headers')
+ print "Got headers from param:\n%s" % headers_string
+ commands_string = rospy.get_param('~joint_commands')
+ print "Got commands from param:\n%s" % commands_string
+
+ headers = headers_string.split()
+
+ cmd_all = [[float(x) for x in cur_line.split()] for cur_line in commands_string.split("\n")]
+ cmd = [x for x in cmd_all if len(x)==len(headers)]
+ print "Commands"
+ print cmd
+
+ arm_controller = 'right_arm_trajectory_controller'
+ arm_query_topic = arm_controller + '/TrajectoryQuery'
+ arm_start_topic = arm_controller + '/TrajectoryStart'
+
+ print "Waiting Query Service: " + arm_query_topic
+ rospy.wait_for_service(arm_query_topic)
+ print "Found Query Service!"
+
+ query_srv = rospy.ServiceProxy(arm_query_topic, TrajectoryQuery)
+ query_resp = query_srv.call(TrajectoryQueryRequest(0))
+ head_publisher = rospy.Publisher('head_controller/set_command_array', JointCmd)
+
+ print "Headers"
+ print headers
+
+ print "Queried Jointnames:"
+ print query_resp.jointnames
+
+ print "***** Remapping Arm Commands *****"
+ arm_mapping = [headers.index(x) for x in query_resp.jointnames]
+ print "Mapping:"
+ print arm_mapping
+ arm_cmd = [[y[x] for x in arm_mapping] for y in cmd]
+ print "First remapped arm cmd:"
+ print arm_cmd[0]
+ print "Successfully Remapped Arm Commands"
+
+ print "***** Remapping Head Commands *****"
+ head_mapping = [headers.index(x) for x in ['HeadPan', 'HeadTilt']]
+ head_cmd = [[y[x] for x in head_mapping] for y in cmd]
+ print "First remapped head cmd:"
+ print head_cmd[0]
+ print "Successfully Remapped Head Commands"
+
+ sub = rospy.Subscriber("~goto_next", Empty, goto_next_callback,
+ [arm_cmd, arm_start_topic, head_cmd, head_publisher])
+
+ print "***** Ready to Send Trajectories *****"
+
+ rospy.spin()
+
Property changes on: pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py
___________________________________________________________________
Added: svn:executable
+ *
Added: pkg/trunk/calibration/kinematic_calibration/src/goto_next.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/goto_next.py (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/src/goto_next.py 2009-01-16 22:04:43 UTC (rev 9567)
@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+import rostools; rostools.update_path('kinematic_calibration')
+import sys
+import rospy
+from roscpp.msg import Empty
+from time import sleep
+
+if __name__ == '__main__':
+ prefix = 'arm_commander'
+ topic = prefix + '/goto_next'
+ command_publisher = rospy.Publisher(topic, Empty)
+ rospy.init_node('goto_next', anonymous=True)
+ sleep(1)
+ cmd = Empty()
+ command_publisher.publish( cmd )
+ sleep(1)
+ print 'Command sent!'
Property changes on: pkg/trunk/calibration/kinematic_calibration/src/goto_next.py
___________________________________________________________________
Added: svn:executable
+ *
Modified: pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2009-01-16 21:53:38 UTC (rev 9566)
+++ pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2009-01-16 22:04:43 UTC (rev 9567)
@@ -2,8 +2,6 @@
<param name="base_controller/odom_publish_rate" value="10" />
<!--node pkg="mechanism_control" type="mech.py" args="sp $(find pr2_gazebo)/controllers_2dnav_test.xml" respawn="false" output="screen" /-->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_default_controller.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
<!--node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_base.xml" output="screen"/-->
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_alpha)/controllers_base.xml" output="screen"/>
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_head_torso_gazebo.xml" output="screen"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-01-16 22:56:36
|
Revision: 9578
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9578&view=rev
Author: stuglaser
Date: 2009-01-16 22:56:34 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
I think we're all tired of having the "cal" topics cluttering up rostopic and rosgraphviz.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2009-01-16 22:56:30 UTC (rev 9577)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2009-01-16 22:56:34 UTC (rev 9578)
@@ -175,7 +175,14 @@
JointCalibrationControllerNode::~JointCalibrationControllerNode()
{
if (pub_calibrated_)
+ {
+ std::string topic = pub_calibrated_->topic_;
delete pub_calibrated_;
+
+ // I think we're all tired of having the "cal" topics cluttering
+ // up rostopic and rosgraphviz.
+ ros::node::instance()->unadvertise(topic);
+ }
}
void JointCalibrationControllerNode::update()
Modified: pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
===================================================================
--- pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2009-01-16 22:56:30 UTC (rev 9577)
+++ pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2009-01-16 22:56:34 UTC (rev 9578)
@@ -170,9 +170,10 @@
is_running_ = false;
}
+ std::string topic_;
+
private:
- std::string topic_;
ros::node *node_;
bool is_running_;
bool keep_running_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-17 01:01:45
|
Revision: 9599
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9599&view=rev
Author: isucan
Date: 2009-01-17 01:01:34 +0000 (Sat, 17 Jan 2009)
Log Message:
-----------
fix some deprecation warnings
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
pkg/trunk/world_models/robot_models/robot_model/include/robot_model/knode.h
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-17 00:57:31 UTC (rev 9598)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-17 01:01:34 UTC (rev 9599)
@@ -148,10 +148,10 @@
robot_model::NodeCollisionModel(dynamic_cast<ros::node*>(this),
robot_model)
{
- advertise_service("plan_kinematic_path_state", &KinematicPlanning::planToState);
- advertise_service("plan_kinematic_path_named", &KinematicPlanning::planToStateNamed);
- advertise_service("plan_kinematic_path_position", &KinematicPlanning::planToPosition);
- advertise_service("plan_joint_state_names", &KinematicPlanning::planJointNames);
+ advertiseService("plan_kinematic_path_state", &KinematicPlanning::planToState);
+ advertiseService("plan_kinematic_path_named", &KinematicPlanning::planToStateNamed);
+ advertiseService("plan_kinematic_path_position", &KinematicPlanning::planToPosition);
+ advertiseService("plan_joint_state_names", &KinematicPlanning::planJointNames);
advertise<std_msgs::String>("planning_statistics", 10);
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-17 00:57:31 UTC (rev 9598)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-17 01:01:34 UTC (rev 9599)
@@ -130,7 +130,7 @@
robot_model::NodeCollisionModel(dynamic_cast<ros::node*>(this),
robot_model)
{
- advertise_service("validate_path", &MotionValidator::validatePath);
+ advertiseService("validate_path", &MotionValidator::validatePath);
}
/** Free the memory */
Modified: pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-01-17 00:57:31 UTC (rev 9598)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-01-17 01:01:34 UTC (rev 9599)
@@ -666,7 +666,12 @@
for (unsigned int i = 0 ; i < l.size() ; ++i)
{
int gid = getGroupID(l[i]);
- out << "Group " << l[i] << " has " << groupChainStart[gid].size() << " roots" << std::endl;
+ out << "Group " << l[i] << " has " << groupChainStart[gid].size() << " roots: ";
+ for (unsigned int j = 0 ; j < groupChainStart[gid].size() ; ++j)
+ out << parameterNames[groupChainStart[gid][j]->name] << " ";
+ for (unsigned int j = 0 ; j < groupChainStart[gid].size() ; ++j)
+ out << groupChainStart[gid][j]->name << " ";
+ out << std::endl;
out << "The state components for this group are: ";
for (unsigned int j = 0 ; j < groupStateIndexList[gid].size() ; ++j)
out << groupStateIndexList[gid][j] << " ";
Modified: pkg/trunk/world_models/robot_models/robot_model/include/robot_model/knode.h
===================================================================
--- pkg/trunk/world_models/robot_models/robot_model/include/robot_model/knode.h 2009-01-17 00:57:31 UTC (rev 9598)
+++ pkg/trunk/world_models/robot_models/robot_model/include/robot_model/knode.h 2009-01-17 01:01:34 UTC (rev 9599)
@@ -174,7 +174,7 @@
if (!m_robotModelName.empty() && m_robotModelName != "-")
{
std::string content;
- if (m_node->get_param(m_robotModelName, content))
+ if (m_node->getParam(m_robotModelName, content))
setRobotDescriptionFromData(content.c_str());
else
fprintf(stderr, "Robot model '%s' not found!\n", m_robotModelName.c_str());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-17 03:23:21
|
Revision: 9612
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9612&view=rev
Author: isucan
Date: 2009-01-17 01:59:07 +0000 (Sat, 17 Jan 2009)
Log Message:
-----------
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/RKPStateValidator.h
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPStateValidator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPStateValidator.h 2009-01-17 01:55:39 UTC (rev 9611)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPStateValidator.h 2009-01-17 01:59:07 UTC (rev 9612)
@@ -60,7 +60,7 @@
if (valid)
valid = m_kce.decide();
-
+
return valid;
}
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-17 01:55:39 UTC (rev 9611)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-17 01:59:07 UTC (rev 9612)
@@ -117,8 +117,8 @@
protected:
- boost::mutex m_lock;
- bool m_selfCollision;
+ boost::mutex m_lock;
+ bool m_selfCollision;
/** List of loaded robot models */
std::vector<planning_models::KinematicModel*> m_models;
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-17 01:55:39 UTC (rev 9611)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-17 01:59:07 UTC (rev 9612)
@@ -312,6 +312,7 @@
// up using it.
dGeomID g1 = m_kgeoms[model_id].geom[vec[j]]->geom;
dGeomID g2 = m_kgeoms[model_id].geom[vec[k]]->geom;
+
dReal aabb1[6], aabb2[6];
dGeomGetAABB(g1, aabb1);
dGeomGetAABB(g2, aabb2);
@@ -321,13 +322,13 @@
aabb1[4] > aabb2[5] ||
aabb1[5] < aabb2[4]))
dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+
if (cdata.collides)
goto OUT1;
}
}
}
-
/* check collision with standalone ode bodies */
OUT1:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-01-17 08:10:22
|
Revision: 9626
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9626&view=rev
Author: jfaustwg
Date: 2009-01-17 08:10:01 +0000 (Sat, 17 Jan 2009)
Log Message:
-----------
roscpp API changes
* ros::node -> ros::Node
* ros::msg -> ros::Message
* deprecated methods removed
* rosconsole/rosconsole.h -> ros/console.h
* goodbye rosthread
Modified Paths:
--------------
pkg/trunk/audio/audio_capture/audio_capture.cpp
pkg/trunk/audio/audio_capture/audio_clip_writer.cpp
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/control_toolbox/src/serialchain_model.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_velocity_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/grasp_point_node.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
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/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_velocity_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/backlash_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/sine_sweep_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_effort_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_smoothing_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_orientation_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_torque_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_wrench_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_blind_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/lqr_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/ros_serialchain_model.cpp
pkg/trunk/deprecated/dc1394_cam/manifest.xml
pkg/trunk/deprecated/dc1394_cam_server/src/check_params/check_params.cpp
pkg/trunk/deprecated/dc1394_cam_server/src/dc1394_cam_server/dc1394_cam_server.cpp
pkg/trunk/deprecated/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/deprecated/ransac_ground_plane_extraction/include/ransac_ground_plane_extraction/ransac_ground_plane_extraction_node.h
pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_node.cpp
pkg/trunk/deprecated/resource_locator/src/reslocator.cpp
pkg/trunk/deprecated/scan_utils/include/listen_node/scanListenNode.h
pkg/trunk/deprecated/scan_utils/src/cloudToOctree/cloudToOctree.cpp
pkg/trunk/deprecated/scan_utils/src/listen_node/scanListenNode.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_demo/axis_demo.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_ptz/axis_ptz.cpp
pkg/trunk/drivers/cam/bumblebee_bridge/bumblebee_bridge.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/check_params.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp
pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp
pkg/trunk/drivers/imu/imu_node/imu_node.cc
pkg/trunk/drivers/input/joy/joy.cpp
pkg/trunk/drivers/input/joy_node/joy_node.cpp
pkg/trunk/drivers/input/joy_view/joy_view.cpp
pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp
pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp
pkg/trunk/drivers/laser/laser_view/laser_view.cpp
pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/ek1122.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg014.cpp
pkg/trunk/drivers/phase_space/phase_space_node.cpp
pkg/trunk/drivers/phase_space/phase_space_node.h
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/irobot_create/include/irobot_create/irobot_create.h
pkg/trunk/drivers/robot/irobot_create/irobot_create.cpp
pkg/trunk/drivers/robot/katana/libkatana/katana.cpp
pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp
pkg/trunk/drivers/robot/katana/nodes/katana_server/katana_server.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/include/point_cloud_assembler/base_assembler_srv.h
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/test/test_assembler.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_odometry.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp
pkg/trunk/drivers/robot/pr2/pr2_power_board/include/power_node.h
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node.cpp
pkg/trunk/drivers/robot/segway_apox/manifest.xml
pkg/trunk/drivers/robot/segway_apox/segway.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_bumper.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_f3d.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_laser.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_ptz.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h
pkg/trunk/estimation/robot_pose_ekf/test/test_ekf.cpp
pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.h
pkg/trunk/hardware_test/self_test/include/self_test/self_test.h
pkg/trunk/hardware_test/self_test/manifest.xml
pkg/trunk/hardware_test/self_test/src/run_selftest.cpp
pkg/trunk/hardware_test/self_test/src/selftest_example.cpp
pkg/trunk/highlevel/executive_python/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/Executive.hh
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/EndEffectorStateAdapter.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/Executive.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/WatchDog.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/trex_watchdog.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp
pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
pkg/trunk/highlevel/highlevel_controllers/include/IndefiniteNav.hh
pkg/trunk/highlevel/highlevel_controllers/src/ControlCli.cpp
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/src/indefinite_nav.cpp
pkg/trunk/highlevel/highlevel_controllers/src/joy_batt_sender.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/manip/arm_trajectory/arm_trajectory.cc
pkg/trunk/manip/spacenav_node/src/spacenav_node.cpp
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
pkg/trunk/mapping/cloud_io/src/tools/bag_pcd.cpp
pkg/trunk/mapping/cloud_io/src/tools/pcd_generator.cpp
pkg/trunk/mapping/collision_map/src/collision_map.cpp
pkg/trunk/mapping/normal_estimation/src/normal_estimation.cpp
pkg/trunk/mapping/normal_estimation/src/normal_estimation_omp.cpp
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/point_cloud_downsampler/src/cloud_downsampler.cpp
pkg/trunk/mapping/sample_consensus/src/tools/sac_ground_removal.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp
pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_control/test/ms_publisher_test.cpp
pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp
pkg/trunk/motion_planning/navfn/src/navfn.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/include/robarm3d/plan_path_node.h
pkg/trunk/motion_planning/planning_research/robarm3d/src/discrete_space_information/robarm/environment_robarm3d.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/plan_path_node.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/sim_grasp_path.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/sim_path_srv.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/simulate_arm_path.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/test_forward_kinematics.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/test_plan_path.cpp
pkg/trunk/motion_planning/test_collision_space/src/test_cs.cpp
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/cli.cpp
pkg/trunk/nav/deadreckon/deadreckon.cpp
pkg/trunk/nav/deadreckon/test_deadreckon_service.cpp
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/fake_localization/ground_truth_controller.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/nav_view/src/nav_view/tools.h
pkg/trunk/nav/nav_view/src/test/nav_view_test.cpp
pkg/trunk/nav/nav_view_sdl/nav_view.cpp
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.h
pkg/trunk/nav/slam_gmapping/src/tftest.cpp
pkg/trunk/nav/teleop_base/teleop_base.cpp
pkg/trunk/nav/teleop_base/teleop_head.cpp
pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc
pkg/trunk/nav/wavefront_player/cli.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h
pkg/trunk/trajectory_rollout/manifest.xml
pkg/trunk/trajectory_rollout/src/governor_node.cpp
pkg/trunk/unported/laser_viewer/src/laser_viewer/laser_viewer.cpp
pkg/trunk/unported/sharks/src/joyshark/joyshark.cpp
pkg/trunk/unported/simple_sdl_gui/src/key_monitor/key_monitor.cpp
pkg/trunk/unported/simple_sdl_gui/src/simple_sdl_gui_node/simple_sdl_gui.cpp
pkg/trunk/util/filter_coefficient_server/src/filter_coeff_client.cpp
pkg/trunk/util/filter_coefficient_server/test/check_function_calls.cpp
pkg/trunk/util/filter_demo/src/filtering_example.cpp
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/pr2_ik_node.h
pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp
pkg/trunk/util/kinematics/robot_kinematics/test/robot_kinematics_test.cpp
pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
pkg/trunk/util/laser_scan/src/median_filter_node.cpp
pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp
pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp
pkg/trunk/util/logsetta/carmen/carmen_logger/carmen_logger.cpp
pkg/trunk/util/message_sequencing/example/testdelay.cpp
pkg/trunk/util/message_sequencing/example/testgen.cpp
pkg/trunk/util/message_sequencing/example/testrecv.cpp
pkg/trunk/util/message_sequencing/include/message_sequencing/time_sequencer.h
pkg/trunk/util/misc_utils/include/misc_utils/advertised_service_guard.h
pkg/trunk/util/misc_utils/include/misc_utils/job_queue.h
pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
pkg/trunk/util/misc_utils/include/misc_utils/subscription_guard.h
pkg/trunk/util/misc_utils/manifest.xml
pkg/trunk/util/mux/mux.cpp
pkg/trunk/util/mux/switch.cpp
pkg/trunk/util/point_cloud_utils/include/point_cloud_utils/scan_assembler.h
pkg/trunk/util/point_cloud_utils/include/point_cloud_utils/timed_scan_assembler.h
pkg/trunk/util/point_cloud_utils/manifest.xml
pkg/trunk/util/point_cloud_utils/src/scan_assembler.cpp
pkg/trunk/util/point_cloud_utils/src/timed_scan_assembler.cpp
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
pkg/trunk/util/tf/include/tf/message_notifier.h
pkg/trunk/util/tf/include/tf/transform_broadcaster.h
pkg/trunk/util/tf/include/tf/transform_datatypes.h
pkg/trunk/util/tf/include/tf/transform_listener.h
pkg/trunk/util/tf/src/transform_sender.cpp
pkg/trunk/util/tf/test/testBroadcaster.cpp
pkg/trunk/util/tf/test/testListener.cpp
pkg/trunk/util/tf/test/test_notifier.cpp
pkg/trunk/util/topic_synchronizer/manifest.xml
pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
pkg/trunk/util/trajectory/include/trajectory/trajectory.h
pkg/trunk/vision/borg/borg.cpp
pkg/trunk/vision/calib_converter/src/calib_converter.cpp
pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/bumblebee_grab_sample.cpp
pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
pkg/trunk/vision/color_calib/calib_node.cpp
pkg/trunk/vision/color_calib/calib_node_dcam.cpp
pkg/trunk/vision/color_calib/calibration.cpp
pkg/trunk/vision/color_calib/color_calib.h
pkg/trunk/vision/cv_view/dcam/cv_view.cpp
pkg/trunk/vision/cv_view/minimal/cv_view.cpp
pkg/trunk/vision/cv_view/src/cv_view.cpp
pkg/trunk/vision/cv_view/src/cv_view_array.cpp
pkg/trunk/vision/cv_view/src/cv_view_array_overlay.cpp
pkg/trunk/vision/cv_wrappers/src/cv_movie_streamer.cpp
pkg/trunk/vision/cv_wrappers/src/cv_view.cpp
pkg/trunk/vision/cv_wrappers/src/cv_view_array.cpp
pkg/trunk/vision/cv_wrappers/src/cv_view_array_overlay.cpp
pkg/trunk/vision/mech_turk/src/cv_mturk.cpp
pkg/trunk/vision/people/manifest.xml
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/filter/people_tracking_node.cpp
pkg/trunk/vision/people/src/filter/people_tracking_node.h
pkg/trunk/vision/people/src/leg_detector.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/people/src/timing_diagnostics_node.cpp
pkg/trunk/vision/people/src/timing_diagnostics_node.h
pkg/trunk/vision/people/src/track_starter_gui.cpp
pkg/trunk/vision/spacetime_stereo/sts_dynamic.cpp
pkg/trunk/vision/spacetime_stereo/sts_node.cpp
pkg/trunk/vision/spectacles/specview/specview.cpp
pkg/trunk/vision/stereo_blob_tracker/src/axis_blob_tracker.cpp
pkg/trunk/vision/stereo_blob_tracker/src/blob_tracker_gui.cpp
pkg/trunk/vision/stereo_blob_tracker/src/listener.cpp
pkg/trunk/vision/stereo_blob_tracker/src/stereo_blob_tracker.cpp
pkg/trunk/vision/stereo_capture/src/project_light.cpp
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
pkg/trunk/vision/stereo_view/stereo_view.cpp
pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
pkg/trunk/visualization/cloud_viewer/src/cloud_node/cloud_node.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/shape.cpp
pkg/trunk/visualization/ogre_tools/test/utest.cpp
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/collision_map_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/collision_map_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/octree_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/octree_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/particle_cloud_2d_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_base.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/polygonal_map_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/polygonal_map_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_base2d_pose_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_model_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/factory.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/ros_topic_property.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/ros_topic_property.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/tool.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.h
pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp
pkg/trunk/visualization/ogre_visualizer/src/test/rgb_cloud_test.cpp
pkg/trunk/visualization/ogre_visualizer/src/test/visualizer_test.cpp
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.h
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraSetupDialog.cpp
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraSetupDialog.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/costmap_2d/src/test/costmap2d_pqueue_benchmark.cpp
pkg/trunk/world_models/map_saver/src/map_saver.cpp
pkg/trunk/world_models/map_server/src/main.cpp
pkg/trunk/world_models/map_server/test/rtest.cpp
pkg/trunk/world_models/robot_models/robot_filter/include/robot_filter/RobotFilter.h
pkg/trunk/world_models/robot_models/robot_filter/src/robot_filter.cpp
pkg/trunk/world_models/robot_models/robot_model/include/robot_model/cnode.h
pkg/trunk/world_models/robot_models/robot_model/include/robot_model/knode.h
pkg/trunk/world_models/topological_map/include/topological_map/ros_bottleneck_graph.h
pkg/trunk/world_models/topological_map/src/bottleneck_graph.cpp
pkg/trunk/world_models/topological_map/src/roadmap.cpp
pkg/trunk/world_models/topological_map/src/ros_bottleneck_graph.cpp
pkg/trunk/world_models/world_3d_map/manifest.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/vision/cv_wrappers/.build_version
pkg/trunk/vision/cv_wrappers/lib/
pkg/trunk/vision/image_synthesizer/bin/
pkg/trunk/vision/image_synthesizer/lib/
Removed Paths:
-------------
pkg/trunk/august-cleanup.py
pkg/trunk/util/misc_utils/include/misc_utils/mutex_guard.h
Property Changed:
----------------
pkg/trunk/
pkg/trunk/3rdparty/kdl/kdl-willow/debian/rules
pkg/trunk/controllers/robot_mechanism_controllers/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:9316
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:9624
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Property changes on: pkg/trunk/3rdparty/kdl/kdl-willow/debian/rules
___________________________________________________________________
Deleted: svn:executable
- *
Modified: pkg/trunk/audio/audio_capture/audio_capture.cpp
===================================================================
--- pkg/trunk/audio/audio_capture/audio_capture.cpp 2009-01-17 07:55:56 UTC (rev 9625)
+++ pkg/trunk/audio/audio_capture/audio_capture.cpp 2009-01-17 08:10:01 UTC (rev 9626)
@@ -33,7 +33,7 @@
#include "robot_msgs/AudioRawStream.h"
const static int SAMPLE_RATE = 44100; // todo: make this a parameter.
-ros::node *g_audio_node = NULL;
+ros::Node *g_audio_node = NULL;
bool g_caught_sigint = false;
#define SHOW_MAX_SAMPLE
@@ -92,7 +92,7 @@
}
ROS_DEBUG("opening default audio stream");
ros::init(argc, argv);
- ros:...
[truncated message content] |
|
From: <ge...@us...> - 2009-01-17 18:58:31
|
Revision: 9636
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9636&view=rev
Author: gerkey
Date: 2009-01-17 18:58:25 +0000 (Sat, 17 Jan 2009)
Log Message:
-----------
converted to cmake
Modified Paths:
--------------
pkg/trunk/drivers/input/joy_view/Makefile
pkg/trunk/util/sdlgl/Makefile
Added Paths:
-----------
pkg/trunk/drivers/input/joy_view/CMakeLists.txt
pkg/trunk/util/sdlgl/CMakeLists.txt
Removed Paths:
-------------
pkg/trunk/drivers/input/joy_view/ROS_BUILD_BLACKLIST
pkg/trunk/util/sdlgl/src/Makefile
Added: pkg/trunk/drivers/input/joy_view/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/input/joy_view/CMakeLists.txt (rev 0)
+++ pkg/trunk/drivers/input/joy_view/CMakeLists.txt 2009-01-17 18:58:25 UTC (rev 9636)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.4.6)
+
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(joy_view)
+rospack_add_executable(joy_view joy_view.cpp)
Modified: pkg/trunk/drivers/input/joy_view/Makefile
===================================================================
--- pkg/trunk/drivers/input/joy_view/Makefile 2009-01-17 18:57:47 UTC (rev 9635)
+++ pkg/trunk/drivers/input/joy_view/Makefile 2009-01-17 18:58:25 UTC (rev 9636)
@@ -1,4 +1,5 @@
-SRC = joy_view.cpp
-OUT = joy_view
-PKG = joy_view
-include $(shell rospack find mk)/node.mk
+include $(shell rospack find mk)/cmake.mk
+#SRC = joy_view.cpp
+#OUT = joy_view
+#PKG = joy_view
+#include $(shell rospack find mk)/node.mk
Deleted: pkg/trunk/drivers/input/joy_view/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/input/joy_view/ROS_BUILD_BLACKLIST 2009-01-17 18:57:47 UTC (rev 9635)
+++ pkg/trunk/drivers/input/joy_view/ROS_BUILD_BLACKLIST 2009-01-17 18:58:25 UTC (rev 9636)
@@ -1 +0,0 @@
-Blacklisting until we move it to cmake.
Added: pkg/trunk/util/sdlgl/CMakeLists.txt
===================================================================
--- pkg/trunk/util/sdlgl/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/sdlgl/CMakeLists.txt 2009-01-17 18:58:25 UTC (rev 9636)
@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+find_package(SDL REQUIRED)
+rospack(sdlgl)
+include_directories(${SDL_INCLUDE_DIR})
+rospack_add_library(sdlgl src/sdlgl.cpp)
+target_link_libraries(sdlgl ${SDL_LIBRARY})
Modified: pkg/trunk/util/sdlgl/Makefile
===================================================================
--- pkg/trunk/util/sdlgl/Makefile 2009-01-17 18:57:47 UTC (rev 9635)
+++ pkg/trunk/util/sdlgl/Makefile 2009-01-17 18:58:25 UTC (rev 9636)
@@ -1,2 +1,3 @@
-SUBDIRS = src
-include $(shell rospack find mk)/recurse_subdirs.mk
+include $(shell rospack find mk)/cmake.mk
+#SUBDIRS = src
+#include $(shell rospack find mk)/recurse_subdirs.mk
Deleted: pkg/trunk/util/sdlgl/src/Makefile
===================================================================
--- pkg/trunk/util/sdlgl/src/Makefile 2009-01-17 18:57:47 UTC (rev 9635)
+++ pkg/trunk/util/sdlgl/src/Makefile 2009-01-17 18:58:25 UTC (rev 9636)
@@ -1,7 +0,0 @@
-SRC = sdlgl.cpp
-OUT = ../lib/libsdlgl.a
-PKG = sdlgl
-CFLAGS = `sdl-config --cflags`
-LFLAGS = `sdl-config --libs`
-include $(shell rospack find mk)/lib.mk
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-01-17 19:02:04
|
Revision: 9638
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9638&view=rev
Author: gerkey
Date: 2009-01-17 19:01:54 +0000 (Sat, 17 Jan 2009)
Log Message:
-----------
moved nepumuk to 3rdparty
Added Paths:
-----------
pkg/trunk/3rdparty/nepumuk/
pkg/trunk/3rdparty/nepumuk/Makefile
Removed Paths:
-------------
pkg/trunk/3rdparty/nepumuk/Makefile
pkg/trunk/simulators/nepumuk/
Deleted: pkg/trunk/3rdparty/nepumuk/Makefile
===================================================================
--- pkg/trunk/simulators/nepumuk/Makefile 2009-01-17 17:43:37 UTC (rev 9631)
+++ pkg/trunk/3rdparty/nepumuk/Makefile 2009-01-17 19:01:54 UTC (rev 9638)
@@ -1,101 +0,0 @@
-# Note that the version of 3rdparty/libsunflower needs to closely
-# match the nepumuk build.
-#
-# If all goes according to plan, all you have to touch for a
-# nepumuk update is the VERSION. This also allows you to build a
-# specific version without touching the Makefile, for example by
-# typing:
-#
-# $ make VERSION=r900
-#
-# Or, to replace an existing build, do e.g.:
-#
-# $ make VERSION=r900 rebuild
-#
-# It is also possible to use svn checkout to get the sources, in case
-# you need the bleeding edge. This is done using:
-#
-# $ make svn-all
-
-##all: tarball-all
-#all: svn-all
-all: installed
-
-TARBALL_VERSION= r912
-SVN_REVISION= -r 964
-
-TARBALL= build/nepumuk-$(TARBALL_VERSION).tar.gz
-TARBALL_URL= http://downloads.sourceforge.net/libsunflower/nepumuk-$(TARBALL_VERSION).tar.gz
-SOURCE_DIR= $(PWD)/build/nepumuk-$(TARBALL_VERSION)
-UNPACK_CMD= tar xfz
-BUILD_DIR= $(PWD)/build
-INST_DIR= $(PWD)/local
-ESTAR_DIR= `rospack find estar`/local
-SFL_DIR= `rospack find libsunflower`/local
-BOOST_DIR= `rospack cflags-only-I boost`
-
-SVN_DIR= $(PWD)/build/nepumuk-svn
-SVN_URL= https://libsunflower.svn.sourceforge.net/svnroot/libsunflower/trunk/nepumuk
-
-SIMULINKS= nepumuk rostest ros0.sh robots.ros0 layout.ros0 expodemo.sh robots.expo layout.expo
-
-
-#include $(shell rospack find mk)/download_unpack_build.mk
-include $(shell rospack find mk)/svn_checkout.mk
-
-installed: $(SVN_DIR) patched
- cd $(SVN_DIR) && ./bootstrap-buildsystem.sh
- test -d $(BUILD_DIR) || mkdir -p $(BUILD_DIR)
- cd $(BUILD_DIR) && $(SVN_DIR)/configure --prefix=$(INST_DIR) \
- --with-sfl=$(SFL_DIR) \
- --with-boost=$(BOOST_DIR)
- $(MAKE) -C $(BUILD_DIR) install
- for foo in $(SIMULINKS); do \
- test -L $$foo || ln -s $(INST_DIR)/bin/$$foo || exit 42; done
- touch installed
-
-.PHONY: svn-all
-svn-all: checkout_or_update
- $(MAKE) SOURCE_DIR=$(SVN_DIR) really-all
-
-.PHONY: tarball-all
-tarball-all: $(SOURCE_DIR)
- $(MAKE) really-all
-
-rebuild: wipe really-all
-
-.PHONY: really-all
-really-all: $(SOURCE_DIR)/configure $(BUILD_DIR)/config.h
- $(MAKE) -C $(BUILD_DIR) install
- $(MAKE) symlinks
-
-checkout_or_update:
- - test -d $(SVN_DIR) && svn up $(SVN_DIR)
- test -d $(SVN_DIR) || svn co -r$(SVN_REV) $(SVN_URL) $(SVN_DIR)
-
-symlinks:
- for foo in $(SIMULINKS); do \
- test -L $$foo || ln -s $(INST_DIR)/bin/$$foo || exit 42; done
-
-$(SOURCE_DIR)/configure $(SOURCE_DIR)/config.h.in: $(SOURCE_DIR)/Makefile.am $(SOURCE_DIR)/configure.ac $(SOURCE_DIR)/bootstrap-buildsystem.sh
- cd $(SOURCE_DIR) && ./bootstrap-buildsystem.sh
-
-## --with-estar=$(ESTAR_DIR)
-## --enable-ros
-$(BUILD_DIR)/config.h: $(SOURCE_DIR)/configure $(SOURCE_DIR)/config.h.in
- test -d $(BUILD_DIR) || mkdir -p $(BUILD_DIR)
- cd $(BUILD_DIR) && $(SOURCE_DIR)/configure --prefix=$(INST_DIR) \
- --with-sfl=$(SFL_DIR) \
- --with-boost=$(BOOST_DIR)
-
-clean:
- - rm $(SIMULINKS)
- $(MAKE) -C $(BUILD_DIR) clean
- rm -f installed
-
-distclean:
- - rm $(SIMULINKS)
- $(MAKE) -C $(BUILD_DIR) distclean
-
-wipe: clean
- rm -rf build $(INST_DIR)
Copied: pkg/trunk/3rdparty/nepumuk/Makefile (from rev 9637, pkg/trunk/simulators/nepumuk/Makefile)
===================================================================
--- pkg/trunk/3rdparty/nepumuk/Makefile (rev 0)
+++ pkg/trunk/3rdparty/nepumuk/Makefile 2009-01-17 19:01:54 UTC (rev 9638)
@@ -0,0 +1,101 @@
+# Note that the version of 3rdparty/libsunflower needs to closely
+# match the nepumuk build.
+#
+# If all goes according to plan, all you have to touch for a
+# nepumuk update is the VERSION. This also allows you to build a
+# specific version without touching the Makefile, for example by
+# typing:
+#
+# $ make VERSION=r900
+#
+# Or, to replace an existing build, do e.g.:
+#
+# $ make VERSION=r900 rebuild
+#
+# It is also possible to use svn checkout to get the sources, in case
+# you need the bleeding edge. This is done using:
+#
+# $ make svn-all
+
+##all: tarball-all
+#all: svn-all
+all: installed
+
+TARBALL_VERSION= r912
+SVN_REVISION= -r 964
+
+TARBALL= build/nepumuk-$(TARBALL_VERSION).tar.gz
+TARBALL_URL= http://downloads.sourceforge.net/libsunflower/nepumuk-$(TARBALL_VERSION).tar.gz
+SOURCE_DIR= $(PWD)/build/nepumuk-$(TARBALL_VERSION)
+UNPACK_CMD= tar xfz
+BUILD_DIR= $(PWD)/build
+INST_DIR= $(PWD)/local
+ESTAR_DIR= `rospack find estar`/local
+SFL_DIR= `rospack find libsunflower`/local
+BOOST_DIR= `rospack cflags-only-I boost`
+
+SVN_DIR= $(PWD)/build/nepumuk-svn
+SVN_URL= https://libsunflower.svn.sourceforge.net/svnroot/libsunflower/trunk/nepumuk
+
+SIMULINKS= nepumuk rostest ros0.sh robots.ros0 layout.ros0 expodemo.sh robots.expo layout.expo
+
+
+#include $(shell rospack find mk)/download_unpack_build.mk
+include $(shell rospack find mk)/svn_checkout.mk
+
+installed: $(SVN_DIR) patched
+ cd $(SVN_DIR) && ./bootstrap-buildsystem.sh
+ test -d $(BUILD_DIR) || mkdir -p $(BUILD_DIR)
+ cd $(BUILD_DIR) && $(SVN_DIR)/configure --prefix=$(INST_DIR) \
+ --with-sfl=$(SFL_DIR) \
+ --with-boost=$(BOOST_DIR)
+ $(MAKE) -C $(BUILD_DIR) install
+ for foo in $(SIMULINKS); do \
+ test -L $$foo || ln -s $(INST_DIR)/bin/$$foo || exit 42; done
+ touch installed
+
+.PHONY: svn-all
+svn-all: checkout_or_update
+ $(MAKE) SOURCE_DIR=$(SVN_DIR) really-all
+
+.PHONY: tarball-all
+tarball-all: $(SOURCE_DIR)
+ $(MAKE) really-all
+
+rebuild: wipe really-all
+
+.PHONY: really-all
+really-all: $(SOURCE_DIR)/configure $(BUILD_DIR)/config.h
+ $(MAKE) -C $(BUILD_DIR) install
+ $(MAKE) symlinks
+
+checkout_or_update:
+ - test -d $(SVN_DIR) && svn up $(SVN_DIR)
+ test -d $(SVN_DIR) || svn co -r$(SVN_REV) $(SVN_URL) $(SVN_DIR)
+
+symlinks:
+ for foo in $(SIMULINKS); do \
+ test -L $$foo || ln -s $(INST_DIR)/bin/$$foo || exit 42; done
+
+$(SOURCE_DIR)/configure $(SOURCE_DIR)/config.h.in: $(SOURCE_DIR)/Makefile.am $(SOURCE_DIR)/configure.ac $(SOURCE_DIR)/bootstrap-buildsystem.sh
+ cd $(SOURCE_DIR) && ./bootstrap-buildsystem.sh
+
+## --with-estar=$(ESTAR_DIR)
+## --enable-ros
+$(BUILD_DIR)/config.h: $(SOURCE_DIR)/configure $(SOURCE_DIR)/config.h.in
+ test -d $(BUILD_DIR) || mkdir -p $(BUILD_DIR)
+ cd $(BUILD_DIR) && $(SOURCE_DIR)/configure --prefix=$(INST_DIR) \
+ --with-sfl=$(SFL_DIR) \
+ --with-boost=$(BOOST_DIR)
+
+clean:
+ - rm $(SIMULINKS)
+ -$(MAKE) -C $(BUILD_DIR) clean
+ rm -f installed patched
+
+distclean:
+ - rm $(SIMULINKS)
+ $(MAKE) -C $(BUILD_DIR) distclean
+
+wipe: clean
+ rm -rf build $(INST_DIR)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-17 22:44:02
|
Revision: 9650
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9650&view=rev
Author: isucan
Date: 2009-01-17 21:48:27 +0000 (Sat, 17 Jan 2009)
Log Message:
-----------
deprecating
Modified Paths:
--------------
pkg/trunk/deprecated/plan_kinematic_path/src/plan_kinematic_path.cpp
Added Paths:
-----------
pkg/trunk/deprecated/robot_model/
Removed Paths:
-------------
pkg/trunk/world_models/robot_models/robot_model/
Modified: pkg/trunk/deprecated/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/deprecated/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-17 21:44:26 UTC (rev 9649)
+++ pkg/trunk/deprecated/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-17 21:48:27 UTC (rev 9650)
@@ -363,6 +363,7 @@
{
ROS_INFO("Done");
}
+ /*
send_traj_query_req.trajectoryid = send_traj_start_res.trajectoryid;
while(!(traj_done == send_traj_query_res.State_Done || traj_done == send_traj_query_res.State_Failed))
{
@@ -375,8 +376,8 @@
ROS_ERROR("Trajectory query failed");
}
}
-
-
+
+ */
// create the service request
// return;
@@ -488,11 +489,14 @@
sleep(1);
}
- plan->shutdown();
- delete plan;
+ // plan->shutdown();
+ // delete plan;
}
else
usage(argv[0]);
+ sleep(100);
+ ros::fini();
+
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-17 22:44:07
|
Revision: 9651
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9651&view=rev
Author: isucan
Date: 2009-01-17 22:09:28 +0000 (Sat, 17 Jan 2009)
Log Message:
-----------
deprecating
Added Paths:
-----------
pkg/trunk/deprecated/robot_filter/
Removed Paths:
-------------
pkg/trunk/world_models/robot_models/robot_filter/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-18 03:18:04
|
Revision: 9659
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9659&view=rev
Author: isucan
Date: 2009-01-18 03:17:55 +0000 (Sun, 18 Jan 2009)
Log Message:
-----------
small updates for comments
Modified Paths:
--------------
pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg
pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
Modified: pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg 2009-01-18 03:17:23 UTC (rev 9658)
+++ pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg 2009-01-18 03:17:55 UTC (rev 9659)
@@ -1,8 +1,8 @@
# This message contains a set of parameters useful in setting up a
# kinematic space for planning
-# The model (defined in pr2.xml as a group) for which the motion
-# planner should plan for
+# The model (defined in the robot description as a group of links)
+# for which the motion planner should plan for
string model_id
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2009-01-18 03:17:23 UTC (rev 9658)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2009-01-18 03:17:55 UTC (rev 9659)
@@ -4,7 +4,7 @@
# Parameters for the state space
robot_msgs/KinematicSpaceParameters params
-# The starting state for the robot. This is eth complete state of the
+# The starting state for the robot. This is the complete state of the
# robot, all joint values, everything that needs to be specified to
# completely define where each part of the robot is in space. The
# meaning for each element in the state vector can be extracted from
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-01-18 18:49:57
|
Revision: 9673
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9673&view=rev
Author: rob_wheeler
Date: 2009-01-18 18:49:35 +0000 (Sun, 18 Jan 2009)
Log Message:
-----------
Move realtime publisher and realtime infuser into realtime tools.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h
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/include/pr2_mechanism_controllers/wrist_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
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/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_orientation_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_orientation_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
pkg/trunk/util/misc_utils/manifest.xml
Added Paths:
-----------
pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_infuser.h
pkg/trunk/util/realtime_tools/include/realtime_tools/realtime_publisher.h
Removed Paths:
-------------
pkg/trunk/util/misc_utils/include/misc_utils/realtime_infuser.h
pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -58,7 +58,7 @@
#include <pr2_msgs/Odometer.h>
#include <pr2_msgs/Covariance2D.h>
-#include <misc_utils/realtime_publisher.h>
+#include <realtime_tools/realtime_publisher.h>
#include <tf/tfMessage.h>
#include <tf/tf.h>
@@ -481,13 +481,13 @@
double odom_publish_rate_; /** rate at which odometry message will be published ( = 1/odom_publish_delta_t_)*/
- misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ; //!< Publishes the odometry msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ; //!< Publishes the odometry msg from the update() realtime loop
- misc_utils::RealtimePublisher <tf::tfMessage>* transform_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <tf::tfMessage>* transform_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
- misc_utils::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
- misc_utils::RealtimePublisher <pr2_msgs::Covariance2D>* covariance_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <pr2_msgs::Covariance2D>* covariance_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
/*
* \brief pointer to ros node
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -60,7 +60,7 @@
#include <pr2_msgs/Odometer.h>
#include <pr2_msgs/Covariance2D.h>
-#include <misc_utils/realtime_publisher.h>
+#include <realtime_tools/realtime_publisher.h>
#include <tf/tfMessage.h>
@@ -499,15 +499,15 @@
double odom_publish_rate_; /** rate at which odometry message will be published ( = 1/odom_publish_delta_t_)*/
- misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ; //!< Publishes the odometry msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ; //!< Publishes the odometry msg from the update() realtime loop
- misc_utils::RealtimePublisher <tf::tfMessage>* transform_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <tf::tfMessage>* transform_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
- misc_utils::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
- misc_utils::RealtimePublisher <pr2_msgs::Covariance2D>* covariance_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <pr2_msgs::Covariance2D>* covariance_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
- misc_utils::RealtimePublisher <pr2_mechanism_controllers::OdometryResiduals>* residuals_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <pr2_mechanism_controllers::OdometryResiduals>* residuals_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
/*
* \brief pointer to ros node
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -35,7 +35,7 @@
#define CASTER_CALIBRATION_CONTROLLER_H
#include "pr2_mechanism_controllers/caster_controller.h"
-#include "misc_utils/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.h"
#include "std_msgs/Empty.h"
#include <robot_mechanism_controllers/CalibrateJoint.h>
@@ -97,7 +97,7 @@
AdvertisedServiceGuard guard_calibrate_;
double last_publish_time_;
- misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
+ realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
};
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -49,7 +49,7 @@
#include "mechanism_model/robot.h"
#include "robot_mechanism_controllers/joint_velocity_controller.h"
-#include "misc_utils/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.h"
#include "std_msgs/Empty.h"
namespace controller
@@ -120,7 +120,7 @@
GripperCalibrationController c_;
double last_publish_time_;
- misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
+ realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
};
}
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-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -39,7 +39,7 @@
#include <mechanism_model/controller.h>
#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
-#include <misc_utils/realtime_publisher.h>
+#include <realtime_tools/realtime_publisher.h>
// Messages
#include <pr2_mechanism_controllers/LaserScannerSignal.h>
@@ -257,7 +257,7 @@
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
bool need_to_send_msg_ ; //!< Tracks whether we still need to send out the m_scanner_signal_ message.
- misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal>* publisher_ ; //!< Publishes the m_scanner_signal msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <pr2_mechanism_controllers::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-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -39,7 +39,7 @@
#include <mechanism_model/controller.h>
#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
-#include <misc_utils/realtime_publisher.h>
+#include <realtime_tools/realtime_publisher.h>
#include "filters/transfer_function.h"
@@ -145,7 +145,7 @@
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
bool need_to_send_msg_ ; //!< Tracks whether we still need to send out the m_scanner_signal_ message.
- misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal>* publisher_ ; //!< Publishes the m_scanner_signal msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal>* publisher_ ; //!< Publishes the m_scanner_signal msg from the update() realtime loop
};
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -44,7 +44,7 @@
#define WRIST_CALIBRATION_CONTROLLER_H
#include "robot_mechanism_controllers/joint_velocity_controller.h"
-#include "misc_utils/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.h"
#include "mechanism_model/wrist_transmission.h"
#include "std_msgs/Empty.h"
@@ -108,7 +108,7 @@
WristCalibrationController c_;
double last_publish_time_;
- misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
+ realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
};
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-01-18 18:49:35 UTC (rev 9673)
@@ -10,6 +10,7 @@
<depend package="wg_robot_description_parser" />
<depend package="stl_utils" />
<depend package="misc_utils" />
+ <depend package="realtime_tools" />
<depend package="robot_mechanism_controllers" />
<depend package="rospy" />
<depend package="libTF" />
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-18 18:49:35 UTC (rev 9673)
@@ -35,7 +35,7 @@
// Original version: Sachin Chitta <sa...@wi...>
#include "pr2_mechanism_controllers/arm_trajectory_controller.h"
-#include "misc_utils/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.h"
#include "std_msgs/String.h"
using namespace controller;
@@ -228,7 +228,7 @@
double end_time = now();
- static misc_utils::RealtimePublisher<std_msgs::String> p("/s", 1);
+ static realtime_tools::RealtimePublisher<std_msgs::String> p("/s", 1);
if (p.trylock()) {
char buf[1000];
sprintf(buf, "Time = %15.6lf\n", end_time - start_time);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-01-18 18:49:35 UTC (rev 9673)
@@ -1002,19 +1002,19 @@
if (publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete publisher_ ;
- publisher_ = new misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
+ publisher_ = new realtime_tools::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
if (odometer_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete odometer_publisher_ ;
- odometer_publisher_ = new misc_utils::RealtimePublisher <pr2_msgs::Odometer> (service_prefix + "/odometer", 1) ;
+ odometer_publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::Odometer> (service_prefix + "/odometer", 1) ;
if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete transform_publisher_ ;
- transform_publisher_ = new misc_utils::RealtimePublisher <tf::tfMessage> ("tf_message", 5) ;
+ transform_publisher_ = new realtime_tools::RealtimePublisher <tf::tfMessage> ("tf_message", 5) ;
if (covariance_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete covariance_publisher_ ;
- covariance_publisher_ = new misc_utils::RealtimePublisher <pr2_msgs::Covariance2D> (service_prefix + "/odometry_covariance", 1) ;
+ covariance_publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::Covariance2D> (service_prefix + "/odometry_covariance", 1) ;
node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-01-18 18:49:35 UTC (rev 9673)
@@ -33,7 +33,7 @@
*********************************************************************/
#include <pr2_mechanism_controllers/base_controller_pos.h>
-#include <misc_utils/realtime_publisher.h>
+#include <realtime_tools/realtime_publisher.h>
#include <control_toolbox/filters.h>
#include <angles/angles.h>
#include <std_msgs/String.h>
@@ -532,7 +532,7 @@
cmd_vel_trajectory_->setTrajectory(cmd_vel_points_);
-/* static misc_utils::RealtimePublisher<std_msgs::String> p("/s", 1);
+/* static realtime_tools::RealtimePublisher<std_msgs::String> p("/s", 1);
if (p.trylock()) {
char buf[1000];
sprintf(buf, "Time = %15.6lf dirn: %1.6f %1.6f %1.6f mag: %1.6f\n Angles: %f %f \n Current dirn: %1.6f %1.6f %1.6f mag: %1.6f\n New cmd: %1.6f %1.6f %1.6f\n Old cmd: %1.6f %1.6f %1.6f\n ",dt,new_vel_direction.x,new_vel_direction.y,new_vel_direction.z,new_cmd_mag,delta_direction,delta_direction_m_pi,cmd_vel_direction_.x,cmd_vel_direction_.y,cmd_vel_direction_.z,cmd_vel_magnitude_,new_cmd.x, new_cmd.y,new_cmd.z,cmd_vel_.x,cmd_vel_.y,cmd_vel_.z);
@@ -1138,23 +1138,23 @@
if (publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete publisher_ ;
- publisher_ = new misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
+ publisher_ = new realtime_tools::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
if (odometer_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete odometer_publisher_ ;
- odometer_publisher_ = new misc_utils::RealtimePublisher <pr2_msgs::Odometer> (service_prefix + "/odometer", 1) ;
+ odometer_publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::Odometer> (service_prefix + "/odometer", 1) ;
if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete transform_publisher_ ;
- transform_publisher_ = new misc_utils::RealtimePublisher <tf::tfMessage> ("tf_message", 5) ;
+ transform_publisher_ = new realtime_tools::RealtimePublisher <tf::tfMessage> ("tf_message", 5) ;
if (covariance_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete covariance_publisher_ ;
- covariance_publisher_ = new misc_utils::RealtimePublisher <pr2_msgs::Covariance2D> (service_prefix + "/odometry_covariance", 1) ;
+ covariance_publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::Covariance2D> (service_prefix + "/odometry_covariance", 1) ;
if (residuals_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete residuals_publisher_ ;
- residuals_publisher_ = new misc_utils::RealtimePublisher <pr2_mechanism_controllers::OdometryResiduals> (service_prefix + "/odometry_residuals", 1) ;
+ residuals_publisher_ = new realtime_tools::RealtimePublisher <pr2_mechanism_controllers::OdometryResiduals> (service_prefix + "/odometry_residuals", 1) ;
node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-01-18 18:49:35 UTC (rev 9673)
@@ -213,7 +213,7 @@
node->advertiseService(topic + "/calibrate", &CasterCalibrationControllerNode::calibrateCommand, this);
guard_calibrate_.set(topic + "/calibrate");
- pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
+ pub_calibrated_ = new realtime_tools::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
return true;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-01-18 18:49:35 UTC (rev 9673)
@@ -185,7 +185,7 @@
if (!c_.initXml(robot, config))
return false;
- pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
+ pub_calibrated_ = new realtime_tools::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
return true;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-01-18 18:49:35 UTC (rev 9673)
@@ -559,7 +559,7 @@
if (publisher_ != NULL) // Make sure that we don't memory leak if initXml gets called twice
delete publisher_ ;
- publisher_ = new misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
+ publisher_ = new realtime_tools::RealtimePublisher <pr2_mechanism_controllers::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-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-01-18 18:49:35 UTC (rev 9673)
@@ -413,7 +413,7 @@
if (publisher_ != NULL) // Make sure that we don't memory leak if initXml gets called twice
delete publisher_ ;
- publisher_ = new misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
+ publisher_ = new realtime_tools::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
prev_profile_time_ = 0.0 ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp 2009-01-18 18:49:35 UTC (rev 9673)
@@ -35,7 +35,7 @@
// Original version: Sachin Chitta <sa...@wi...>
#include "pr2_mechanism_controllers/pr2_arm_dynamics_controller.h"
-#include "misc_utils/realtime_publisher.h"
+#include "realtime_tools/realtime_publisher.h"
#include "std_msgs/String.h"
// Math utils
@@ -255,7 +255,7 @@
control_torque_[i] = gravity_torque_[i][2] + pid_torque;
// fprintf(stderr,"%d:: %f, %f, %f, %f, %f, %f, %f\n",i,actual,command,error,pid_torque,gravity_torque_[i][2],control_torque_[i],time-last_time_);
- static misc_utils::RealtimePublisher<std_msgs::String> p("/s", 1);
+ static realtime_tools::RealtimePublisher<std_msgs::String> p("/s", 1);
if (p.trylock()) {
char buf[1000];
sprintf(buf, "Joint torques %d:: %s, %15.6lf %15.61f %15.61f %15.61f\n", i, joint_effort_controllers_[i]->joint_state_->joint_->name_.c_str(),control_torque_[i],gravity_torque_[i][2],gravity_torque_uncompensated_[i][2],pid_torque);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-01-18 18:49:35 UTC (rev 9673)
@@ -307,7 +307,7 @@
if (!c_.initXml(robot, config))
return false;
- pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(name + "/calibrated", 1);
+ pub_calibrated_ = new realtime_tools::RealtimePublisher<std_msgs::Empty>(name + "/calibrated", 1);
return true;
}
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -46,7 +46,7 @@
#include <ros/node.h>
#include <math.h>
#include <robot_msgs/DiagnosticMessage.h>
-#include <misc_utils/realtime_publisher.h>
+#include <realtime_tools/realtime_publisher.h>
#include <mechanism_model/controller.h>
#include <control_toolbox/sine_sweep.h>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -48,7 +48,7 @@
#include <ros/node.h>
#include <math.h>
#include <robot_msgs/DiagnosticMessage.h>
-#include <misc_utils/realtime_publisher.h>
+#include <realtime_tools/realtime_publisher.h>
#include <mechanism_model/controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h 2009-01-18 18:26:39 UTC (rev 9672)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h 2009-01-18 18:49:35 UTC (rev 9673)
@@ -46,7 +46,7 @@
#include <ros/node.h>
#include <math.h>
#include <robot_msgs/DiagnosticMessage.h>
-#include <misc_utils/realtime_publisher.h>
+#include <realtime_tools/realtime_publisher.h>
#include <mechanism_model/controller.h>
#include <control_toolbox/sine_sweep.h>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
============================...
[truncated message content] |
|
From: <ei...@us...> - 2009-01-19 18:12:57
|
Revision: 9693
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9693&view=rev
Author: eitanme
Date: 2009-01-19 18:12:52 +0000 (Mon, 19 Jan 2009)
Log Message:
-----------
Moving trajectory_rollout under nav where it belongs.
Added Paths:
-----------
pkg/trunk/nav/trajectory_rollout/
Removed Paths:
-------------
pkg/trunk/trajectory_rollout/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-01-19 20:06:54
|
Revision: 9710
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9710&view=rev
Author: hsujohnhsu
Date: 2009-01-19 20:06:48 +0000 (Mon, 19 Jan 2009)
Log Message:
-----------
* rename gazebo controller to start with gazebo_
* reorganize launch scripts for pr2_prototype1_gazebo and pr2_gazebo.
* turn test_2dnav_wg headless, though this test is still empty.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_gazebo/test/test_2dnav_wg.xml
pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_empty.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_empty_no_x.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_obs.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_simple.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg_no_x.launch
Added Paths:
-----------
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_default_controllers.launch
pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gazebo_head_torso_lift_controller.xml
Removed Paths:
-------------
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/controllers_head_torso_gazebo.xml
Modified: pkg/trunk/demos/2dnav_gazebo/test/test_2dnav_wg.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/test/test_2dnav_wg.xml 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/2dnav_gazebo/test/test_2dnav_wg.xml 2009-01-19 20:06:48 UTC (rev 9710)
@@ -2,7 +2,7 @@
<master auto="start" />
<!-- load robot -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_wg.launch"/>
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_wg_no_x.launch"/>
<!-- load map -->
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap25mm.png 0.025" respawn="false" output="screen" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -3,7 +3,7 @@
<!--node pkg="mechanism_control" type="mech.py" args="sp $(find pr2_gazebo)/controllers_2dnav_test.xml" respawn="false" output="screen" /-->
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="mech.py" args="sp $(find pr2_default_controllers)/controllers_head_torso_gazebo.xml" output="screen"/>
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
<!-- Tug Arms For Navigation -->
<!--node pkg="pr2_gazebo" type="tuck_arms.py" output="screen"/-->
Modified: pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -15,7 +15,6 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- load controllers -->
- <!--include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" /-->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -15,7 +15,6 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- load controllers -->
- <!--include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" /-->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Deleted: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -1,13 +0,0 @@
-<launch>
- <!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <param name="base_controller/odom_publish_rate" value="10" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="mech.py" args="sp $(find pr2_default_controllers)/controllers_head_torso_gazebo.xml" output="screen"/>
-
- <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
- for details of the profile, rates, see controller::LaserScannerControllerNode. -->
- <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
- <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 2 .45 .40" />
-</launch>
-
Copied: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_default_controllers.launch (from rev 9699, pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch)
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_default_controllers.launch (rev 0)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_default_controllers.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -0,0 +1,13 @@
+<launch>
+ <!-- use mech.py to spawn all controllers listed in controllers.xml -->
+ <param name="base_controller/odom_publish_rate" value="10" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
+
+ <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
+ for details of the profile, rates, see controller::LaserScannerControllerNode. -->
+ <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
+ <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 2 .45 .40" />
+</launch>
+
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_empty.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_empty.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_empty.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -19,7 +19,7 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0 0 0 0 0 0" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_empty_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_empty_no_x.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_empty_no_x.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -19,7 +19,7 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0 0 0 0 0 0" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_obs.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_obs.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_obs.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -15,7 +15,7 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_simple.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_simple.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_simple.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -19,7 +19,7 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -19,7 +19,7 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 0 0" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -19,7 +19,7 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 0 0" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -19,7 +19,7 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg_no_x.launch 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg_no_x.launch 2009-01-19 20:06:48 UTC (rev 9710)
@@ -19,7 +19,7 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_default_controllers.launch" />
</group>
</launch>
Deleted: pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/controllers_head_torso_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/controllers_head_torso_gazebo.xml 2009-01-19 19:58:22 UTC (rev 9709)
+++ pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/controllers_head_torso_gazebo.xml 2009-01-19 20:06:48 UTC (rev 9710)
@@ -1,28 +0,0 @@
-<?xml version="1.0"?>
-
-<controllers>
-
-
- <!-- ========================================= -->
- <!-- torso_lift array -->
- <controller name="torso_lift_controller" topic="torso_lift_controller" type="JointPositionControllerNode">
- <joint name="torso_lift_joint">
- <pid p="1000" d="0" i="1" iClamp="100" />
- </joint>
- </controller>
- <!-- ========================================= -->
- <!-- head and above array -->
- <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
- <listen_topic name="head_pan_commands" />
- <joint name="head_pan_joint" >
- <pid p="100" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
- <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
- <listen_topic name="head_tilt_commands" />
- <joint name="head_tilt_joint" >
- <pid p="100" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
-
-</controllers>
Copied: pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gazebo_head_torso_lift_controller.xml (from rev 9698, pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/controllers_head_torso_gazebo.xml)
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gazebo_head_torso_lift_controller.xml (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gazebo_head_torso_lift_controller.xml 2009-01-19 20:06:48 UTC (rev 9710)
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<controllers>
+
+
+ <!-- ========================================= -->
+ <!-- torso_lift array -->
+ <controller name="torso_lift_controller" topic="torso_lift_controller" type="JointPositionControllerNode">
+ <joint name="torso_lift_joint">
+ <pid p="1000" d="0" i="1" iClamp="100" />
+ </joint>
+ </controller>
+ <!-- ========================================= -->
+ <!-- head and above array -->
+ <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
+ <listen_topic name="head_pan_commands" />
+ <joint name="head_pan_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
+ <listen_topic name="head_tilt_commands" />
+ <joint name="head_tilt_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+
+</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-19 23:46:56
|
Revision: 9752
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9752&view=rev
Author: isucan
Date: 2009-01-19 23:46:52 +0000 (Mon, 19 Jan 2009)
Log Message:
-----------
bringing back to life...
Added Paths:
-----------
pkg/trunk/motion_planning/plan_kinematic_path/
Removed Paths:
-------------
pkg/trunk/deprecated/plan_kinematic_path/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-20 22:26:49
|
Revision: 9801
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9801&view=rev
Author: isucan
Date: 2009-01-20 21:40:36 +0000 (Tue, 20 Jan 2009)
Log Message:
-----------
check if the starting state is already a solution in a motion plan
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/CMakeLists.txt
pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h
pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-01-20 21:40:36 UTC (rev 9801)
@@ -168,56 +168,81 @@
static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->setPoseConstraints(cstrs);
}
- /** Compute the actual motion plan */
- void computePlan(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
+ /** Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
+ bool computePlan(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
ompl::SpaceInformationKinematic::PathKinematic_t &bestPath, double &bestDifference)
{
if (times <= 0)
{
ROS_ERROR("Request specifies motion plan cannot be computed %d times", times);
- return;
+ return false;
}
- /* do the planning */
- bestPath = NULL;
- bestDifference = 0.0;
- double totalTime = 0.0;
- ompl::SpaceInformation::Goal_t goal = psetup->si->getGoal();
+ unsigned int t_index = 0;
+ double t_distance = 0.0;
+ bool result = psetup->mp->isTrivial(&t_index, &t_distance);
- for (int i = 0 ; i < times ; ++i)
+ if (result)
{
- ros::Time startTime = ros::Time::now();
- bool ok = psetup->mp->solve(allowed_time);
- double tsolve = (ros::Time::now() - startTime).to_double();
- ROS_INFO("%s Motion planner spend %g seconds", (ok ? "[Success]" : "[Failure]"), tsolve);
- totalTime += tsolve;
+ ROS_INFO("Solution already achieved");
+ bestDifference = t_distance;
+
+ /* we want to maintain the invariant that a path will
+ at least consist of start & goal states, so we copy
+ the start state twice */
+ bestPath = new ompl::SpaceInformationKinematic::PathKinematic(psetup->si);
- /* do path smoothing */
- if (ok)
+ ompl::SpaceInformationKinematic::StateKinematic_t s0 = new ompl::SpaceInformationKinematic::StateKinematic(psetup->si->getStateDimension());
+ ompl::SpaceInformationKinematic::StateKinematic_t s1 = new ompl::SpaceInformationKinematic::StateKinematic(psetup->si->getStateDimension());
+ psetup->si->copyState(s0, static_cast<ompl::SpaceInformationKinematic::StateKinematic_t>(psetup->si->getStartState(t_index)));
+ psetup->si->copyState(s1, static_cast<ompl::SpaceInformationKinematic::StateKinematic_t>(psetup->si->getStartState(t_index)));
+ bestPath->states.push_back(s0);
+ bestPath->states.push_back(s1);
+ }
+ else
+ {
+ /* do the planning */
+ bestPath = NULL;
+ bestDifference = 0.0;
+ double totalTime = 0.0;
+ ompl::SpaceInformation::Goal_t goal = psetup->si->getGoal();
+
+ for (int i = 0 ; i < times ; ++i)
{
ros::Time startTime = ros::Time::now();
- ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
- psetup->smoother->smoothMax(path);
- double tsmooth = (ros::Time::now() - startTime).to_double();
- ROS_INFO(" Smoother spent %g seconds (%g seconds in total)", tsmooth, tsmooth + tsolve);
- if (interpolate)
- psetup->si->interpolatePath(path);
- if (bestPath == NULL || bestDifference > goal->getDifference() ||
- (bestPath && bestDifference == goal->getDifference() && bestPath->states.size() > path->states.size()))
+ bool ok = psetup->mp->solve(allowed_time);
+ double tsolve = (ros::Time::now() - startTime).to_double();
+ ROS_INFO("%s Motion planner spend %g seconds", (ok ? "[Success]" : "[Failure]"), tsolve);
+ totalTime += tsolve;
+
+ /* do path smoothing */
+ if (ok)
{
- if (bestPath)
- delete bestPath;
- bestPath = path;
- bestDifference = goal->getDifference();
- goal->forgetSolutionPath();
- ROS_INFO(" Obtained better solution");
+ ros::Time startTime = ros::Time::now();
+ ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
+ psetup->smoother->smoothMax(path);
+ double tsmooth = (ros::Time::now() - startTime).to_double();
+ ROS_INFO(" Smoother spent %g seconds (%g seconds in total)", tsmooth, tsmooth + tsolve);
+ if (interpolate)
+ psetup->si->interpolatePath(path);
+ if (bestPath == NULL || bestDifference > goal->getDifference() ||
+ (bestPath && bestDifference == goal->getDifference() && bestPath->states.size() > path->states.size()))
+ {
+ if (bestPath)
+ delete bestPath;
+ bestPath = path;
+ bestDifference = goal->getDifference();
+ goal->forgetSolutionPath();
+ ROS_INFO(" Obtained better solution");
+ }
}
+ psetup->mp->clear();
}
- psetup->mp->clear();
+
+ ROS_INFO("Total planning time: %g; Average planning time: %g", totalTime, (totalTime / (double)times));
}
-
- ROS_INFO("\nTotal planning time: %g; Average planning time: %g", totalTime, (totalTime / (double)times));
+ return result;
}
void fillSolution(RKPPlannerSetup *psetup, ompl::SpaceInformationKinematic::PathKinematic_t bestPath, double bestDifference,
@@ -252,7 +277,7 @@
psetup->si->clearStartStates();
}
- bool execute(ModelMap &models, _R &req, robot_msgs::KinematicPath &path, double &distance)
+ bool execute(ModelMap &models, _R &req, robot_msgs::KinematicPath &path, double &distance, bool &trivial)
{
// make sure the same motion planner instance is not used by other calls
boost::mutex::scoped_lock(m_lock);
@@ -286,7 +311,7 @@
double bestDifference = 0.0;
m->collisionSpace->lock();
- computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
+ trivial = computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
m->collisionSpace->unlock();
/* fill in the results */
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-20 21:40:36 UTC (rev 9801)
@@ -256,21 +256,24 @@
{
robot_msgs::KinematicPath solution;
unsigned int step = 0;
- while (m_replanning)
+ bool trivial = false;
+
+ while (m_replanning && !trivial)
{
step++;
ROS_INFO("Replanning step %d", step);
- m_continueReplanningLock.lock();
+ boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
const double *start_state = m_robotState->getParams();
for (unsigned int i = 0 ; i < m_kmodel->stateDimension ; ++i)
m_currentPlanToStateRequest.start_state.vals[i] = start_state[i];
- m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance);
+ m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance, trivial);
publish("path_to_goal", solution);
+ if (trivial)
+ break;
while (!m_collisionMonitorChange)
m_collisionMonitorCondition.wait(m_continueReplanningLock);
- m_continueReplanningLock.unlock();
}
}
@@ -279,22 +282,24 @@
{
robot_msgs::KinematicPath solution;
unsigned int step = 0;
-
+ bool trivial = false;
+
while (m_replanning)
{
step++;
ROS_INFO("Replanning step %d", step);
- m_continueReplanningLock.lock();
+ boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
const double *start_state = m_robotState->getParams();
for (unsigned int i = 0 ; i < m_kmodel->stateDimension ; ++i)
m_currentPlanToPositionRequest.start_state.vals[i] = start_state[i];
- m_requestLinkPosition.execute(m_models, m_currentPlanToPositionRequest, solution, distance);
+ m_requestLinkPosition.execute(m_models, m_currentPlanToPositionRequest, solution, distance, trivial);
publish("path_to_goal", solution);
+ if (trivial)
+ break;
while (!m_collisionMonitorChange)
m_collisionMonitorCondition.wait(m_continueReplanningLock);
- m_continueReplanningLock.unlock();
}
}
@@ -441,7 +446,8 @@
//Acutally run the service.
- bool r = m_requestState.execute(m_models, req, res.path, res.distance);
+ bool trivial = false;
+ bool r = m_requestState.execute(m_models, req, res.path, res.distance, trivial);
//Copy the path.
@@ -483,14 +489,20 @@
bool planToState(robot_srvs::KinematicPlanState::request &req, robot_srvs::KinematicPlanState::response &res)
{
- ROS_INFO("\nRequest for planning to a state");
- return m_requestState.execute(m_models, req, res.path, res.distance);
+ ROS_INFO("Request for planning to a state");
+ bool trivial = false;
+ bool result = m_requestState.execute(m_models, req, res.path, res.distance, trivial);
+ res.trivial = trivial ? 1 : 0;
+ return result;
}
bool planToPosition(robot_srvs::KinematicPlanLinkPosition::request &req, robot_srvs::KinematicPlanLinkPosition::response &res)
{
- ROS_INFO("\nRequest for planning to a position");
- return m_requestLinkPosition.execute(m_models, req, res.path, res.distance);
+ ROS_INFO("Request for planning to a position");
+ bool trivial = false;
+ bool result = m_requestLinkPosition.execute(m_models, req, res.path, res.distance, trivial);
+ res.trivial = trivial ? 1 : 0;
+ return result;
}
virtual void setRobotDescription(robot_desc::URDF *file)
Modified: pkg/trunk/motion_planning/ompl/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-01-20 21:40:36 UTC (rev 9801)
@@ -4,6 +4,7 @@
include_directories(${PROJECT_SOURCE_DIR}/code)
rospack_add_library(ompl code/ompl/base/util/src/time.cpp
code/ompl/base/util/src/output.cpp
+ code/ompl/base/src/Planner.cpp
code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
code/ompl/extension/samplingbased/kinematic/src/PathSmootherKinematic.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/Planner.h 2009-01-20 21:40:36 UTC (rev 9801)
@@ -86,6 +86,13 @@
return m_type;
}
+ /** A problem is trivial if the given starting state already
+ in the goal region, so we need no motion planning. startID
+ will be set to the index of the starting state that
+ satisfies the goal. The distance to the goal can
+ optionally be returned as well. */
+ virtual bool isTrivial(unsigned int *startID = NULL, double *distance = NULL);
+
protected:
SpaceInformation_t m_si;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2009-01-20 21:40:36 UTC (rev 9801)
@@ -39,6 +39,7 @@
#include "ompl/base/General.h"
#include <vector>
+#include <cassert>
/** Main namespace */
namespace ompl
@@ -357,9 +358,16 @@
/** Perform additional setup tasks (run once, before use) */
virtual void setup(void)
{
+ assert(m_stateValidityChecker);
m_setup = true;
}
+ /** Check if a given state is valid or not */
+ bool isValid(const State_t state)
+ {
+ return (*m_stateValidityChecker)(state);
+ }
+
protected:
bool m_setup;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2009-01-20 21:40:36 UTC (rev 9801)
@@ -249,9 +249,6 @@
/** Insert states in a path, at the collision checking resolution */
virtual void interpolatePath(PathKinematic_t path, double factor = 1.0);
-
- /** Check if a given state is valid or not */
- bool isValid(const StateKinematic_t state);
/** Print information about the current instance of the state space */
virtual void printSettings(std::ostream &out = std::cout) const;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2009-01-20 21:40:36 UTC (rev 9801)
@@ -107,7 +107,6 @@
{
assert(m_stateDimension > 0);
assert(m_stateComponent.size() == m_stateDimension);
- assert(m_stateValidityChecker);
assert(m_stateDistanceEvaluator);
SpaceInformation::setup();
}
@@ -298,11 +297,6 @@
{
return (*m_stateDistanceEvaluator)(static_cast<const State_t>(s1), static_cast<const State_t>(s2));
}
-
-bool ompl::SpaceInformationKinematic::isValid(const StateKinematic_t state)
-{
- return (*m_stateValidityChecker)(static_cast<const State_t>(state));
-}
void ompl::SpaceInformationKinematic::printSettings(std::ostream &out) const
{
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2009-01-20 21:40:36 UTC (rev 9801)
@@ -50,3 +50,7 @@
# it may return a distance larger than the desired threshold. The user may choose to
# discard the path
float64 distance
+
+# If the starting state was already in the goal region,
+# this is set to 1. Otherwise, it will be set to 0
+byte trivial
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2009-01-20 20:47:30 UTC (rev 9800)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2009-01-20 21:40:36 UTC (rev 9801)
@@ -52,3 +52,8 @@
# it may return a distance larger than the desired threshold. The user may choose to
# discard the path
float64 distance
+
+
+# If the starting state was already in the goal region,
+# this is set to 1. Otherwise, it will be set to 0
+byte trivial
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|