|
From: <is...@us...> - 2009-02-19 23:27:18
|
Revision: 11420
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11420&view=rev
Author: isucan
Date: 2009-02-19 22:36:52 +0000 (Thu, 19 Feb 2009)
Log Message:
-----------
adding IKKPIECE
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h 2009-02-19 22:31:23 UTC (rev 11419)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h 2009-02-19 22:36:52 UTC (rev 11420)
@@ -45,6 +45,7 @@
#include "kinematic_planning/RKPESTSetup.h"
#include "kinematic_planning/RKPIKSBLSetup.h"
#include "kinematic_planning/RKPKPIECESetup.h"
+#include "kinematic_planning/RKPIKKPIECESetup.h"
#include <string>
#include <map>
@@ -119,6 +120,17 @@
else
delete kpiece;
}
+
+
+ void addIKKPIECE(std::map<std::string, std::string> &options)
+ {
+ RKPPlannerSetup *kpiece = new RKPIKKPIECESetup();
+ if (kpiece->setup(dynamic_cast<RKPModelBase*>(this), options))
+ planners[kpiece->name] = kpiece;
+ else
+ delete kpiece;
+ }
+
std::map<std::string, RKPPlannerSetup*> planners;
};
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-19 22:31:23 UTC (rev 11419)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-19 22:36:52 UTC (rev 11420)
@@ -766,6 +766,14 @@
options = data.getMapTagValues("planning", "KPIECE");
}
model->addKPIECE(options);
+
+ options.clear();
+ if (group)
+ {
+ const robot_desc::URDF::Map &data = group->data;
+ options = data.getMapTagValues("planning", "IKKPIECE");
+ }
+ model->addIKKPIECE(options);
}
ModelMap m_models;
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-19 22:31:23 UTC (rev 11419)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-19 22:36:52 UTC (rev 11420)
@@ -125,7 +125,7 @@
req.params.model_id = GROUPNAME;
req.params.distance_metric = "L2Square";
- req.params.planner_id = "IKSBL";
+ req.params.planner_id = "IKKPIECE";
req.interpolate = 1;
req.times = 1;
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml 2009-02-19 22:31:23 UTC (rev 11419)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml 2009-02-19 22:36:52 UTC (rev 11420)
@@ -92,6 +92,11 @@
<elem key="celldim" value="0.1 0.1 0.1"/>
</map>
+ <map name="IKKPIECE" flag="planning">
+ <elem key="projection" value="link r_wrist_roll_link" />
+ <elem key="celldim" value="0.1 0.1 0.1"/>
+ </map>
+
<map name="IKSBL" flag="planning">
<elem key="projection" value="0 1"/>
<elem key="celldim" value="1 1"/>
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-19 22:31:23 UTC (rev 11419)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-19 22:36:52 UTC (rev 11420)
@@ -659,8 +659,13 @@
/** Returns the dimension of the group (as a state, not number of joints) */
unsigned int getGroupDimension(const std::string &name) const;
+ /** Bring the robot to a default state */
void defaultState(void);
+
+ /** Apply the transforms to a group, based on the params */
void computeTransformsGroup(const double *params, int groupID);
+
+ /** Apply the transforms to the entire robot, based on the params */
void computeTransforms(const double *params);
/** Add thansforms to the rootTransform such that the robot is in its planar/floating link frame */
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-02-20 20:58:39
|
Revision: 11459
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11459&view=rev
Author: gerkey
Date: 2009-02-20 20:58:25 +0000 (Fri, 20 Feb 2009)
Log Message:
-----------
Got nav_view building (but not running) on OSX.
Modified Paths:
--------------
pkg/trunk/3rdparty/Cg/manifest.xml
pkg/trunk/3rdparty/ogre/Makefile
pkg/trunk/3rdparty/ogre/manifest.xml
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.cpp
pkg/trunk/nav/nav_view/src/nav_view/tools.h
pkg/trunk/nav/nav_view/src/test/nav_view_test.cpp
pkg/trunk/visualization/ogre_tools/CMakeLists.txt
pkg/trunk/visualization/ogre_tools/src/ogre_tools/arrow.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/axes.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/initialization.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/movable_text.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/movable_text.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/orthographic.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/shape.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/shape.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/wx_ogre_render_window.cpp
pkg/trunk/visualization/ogre_tools/src/stl_to_mesh/stl_to_mesh.cpp
pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp
Modified: pkg/trunk/3rdparty/Cg/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/Cg/manifest.xml 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/3rdparty/Cg/manifest.xml 2009-02-20 20:58:25 UTC (rev 11459)
@@ -11,6 +11,7 @@
<url>http://developer.nvidia.com/page/cg_main.html</url>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/Cg/lib -L${prefix}/Cg/lib -lCg" cflags="-I${prefix}/Cg/include"/>
+ <cpp os="osx" lflags="" cflags=""/>
<doxymaker external="http://developer.nvidia.com/page/documentation.html"/>
</export>
<sysdepend os="ubuntu" version="7.04-feisty" package="wget"/>
Modified: pkg/trunk/3rdparty/ogre/Makefile
===================================================================
--- pkg/trunk/3rdparty/ogre/Makefile 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/3rdparty/ogre/Makefile 2009-02-20 20:58:25 UTC (rev 11459)
@@ -51,7 +51,9 @@
#
#ogre: ogre/lib/pkgconfig/OGRE.pc
ogre:
- if ! pkg-config --exact-version 1.4.9 OGRE; then \
+ @if test `uname` = Darwin ; then \
+ echo "OS X detected; assuming that you have installed the OGRE SDK"; \
+ elif ! pkg-config --exact-version 1.4.9 OGRE; then \
echo "You don't have OGRE. I'm going to build it."; \
if [ -h ogre/lib/OGRE ]; then \
rm ogre/lib/OGRE; \
Modified: pkg/trunk/3rdparty/ogre/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/ogre/manifest.xml 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/3rdparty/ogre/manifest.xml 2009-02-20 20:58:25 UTC (rev 11459)
@@ -12,6 +12,11 @@
<depend package="Cg"/>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/ogre/lib -L${prefix}/ogre/lib/OGRE `PKG_CONFIG_PATH=${prefix}/ogre/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --libs OGRE`" cflags="`PKG_CONFIG_PATH=${prefix}/ogre/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --cflags OGRE`"/>
+ <!-- The usage of -dylib_file is a workaround for a bug in Apple's
+ installation of libGL.dylib, as explained here:
+ http://developer.apple.com/qa/qa2007/qa1567.html
+ -->
+ <cpp os="osx" lflags="-framework Ogre -framework Carbon -dylib_file /System/Library/Frameworks/OpenGL.framework/Versions/A/Libraries/libGL.dylib:/System/Library/Frameworks/OpenGL.framework/Versions/A/Libraries/libGL.dylib " cflags=""/>
<doxymaker external="http://ogre3d.org"/>
</export>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libzzip-0-13"/>
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -35,7 +35,7 @@
#include "ros/node.h"
#include <tf/transform_listener.h>
-#include <Ogre.h>
+#include <OGRE/Ogre.h>
#include "ogre_tools/axes.h"
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-02-20 20:58:25 UTC (rev 11459)
@@ -38,9 +38,9 @@
#include "deprecated_msgs/Pose2DFloat32.h"
#include "robot_srvs/StaticMap.h"
-#include <OgreTexture.h>
-#include <OgreMaterial.h>
-#include <OgreRenderOperation.h>
+#include <OGRE/OgreTexture.h>
+#include <OGRE/OgreMaterial.h>
+#include <OGRE/OgreRenderOperation.h>
#include <list>
Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -32,7 +32,7 @@
#include <ogre_tools/wx_ogre_render_window.h>
#include <ros/node.h>
-#include <Ogre.h>
+#include <OGRE/Ogre.h>
#include <wx/wx.h>
#include "nav_view_panel.h"
Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.h 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.h 2009-02-20 20:58:25 UTC (rev 11459)
@@ -30,7 +30,7 @@
#ifndef NAV_VIEW_TOOLS_H
#define NAV_VIEW_TOOLS_H
-#include <OgreVector3.h>
+#include <OGRE/OgreVector3.h>
namespace Ogre
{
Modified: pkg/trunk/nav/nav_view/src/test/nav_view_test.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/test/nav_view_test.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/nav/nav_view/src/test/nav_view_test.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -40,7 +40,7 @@
#include "nav_view_panel.h"
-#include "Ogre.h"
+#include "OGRE/Ogre.h"
using namespace nav_view;
Modified: pkg/trunk/visualization/ogre_tools/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/ogre_tools/CMakeLists.txt 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/CMakeLists.txt 2009-02-20 20:58:25 UTC (rev 11459)
@@ -103,7 +103,7 @@
# ...and we make another library with just the SWIG wrappers, linked
# against the C++ lib.
rospack_add_library(python_${PROJECT_NAME} ${SWIG_OUTPUT_CPP_FILE})
-target_link_libraries(python_${PROJECT_NAME} ${PROJECT_NAME})
+target_link_libraries(python_${PROJECT_NAME} ${PROJECT_NAME} ${PYTHON_LIBRARIES})
# swig python needs a shared library named _<modulename>.[so|dll|...]
# this renames the output file to conform to that by prepending an underscore and removing the "lib" prefix
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/arrow.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/arrow.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/arrow.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -30,10 +30,10 @@
#include "arrow.h"
#include "shape.h"
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
#include <sstream>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/axes.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/axes.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/axes.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -30,10 +30,10 @@
#include "axes.h"
#include "shape.h"
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
#include <sstream>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -29,12 +29,12 @@
#include "billboard_line.h"
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
-#include <OgreBillboardChain.h>
-#include <OgreMaterialManager.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
+#include <OGRE/OgreBillboardChain.h>
+#include <OGRE/OgreMaterialManager.h>
#include <sstream>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.h 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/billboard_line.h 2009-02-20 20:58:25 UTC (rev 11459)
@@ -35,9 +35,9 @@
#include <stdint.h>
#include <vector>
-#include <OgreVector3.h>
-#include <OgreColourValue.h>
-#include <OgreMaterial.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreColourValue.h>
+#include <OGRE/OgreMaterial.h>
namespace Ogre
{
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -29,11 +29,11 @@
#include "camera_base.h"
-#include <OgreCamera.h>
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
+#include <OGRE/OgreCamera.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
#include <stdint.h>
#include <sstream>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h 2009-02-20 20:58:25 UTC (rev 11459)
@@ -30,8 +30,8 @@
#ifndef OGRE_TOOLS_CAMERA_BASE_H_
#define OGRE_TOOLS_CAMERA_BASE_H_
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
namespace Ogre
{
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -29,11 +29,11 @@
#include "fps_camera.h"
-#include <OgreCamera.h>
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
+#include <OGRE/OgreCamera.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
#include <stdint.h>
#include <sstream>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -29,12 +29,12 @@
#include "grid.h"
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
-#include <OgreManualObject.h>
-#include <OgreMaterialManager.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
+#include <OGRE/OgreManualObject.h>
+#include <OGRE/OgreMaterialManager.h>
#include <sstream>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/initialization.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/initialization.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/initialization.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -1,8 +1,8 @@
#include "initialization.h"
-#include <OgreRoot.h>
-#include <OgreRenderSystem.h>
-#include <OgreLogManager.h>
+#include <OGRE/OgreRoot.h>
+#include <OGRE/OgreRenderSystem.h>
+#include <OGRE/OgreLogManager.h>
#include <exception>
#include <stdexcept>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/movable_text.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/movable_text.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/movable_text.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -40,8 +40,8 @@
#include "movable_text.h"
-#include "Ogre.h"
-#include "OgreFontManager.h"
+#include "OGRE/Ogre.h"
+#include "OGRE/OgreFontManager.h"
#include <sstream>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/movable_text.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/movable_text.h 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/movable_text.h 2009-02-20 20:58:25 UTC (rev 11459)
@@ -41,10 +41,10 @@
#ifndef OGRE_TOOLS_MOVABLE_TEXT_H
#define OGRE_TOOLS_MOVABLE_TEXT_H
-#include <OgreMovableObject.h>
-#include <OgreRenderable.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
+#include <OGRE/OgreMovableObject.h>
+#include <OGRE/OgreRenderable.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
namespace Ogre
{
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -30,12 +30,12 @@
#include "orbit_camera.h"
#include "shape.h"
-#include <OgreCamera.h>
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
-#include <OgreViewport.h>
+#include <OGRE/OgreCamera.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
+#include <OGRE/OgreViewport.h>
#include <stdint.h>
#include <sstream>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h 2009-02-20 20:58:25 UTC (rev 11459)
@@ -31,7 +31,7 @@
#define OGRE_TOOLS_ORBIT_CAMERA_H_
#include "camera_base.h"
-#include <OgreVector3.h>
+#include <OGRE/OgreVector3.h>
namespace Ogre
{
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -33,11 +33,11 @@
#include "ros/assert.h"
-#include <OgreCamera.h>
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
+#include <OGRE/OgreCamera.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
namespace ogre_tools
{
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h 2009-02-20 20:58:25 UTC (rev 11459)
@@ -32,7 +32,7 @@
#define OGRE_TOOLS_ORTHO_CAMERA_H_
#include "camera_base.h"
-#include <OgreVector3.h>
+#include <OGRE/OgreVector3.h>
namespace ogre_tools
{
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/orthographic.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/orthographic.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/orthographic.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -1,6 +1,6 @@
#include "orthographic.h"
-#include <OgreMatrix4.h>
+#include <OGRE/OgreMatrix4.h>
namespace ogre_tools
{
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -30,14 +30,14 @@
#include "point_cloud.h"
#include <ros/assert.h>
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
-#include <OgreManualObject.h>
-#include <OgreMaterialManager.h>
-#include <OgreBillboardSet.h>
-#include <OgreBillboard.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
+#include <OGRE/OgreManualObject.h>
+#include <OGRE/OgreMaterialManager.h>
+#include <OGRE/OgreBillboardSet.h>
+#include <OGRE/OgreBillboard.h>
#include <sstream>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.h 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.h 2009-02-20 20:58:25 UTC (rev 11459)
@@ -30,10 +30,10 @@
#ifndef OGRE_TOOLS_OGRE_POINT_CLOUD_H
#define OGRE_TOOLS_OGRE_POINT_CLOUD_H
-#include <OgreMovableObject.h>
-#include <OgreString.h>
-#include <OgreAxisAlignedBox.h>
-#include <OgreVector3.h>
+#include <OGRE/OgreMovableObject.h>
+#include <OGRE/OgreString.h>
+#include <OGRE/OgreAxisAlignedBox.h>
+#include <OGRE/OgreVector3.h>
#include <stdint.h>
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/shape.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/shape.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/shape.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -30,12 +30,12 @@
#include "shape.h"
#include <ros/assert.h>
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <OgreVector3.h>
-#include <OgreQuaternion.h>
-#include <OgreEntity.h>
-#include <OgreMaterialManager.h>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreVector3.h>
+#include <OGRE/OgreQuaternion.h>
+#include <OGRE/OgreEntity.h>
+#include <OGRE/OgreMaterialManager.h>
namespace ogre_tools
{
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/shape.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/shape.h 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/shape.h 2009-02-20 20:58:25 UTC (rev 11459)
@@ -32,8 +32,8 @@
#include "object.h"
-#include <OgreMaterial.h>
-#include <OgreVector3.h>
+#include <OGRE/OgreMaterial.h>
+#include <OGRE/OgreVector3.h>
namespace Ogre
{
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/wx_ogre_render_window.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/wx_ogre_render_window.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/wx_ogre_render_window.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -1,11 +1,11 @@
#include "wx_ogre_render_window.h"
#include "orthographic.h"
-#include <OgreRoot.h>
-#include <OgreViewport.h>
-#include <OgreCamera.h>
-#include <OgreRenderWindow.h>
-#include <OgreStringConverter.h>
+#include <OGRE/OgreRoot.h>
+#include <OGRE/OgreViewport.h>
+#include <OGRE/OgreCamera.h>
+#include <OGRE/OgreRenderWindow.h>
+#include <OGRE/OgreStringConverter.h>
#ifdef __WXGTK__
#include <gdk/gdk.h>
@@ -232,7 +232,8 @@
handle = str.str();
#else
// Any other unsupported system
- #error Not supported on this platform.
+ // #error Not supported on this platform.
+ handle = std::string("none");
#endif
return handle;
Modified: pkg/trunk/visualization/ogre_tools/src/stl_to_mesh/stl_to_mesh.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/stl_to_mesh/stl_to_mesh.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/stl_to_mesh/stl_to_mesh.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -1,13 +1,13 @@
-#include <OgreRoot.h>
-#include <OgreLogManager.h>
-#include <OgreMaterialManager.h>
-#include <OgreSkeletonManager.h>
-#include <OgreMeshSerializer.h>
-#include <OgreMeshManager.h>
-#include <OgreResourceGroupManager.h>
-#include <OgreMath.h>
-#include <OgreDefaultHardwareBufferManager.h>
-#include <OgreManualObject.h>
+#include <Ogre/OgreRoot.h>
+#include <Ogre/OgreLogManager.h>
+#include <Ogre/OgreMaterialManager.h>
+#include <Ogre/OgreSkeletonManager.h>
+#include <Ogre/OgreMeshSerializer.h>
+#include <Ogre/OgreMeshManager.h>
+#include <Ogre/OgreResourceGroupManager.h>
+#include <Ogre/OgreMath.h>
+#include <Ogre/OgreDefaultHardwareBufferManager.h>
+#include <Ogre/OgreManualObject.h>
/**
* @file
Modified: pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp 2009-02-20 20:50:17 UTC (rev 11458)
+++ pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp 2009-02-20 20:58:25 UTC (rev 11459)
@@ -41,9 +41,9 @@
#include "../ogre_tools/billboard_line.h"
#include "../ogre_tools/initialization.h"
-#include <OgreRoot.h>
-#include <OgreSceneManager.h>
-#include <OgreViewport.h>
+#include <Ogre/OgreRoot.h>
+#include <Ogre/OgreSceneManager.h>
+#include <Ogre/OgreViewport.h>
using namespace ogre_tools;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-02-20 23:30:39
|
Revision: 11483
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11483&view=rev
Author: isucan
Date: 2009-02-20 23:30:35 +0000 (Fri, 20 Feb 2009)
Log Message:
-----------
enabling compiler optimization
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/ompl/CMakeLists.txt
pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-20 23:15:33 UTC (rev 11482)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-20 23:30:35 UTC (rev 11483)
@@ -104,7 +104,7 @@
double evaluateGoalAux(ompl::SpaceInformationKinematic::StateKinematic_t state, std::vector<bool> *decision) const
{
- m_model->lock.lock();
+ m_model->kmodel->lock();
update(state);
if (decision)
@@ -118,7 +118,7 @@
(*decision)[i] = m_pce[i]->decide(dPos, dAng);
distance += dPos + m_pce[i]->getConstraintMessage().orientation_importance * dAng;
}
- m_model->lock.unlock();
+ m_model->kmodel->unlock();
return distance;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h 2009-02-20 23:15:33 UTC (rev 11482)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h 2009-02-20 23:30:35 UTC (rev 11483)
@@ -64,13 +64,13 @@
virtual void operator()(const ompl::SpaceInformation::State *state, double *projection) const
{
const ompl::SpaceInformationKinematic::StateKinematic *kstate = static_cast<const ompl::SpaceInformationKinematic::StateKinematic*>(state);
- m_model->lock.lock();
+ m_model->kmodel->lock();
m_model->kmodel->computeTransformsGroup(kstate->values, m_model->groupID);
const btVector3 &origin = m_link->globalTrans.getOrigin();
projection[0] = origin.x();
projection[1] = origin.y();
projection[2] = origin.z();
- m_model->lock.unlock();
+ m_model->kmodel->unlock();
}
protected:
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h 2009-02-20 23:15:33 UTC (rev 11482)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h 2009-02-20 23:30:35 UTC (rev 11483)
@@ -56,7 +56,7 @@
virtual bool operator()(const ompl::SpaceInformation::State_t state) const
{
- m_model->lock.lock();
+ m_model->kmodel->lock();
m_model->kmodel->computeTransformsGroup(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
@@ -64,7 +64,7 @@
if (valid)
valid = m_kce.decide();
- m_model->lock.unlock();
+ m_model->kmodel->unlock();
return valid;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-20 23:15:33 UTC (rev 11482)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-20 23:30:35 UTC (rev 11483)
@@ -20,7 +20,7 @@
<depend package="ompl" />
<export>
- <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lkinematic_planning_helpers"/>
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lkinematic_planning_helpers `rosboost-cfg --lflags thread`"/>
</export>
</package>
Modified: pkg/trunk/motion_planning/ompl/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-02-20 23:15:33 UTC (rev 11482)
+++ pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-02-20 23:30:35 UTC (rev 11483)
@@ -3,7 +3,7 @@
rospack(ompl)
rospack_add_boost_directories()
include_directories(${PROJECT_SOURCE_DIR}/code)
-#set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_BUILD_TYPE Release)
rospack_add_library(ompl code/ompl/base/util/src/time.cpp
code/ompl/base/util/src/output.cpp
code/ompl/base/util/src/random_utils.cpp
Modified: pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt 2009-02-20 23:15:33 UTC (rev 11482)
+++ pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt 2009-02-20 23:30:35 UTC (rev 11483)
@@ -4,8 +4,9 @@
rospack_add_boost_directories()
+set(CMAKE_BUILD_TYPE Release)
+
rospack_add_library(collision_space src/collision_space/environment.cpp
-# src/collision_space/environmentOctree.cpp
src/collision_space/environmentODE.cpp)
rospack_link_boost(collision_space thread)
Modified: pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-20 23:15:33 UTC (rev 11482)
+++ pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-20 23:30:35 UTC (rev 11483)
@@ -2,6 +2,9 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(planning_models)
rospack_add_boost_directories()
+
+set(CMAKE_BUILD_TYPE Release)
+
rospack_add_library(planning_models src/kinematic.cpp)
# Unit tests
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-02-21 01:06:03
|
Revision: 11496
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11496&view=rev
Author: tpratkanis
Date: 2009-02-21 01:05:58 +0000 (Sat, 21 Feb 2009)
Log Message:
-----------
Changing highlevel_controllers to use the new state storage method for milestone2. The three booleans (active, aborted, preempted) have been compressed into a single status flag.
Modified Paths:
--------------
pkg/trunk/highlevel/executive_python/src/movearm_adapter.py
pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
pkg/trunk/highlevel/executive_python/src/recharge_adapter.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh
pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
pkg/trunk/pr2_msgs/msg/MoveArmState.msg
pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg
Modified: pkg/trunk/highlevel/executive_python/src/movearm_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/movearm_adapter.py 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/executive_python/src/movearm_adapter.py 2009-02-21 01:05:58 UTC (rev 11496)
@@ -65,7 +65,7 @@
self.last_plan_time = rospy.get_time()
def active(self):
- return self.state.active == 1
+ return self.state.active == self.state.ACTIVE
#Have we reached a goal
def goalReached(self):
Modified: pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-02-21 01:05:58 UTC (rev 11496)
@@ -63,7 +63,7 @@
self.last_plan_time = rospy.get_time()
def active(self):
- return self.state.active == 1
+ return self.state.status == self.state.ACTIVE
#Have we reached a goal
def goalReached(self):
Modified: pkg/trunk/highlevel/executive_python/src/recharge_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/recharge_adapter.py 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/executive_python/src/recharge_adapter.py 2009-02-21 01:05:58 UTC (rev 11496)
@@ -69,4 +69,4 @@
return self.state.done == 1
def charging(self):
- return self.state.active == 1
+ return self.state.status == self.state.ACTIVE
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh 2009-02-21 01:05:58 UTC (rev 11496)
@@ -42,12 +42,12 @@
if(lastUpdated == getCurrentTick() && state != UNDEFINED)
return;
- if(stateMsg.active && state != ACTIVE){
+ if(stateMsg.status == stateMsg.ACTIVE && state != ACTIVE){
state = ACTIVE;
lastUpdated = getCurrentTick();
ROS_DEBUG("Received transition to ACTIVE");
}
- else if(!stateMsg.active && state != INACTIVE){
+ else if(stateMsg.status != stateMsg.ACTIVE && state != INACTIVE){
state = INACTIVE;
lastUpdated = getCurrentTick();
ROS_DEBUG("Received transition to INACTIVE");
@@ -83,8 +83,8 @@
if(state == INACTIVE){
obs = new ObservationByValue(timelineName, inactivePredicate);
fillInactiveObservationParameters(obs);
- obs->push_back("aborted", new BoolDomain(stateMsg.aborted));
- obs->push_back("preempted", new BoolDomain(stateMsg.preempted));
+ obs->push_back("aborted", new BoolDomain(stateMsg.status == stateMsg.ABORTED));
+ obs->push_back("preempted", new BoolDomain(stateMsg.status == stateMsg.PREEMPTED));
}
else {
obs = new ObservationByValue(timelineName, activePredicate);
@@ -132,8 +132,7 @@
// synchronizing
if(!enableController){
stateMsg.lock();
- stateMsg.active = false;
- stateMsg.preempted = true;
+ stateMsg.status = stateMsg.PREEMPTED;
handleCallback();
stateMsg.unlock();
}
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh 2009-02-21 01:05:58 UTC (rev 11496)
@@ -172,14 +172,13 @@
ROS_DEBUG("Last plan at %f seconds:", lastPlan.toSec());
ROS_DEBUG("Elapsed time is %f seconds:", elapsed_time.toSec());
if ((elapsed_time > timeout) && timeout.toSec() != 0.0) {
- this->stateMsg.aborted = 1;
+ this->stateMsg.status = this->stateMsg.ABORTED;
ROS_INFO("Controller timed out.");
deactivate();
}
handlePlanningFailure();
unlock();
} else {
- this->stateMsg.aborted = 0;
lastPlan = ros::Time::now();
}
}
@@ -207,19 +206,18 @@
bool isValid() {
return this->stateMsg.valid;
}
-
/**
* @brief Access aborted state of the planner.
*/
bool isAborted() {
- return this->stateMsg.aborted;
+ return this->stateMsg.status == this->stateMsg.ABORTED;
}
/**
* @brief Access preempted state of the planner.
*/
bool isPreempted() {
- return this->stateMsg.preempted;
+ return this->stateMsg.status == this->stateMsg.PREEMPTED;
}
/**
@@ -230,11 +228,8 @@
ROS_INFO("Activating controller with timeout of %f seconds\n", this->goalMsg.timeout);
this->state = ACTIVE;
- this->stateMsg.active = 1;
- this->stateMsg.valid = 0;
+ this->stateMsg.status = this->stateMsg.ACTIVE;
this->stateMsg.done = 0;
- this->stateMsg.aborted = 0;
- this->stateMsg.preempted = 0;
handleActivation();
}
@@ -246,7 +241,9 @@
ROS_INFO("Deactivating controller\n");
this->state = INACTIVE;
- this->stateMsg.active = 0;
+ if (this->stateMsg.status == this->stateMsg.ACTIVE) {
+ this->stateMsg.status = this->stateMsg.INACTIVE;
+ }
this->stateMsg.valid = 0;
handleDeactivation();
@@ -376,7 +373,7 @@
}
else if(state == ACTIVE){
ROS_INFO("Controller preempted.");
- this->stateMsg.preempted = 1;
+ this->stateMsg.status = this->stateMsg.PREEMPTED;
deactivate();
// If we are active, and this is a goal, publish the state message and activate. This allows us
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-02-21 01:05:58 UTC (rev 11496)
@@ -1,20 +1,27 @@
Header header
-# Is the controller actively going to a goal?
-byte active
+#Status options
+byte INACTIVE=0
+byte ACTIVE=1
+byte ABORTED=2
+byte PREEMPTED=3
+# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
+byte status
+
+#Id of the goal
+int32 goal_id
+
+#Comment for debug
+string comment
+
+
# Did the controller find a valid path?
byte valid
# Have we arrived at the goal?
byte done
-#Did the planner give up?
-byte aborted
-
-#Was the planner told to stop?
-byte preempted
-
# Actual recharge level
float32 recharge_level
Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-02-21 01:05:58 UTC (rev 11496)
@@ -1,14 +1,25 @@
Header header
-#Is the planner/controller actively trying to achieve a goal
-byte active
+
+
+#Status options
+byte INACTIVE=0
+byte ACTIVE=1
+byte ABORTED=2
+byte PREEMPTED=3
+
+# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
+byte status
+
+#Id of the goal
+int32 goal_id
+
+#Comment for debug
+string comment
+
+#Did we reach the goal?
+byte done
#Did the planner/controller find a valid way to achieve the goal
byte valid
-#Did we actually successfully arrive at the goal?
-byte done
-#Did the planner give up?
-byte aborted
-#Was the planner told to stop?
-byte preempted
#Current arm configuration
robot_msgs/JointState[] configuration
#Goal arm configuration
Modified: pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-02-21 01:05:58 UTC (rev 11496)
@@ -1,14 +1,24 @@
Header header
-#Is the planner/controller actively trying to achieve a goal
-byte active
+
+#Status options
+byte INACTIVE=0
+byte ACTIVE=1
+byte ABORTED=2
+byte PREEMPTED=3
+
+# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
+byte status
+
+#Id of the goal
+int32 goal_id
+
+#Comment for debug
+string comment
+
#Did the planner/controller find a valid way to achieve the goal
byte valid
#Did we actually successfully arrive at the goal?
byte done
-#Did the planner give up?
-byte aborted
-#Was the planner told to stop?
-byte preempted
#Current arm configuration
robot_msgs/JointState[] configuration
#Goal arm configuration
Modified: pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg 2009-02-21 00:57:29 UTC (rev 11495)
+++ pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg 2009-02-21 01:05:58 UTC (rev 11496)
@@ -1,14 +1,25 @@
Header header
-# Is the planner actively going to a goal?
-byte active
+
+
+#Status options
+byte INACTIVE=0
+byte ACTIVE=1
+byte ABORTED=2
+byte PREEMPTED=3
+
+# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
+byte status
+
+#Id of the goal
+int32 goal_id
+
+#Comment for debug
+string comment
+
# Did the planner find a valid path?
byte valid
# Have we arrived at the goal?
byte done
-#Did the planner give up?
-byte aborted
-#Was the planner told to stop?
-byte preempted
# Current location (m,m,rad)
deprecated_msgs/Pose2DFloat32 pos
# Goal location (m,m,rad)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-02-21 02:22:13
|
Revision: 11502
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11502&view=rev
Author: vijaypradeep
Date: 2009-02-21 01:38:11 +0000 (Sat, 21 Feb 2009)
Log Message:
-----------
Fixing pedal axes names to make more sense. Updating origins of tong rigid bodies
Modified Paths:
--------------
pkg/trunk/manip/teleop_base_pedals/src/teleop_base_pedals
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs13.rb
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs46.rb
Modified: pkg/trunk/manip/teleop_base_pedals/src/teleop_base_pedals
===================================================================
--- pkg/trunk/manip/teleop_base_pedals/src/teleop_base_pedals 2009-02-21 01:37:35 UTC (rev 11501)
+++ pkg/trunk/manip/teleop_base_pedals/src/teleop_base_pedals 2009-02-21 01:38:11 UTC (rev 11502)
@@ -39,12 +39,12 @@
while not rospy.is_shutdown():
if(valid_pedals(l_pedal_state) & valid_pedals(r_pedal_state)):
- cmd_vel.vel.vx = (l_pedal_state.axes[2] - r_pedal_state.axes[2]) * 0.5 * max_x_vel
- cmd_vel.vel.vy = (l_pedal_state.axes[0] - r_pedal_state.axes[1]) * 0.5 * max_y_vel
+ cmd_vel.vel.vx = (r_pedal_state.axes[2] - l_pedal_state.axes[2]) * 0.5 * max_x_vel
+ cmd_vel.vel.vy = (r_pedal_state.axes[0] - l_pedal_state.axes[1]) * 0.5 * max_y_vel
cmd_vel.vel.vz = 0
cmd_vel.ang_vel.vx = 0
cmd_vel.ang_vel.vy = 0
- cmd_vel.ang_vel.vz = (l_pedal_state.axes[2] + r_pedal_state.axes[2]) * 0.5 * max_rot_vel
+ cmd_vel.ang_vel.vz = (r_pedal_state.axes[2] + l_pedal_state.axes[2]) * 0.5 * max_rot_vel
pub.publish(cmd_vel)
rospy.sleep(0.05)
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs13.rb
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs13.rb 2009-02-21 01:37:35 UTC (rev 11501)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs13.rb 2009-02-21 01:38:11 UTC (rev 11502)
@@ -1,16 +1,16 @@
-0, -105.34 0.00 0.00
-1, -58.81 -2.79 26.35
-2, -61.41 -21.99 30.36
-3, -68.16 -26.36 15.17
-4, -3.29 -14.90 42.76
-5, -18.97 -39.18 56.78
-6, -22.29 -43.86 37.44
-7, -34.19 -44.18 20.76
-8, -80.45 5.23 -40.77
-9, -33.99 -19.06 -41.70
-10, -29.03 -12.42 -37.33
-11, -36.04 -5.23 -23.89
-12, -5.00 -56.40 -7.10
-13, 7.77 -28.04 -15.04
-14, 17.65 -24.83 -10.34
-15, 1.27 -2.26 -2.96
+0, 64.65 -28.11 -46.63
+1, 112.05 -41.08 -25.38
+2, 119.59 -45.56 -43.12
+3, 112.18 -32.71 -51.80
+4, 169.05 -42.03 -9.61
+5, 170.11 -59.63 -36.46
+6, 164.79 -41.86 -44.39
+7, 150.77 -28.96 -51.93
+8, 73.82 17.80 -35.82
+9, 124.50 31.19 -36.95
+10, 126.75 28.28 -28.33
+11, 120.66 13.43 -23.45
+12, 174.70 5.73 -53.18
+13, 170.73 16.68 -23.27
+14, 178.84 14.77 -15.48
+15, 156.24 3.20 -1.80
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs46.rb
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs46.rb 2009-02-21 01:37:35 UTC (rev 11501)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs46.rb 2009-02-21 01:38:11 UTC (rev 11502)
@@ -1,16 +1,16 @@
-16, -118.83 0.41 -0.51
-17, -107.87 40.54 8.36
-18, -59.95 -16.11 -19.74
-19, -56.06 24.66 -42.82
-20, -49.93 44.77 -7.53
-21, -22.54 -22.55 -45.98
-22, -1.90 13.67 -40.24
-23, -8.71 48.06 -45.74
-24, -104.74 32.04 46.83
-25, -112.26 -15.36 40.75
-26, -46.58 34.65 30.32
-27, -31.37 -9.49 40.71
-28, -51.77 -32.64 16.52
-29, -0.80 20.22 5.61
-30, 19.16 -18.43 7.52
-31, -11.95 -37.55 0.63
+16, 37.05 -0.51 -0.41
+17, 48.01 8.54 -40.50
+18, 95.93 -19.81 16.02
+19, 99.82 -42.71 -24.86
+20, 105.95 -7.33 -44.80
+21, 133.34 -46.08 22.34
+22, 153.98 -40.18 -13.85
+23, 147.17 -45.52 -48.27
+24, 51.14 46.98 -31.82
+25, 43.62 40.68 15.55
+26, 109.30 30.48 -34.51
+27, 124.51 40.66 9.68
+28, 104.11 16.37 32.72
+29, 155.08 5.70 -20.19
+30, 175.04 7.43 18.46
+31, 143.93 0.46 37.55
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-02-21 02:22:27
|
Revision: 11505
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11505&view=rev
Author: tpratkanis
Date: 2009-02-21 01:50:27 +0000 (Sat, 21 Feb 2009)
Log Message:
-----------
Remove done and valid booleans from highlevel controller state messages.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh
pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
pkg/trunk/pr2_msgs/msg/MoveArmState.msg
pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh 2009-02-21 01:42:39 UTC (rev 11504)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh 2009-02-21 01:50:27 UTC (rev 11505)
@@ -70,7 +70,8 @@
*/
HighlevelController(const std::string& nodeName, const std::string& _stateTopic, const std::string& _goalTopic):
initialized(false), terminated(false), stateTopic(_stateTopic),
- goalTopic(_goalTopic), controllerCycleTime_(0.1), plannerCycleTime_(0.0), plannerThread_(NULL), timeout(0, 0) {
+ goalTopic(_goalTopic), controllerCycleTime_(0.1), plannerCycleTime_(0.0), plannerThread_(NULL),
+ timeout(0, 0), done_(false), valid_(false) {
// Obtain the control frequency for this node
double controller_frequency(10);
@@ -204,7 +205,7 @@
* @brief Access for valid status of the controller
*/
bool isValid() {
- return this->stateMsg.valid;
+ return valid_;
}
/**
* @brief Access aborted state of the planner.
@@ -229,7 +230,7 @@
this->state = ACTIVE;
this->stateMsg.status = this->stateMsg.ACTIVE;
- this->stateMsg.done = 0;
+ done_ = 0;
handleActivation();
}
@@ -244,7 +245,7 @@
if (this->stateMsg.status == this->stateMsg.ACTIVE) {
this->stateMsg.status = this->stateMsg.INACTIVE;
}
- this->stateMsg.valid = 0;
+ valid_ = 0;
handleDeactivation();
}
@@ -352,6 +353,28 @@
G goalMsg; /*!< Message populated by callback */
S stateMsg; /*!< Message published. Will be populated in the control loop */
+ /**
+ * @brief Setter for state msg done flag
+ */
+ void setDone(bool isDone){
+ done_ = isDone;
+ }
+
+ /**
+ * @brief Get the done flag
+ */
+ bool getDone() {
+ return done_;
+ }
+
+ /**
+ * @brief Setter for state msg valid flag
+ */
+ void setValid(bool isValid){
+ valid_ = isValid;
+ }
+
+
private:
void goalCallback(){
@@ -452,20 +475,7 @@
unlock();
}
- /**
- * @brief Setter for state msg done flag
- */
- void setDone(bool isDone){
- this->stateMsg.done = isDone;
- }
- /**
- * @brief Setter for state msg valid flag
- */
- void setValid(bool isValid){
- this->stateMsg.valid = isValid;
- }
-
bool initialized; /*!< Marks if the node has been initialized, and is ready for use. */
bool terminated; /*!< Marks if the node has been terminated. */
const std::string stateTopic; /*!< The topic on which state updates are published */
@@ -477,6 +487,8 @@
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. */
+ bool done_; /*< True if the action is done */
+ bool valid_; /*< True if it is valid */
};
#endif
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-02-21 01:42:39 UTC (rev 11504)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-02-21 01:50:27 UTC (rev 11505)
@@ -15,13 +15,6 @@
#Comment for debug
string comment
-
-# Did the controller find a valid path?
-byte valid
-
-# Have we arrived at the goal?
-byte done
-
# Actual recharge level
float32 recharge_level
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp 2009-02-21 01:42:39 UTC (rev 11504)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp 2009-02-21 01:50:27 UTC (rev 11505)
@@ -201,7 +201,6 @@
{
lock();
stateMsg.goal = goalMsg.configuration;
- stateMsg.valid = false;
unlock();
}
@@ -303,7 +302,7 @@
if (!ret)
ROS_ERROR("Service call on plan_kinematic_path_state failed");
else
- stateMsg.done = res.value.done;
+ setDone(res.value.done);
ROS_INFO("Plan created.");
@@ -334,19 +333,19 @@
bool MoveArm::goalReached()
{
- return stateMsg.done;
+ return getDone();
}
bool MoveArm::dispatchCommands()
{
puts("in dispatchCommands");
lock();
- if (stateMsg.done || !isValid())
+ if (getDone() || !isValid())
stopArm();
else if (trajectory_changed_)
sendArmCommand(current_trajectory_, kinematic_model_);
unlock();
- return stateMsg.valid; //Do not change.
+ return isValid(); //Do not change.
}
void MoveArm::printKinematicPath(robot_msgs::KinematicPath &path)
Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-02-21 01:42:39 UTC (rev 11504)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2009-02-21 01:50:27 UTC (rev 11505)
@@ -16,10 +16,6 @@
#Comment for debug
string comment
-#Did we reach the goal?
-byte done
-#Did the planner/controller find a valid way to achieve the goal
-byte valid
#Current arm configuration
robot_msgs/JointState[] configuration
#Goal arm configuration
Modified: pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-02-21 01:42:39 UTC (rev 11504)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2009-02-21 01:50:27 UTC (rev 11505)
@@ -15,10 +15,6 @@
#Comment for debug
string comment
-#Did the planner/controller find a valid way to achieve the goal
-byte valid
-#Did we actually successfully arrive at the goal?
-byte done
#Current arm configuration
robot_msgs/JointState[] configuration
#Goal arm configuration
Modified: pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg 2009-02-21 01:42:39 UTC (rev 11504)
+++ pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg 2009-02-21 01:50:27 UTC (rev 11505)
@@ -16,10 +16,6 @@
#Comment for debug
string comment
-# Did the planner find a valid path?
-byte valid
-# Have we arrived at the goal?
-byte done
# Current location (m,m,rad)
deprecated_msgs/Pose2DFloat32 pos
# Goal location (m,m,rad)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-02-21 21:24:41
|
Revision: 11522
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11522&view=rev
Author: isucan
Date: 2009-02-21 21:24:37 +0000 (Sat, 21 Feb 2009)
Log Message:
-----------
better organization of requests; it should be easier to extend things now;
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPESTSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKKPIECESetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPKPIECESetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPLazyRRTSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPRRTSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPESTSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKKPIECESetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPIKSBLSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPKPIECESetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPLazyRRTSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPPlannerSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPRRTSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/helpers/ompl_planner/RKPSBLSetup.cpp
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/LBKPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/LBKPIECE1.cpp
pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -44,7 +44,6 @@
#include <robot_msgs/PoseConstraint.h>
#include <ros/console.h>
-#include <boost/thread/mutex.hpp>
#include <iostream>
#include <sstream>
@@ -66,13 +65,143 @@
RKPBasicRequest(void)
{
- type = R_NONE;
+ m_type = R_NONE;
+ m_activePSetup = NULL;
}
virtual ~RKPBasicRequest(void)
{
}
+ int getType(void) const
+ {
+ return m_type;
+ }
+
+ /** Set up all the data needed by motion planning based on a request and lock the planner setup
+ * using this data */
+ bool configure(ModelMap &models, const _R &req)
+ {
+ // make sure the same motion planner instance is not used by other calls
+ m_activeReq = req;
+
+ if (!isRequestValid(models, m_activeReq))
+ return false;
+
+ ROS_INFO("Selected motion planner: '%s'", m_activeReq.params.planner_id.c_str());
+
+ /* find the data we need */
+ RKPModel *m = models[m_activeReq.params.model_id];
+
+ // lock the model
+ m->lock.lock();
+
+ // get the planner setup
+ m_activePSetup = m->planners[m_activeReq.params.planner_id];
+
+ update();
+
+ /* print some information */
+ ROS_INFO("=======================================");
+ std::stringstream ss;
+ m_activePSetup->si->printSettings(ss);
+ static_cast<StateValidityPredicate*>(m_activePSetup->si->getStateValidityChecker())->getKinematicConstraintEvaluatorSet().print(ss);
+ ROS_INFO("%s", ss.str().c_str());
+ ROS_INFO("=======================================");
+
+ /* clear memory */
+ cleanupPlanningData(m_activePSetup);
+
+ return true;
+ }
+
+ void release(void)
+ {
+ if (m_activePSetup)
+ {
+ m_activePSetup->model->lock.unlock();
+ m_activePSetup = NULL;
+ }
+ }
+
+ bool isActive(void)
+ {
+ return m_activePSetup != NULL;
+ }
+
+ RKPPlannerSetup *activePlannerSetup(void)
+ {
+ return m_activePSetup;
+ }
+
+ _R& activeRequest(void)
+ {
+ return m_activeReq;
+ }
+
+ bool isStillValid(robot_msgs::KinematicPath &path)
+ {
+ update();
+
+ /* copy the path msg into a kinematic path structure (ompl style) */
+ ompl::SpaceInformationKinematic::PathKinematic_t kpath = new ompl::SpaceInformationKinematic::PathKinematic(m_activePSetup->si);
+ unsigned int dim = m_activePSetup->si->getStateDimension();
+ for (unsigned int i = 0 ; i < path.states.size() ; ++i)
+ {
+ ompl::SpaceInformationKinematic::StateKinematic_t kstate = new ompl::SpaceInformationKinematic::StateKinematic(dim);
+ kpath->states.push_back(kstate);
+ for (unsigned int j = 0 ; j < dim ; ++j)
+ kstate->values[j] = path.states[i].vals[j];
+ }
+
+ m_activePSetup->model->collisionSpace->lock();
+ bool valid = m_activePSetup->si->checkPath(kpath);
+ m_activePSetup->model->collisionSpace->unlock();
+
+ delete kpath;
+
+ /* clear memory */
+ cleanupPlanningData(m_activePSetup);
+
+ return valid;
+ }
+
+ bool isTrivial(double *distance = NULL)
+ {
+ update();
+
+ m_activePSetup->model->collisionSpace->lock();
+ bool trivial = m_activePSetup->mp->isTrivial(NULL, distance);
+ m_activePSetup->model->collisionSpace->unlock();
+
+ /* clear memory */
+ cleanupPlanningData(m_activePSetup);
+
+ return trivial;
+ }
+
+ void execute(robot_msgs::KinematicPath &path, double &distance, bool &trivial, bool &approximate)
+ {
+ update();
+
+ /* compute actual motion plan */
+ ompl::SpaceInformationKinematic::PathKinematic_t bestPath = NULL;
+ double bestDifference = 0.0;
+
+ m_activePSetup->model->collisionSpace->lock();
+ trivial = computePlan(m_activePSetup, m_activeReq.times, m_activeReq.allowed_time, m_activeReq.interpolate,
+ bestPath, bestDifference, approximate);
+ m_activePSetup->model->collisionSpace->unlock();
+
+ /* fill in the results */
+ fillSolution(m_activePSetup, bestPath, bestDifference, path, distance);
+
+ /* clear memory */
+ cleanupPlanningData(m_activePSetup);
+ }
+
+ protected:
+
/** Specializations of this class should implement this function */
bool isRequestValid(ModelMap &models, _R &req)
{
@@ -80,10 +209,23 @@
}
/** Specializations of this class should implement this function */
- void setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, _R &req)
+ void setupGoalState(RKPPlannerSetup *psetup, _R &req)
{
}
+ void update(void)
+ {
+ /* configure state space and starting state */
+ setupStateSpaceAndStartState(m_activePSetup, m_activeReq.params, m_activeReq.start_state);
+
+ std::vector<robot_msgs::PoseConstraint> cstrs;
+ m_activeReq.constraints.get_pose_vec(cstrs);
+ setupPoseConstraints(m_activePSetup, cstrs);
+
+ /* add goal state */
+ setupGoalState(m_activePSetup, m_activeReq);
+ }
+
/** Validate common space parameters */
bool areSpaceParamsValid(const ModelMap &modelsRef, robot_msgs::KinematicSpaceParameters ¶ms) const
{
@@ -126,7 +268,7 @@
}
/** Configure the state space for a given set of parameters and set the starting state */
- void setupStateSpaceAndStartState(RKPModel *model, RKPPlannerSetup *psetup,
+ void setupStateSpaceAndStartState(RKPPlannerSetup *psetup,
robot_msgs::KinematicSpaceParameters ¶ms,
robot_msgs::KinematicState &start_state)
{
@@ -144,15 +286,15 @@
const unsigned int dim = psetup->si->getStateDimension();
ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
- if (model->groupID >= 0)
+ if (psetup->model->groupID >= 0)
{
/* set the pose of the whole robot */
- model->kmodel->computeTransforms(&start_state.vals[0]);
- model->collisionSpace->updateRobotModel(model->collisionSpaceID);
+ psetup->model->kmodel->computeTransforms(&start_state.vals[0]);
+ psetup->model->collisionSpace->updateRobotModel(psetup->model->collisionSpaceID);
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = start_state.vals[model->kmodel->getModelInfo().groupStateIndexList[model->groupID][i]];
+ start->values[i] = start_state.vals[psetup->model->kmodel->getModelInfo().groupStateIndexList[psetup->model->groupID][i]];
}
else
{
@@ -172,7 +314,7 @@
/** 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)
+ ompl::SpaceInformationKinematic::PathKinematic_t &bestPath, double &bestDifference, bool &approximate)
{
if (times <= 0)
@@ -192,7 +334,8 @@
{
ROS_INFO("Solution already achieved");
bestDifference = t_distance;
-
+ approximate = false;
+
/* we want to maintain the invariant that a path will
at least consist of start & goal states, so we copy
the start state twice */
@@ -238,6 +381,7 @@
delete bestPath;
bestPath = path;
bestDifference = goal->getDifference();
+ approximate = goal->isApproximate();
goal->forgetSolutionPath();
ROS_INFO(" Obtained better solution: distance is %f", bestDifference);
}
@@ -281,139 +425,12 @@
psetup->si->clearGoal();
psetup->si->clearStartStates();
}
-
- bool isStillValid(ModelMap &models, _R &req, robot_msgs::KinematicPath &path)
- {
- // make sure the same motion planner instance is not used by other calls
- boost::mutex::scoped_lock(m_lock);
-
- if (!isRequestValid(models, req))
- return false;
-
- /* find the data we need */
- RKPModel *m = models[req.params.model_id];
- RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
-
- /* configure state space and starting state */
- setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
- std::vector<robot_msgs::PoseConstraint> cstrs;
- req.constraints.get_pose_vec(cstrs);
- setupPoseConstraints(psetup, cstrs);
-
- /* add goal state */
- setupGoalState(m, psetup, req);
-
- /* copy the path msg into a kinematic path structure (ompl style) */
- ompl::SpaceInformationKinematic::PathKinematic_t kpath = new ompl::SpaceInformationKinematic::PathKinematic(psetup->si);
- unsigned int dim = psetup->si->getStateDimension();
- for (unsigned int i = 0 ; i < path.states.size() ; ++i)
- {
- ompl::SpaceInformationKinematic::StateKinematic_t kstate = new ompl::SpaceInformationKinematic::StateKinematic(dim);
- kpath->states.push_back(kstate);
- for (unsigned int j = 0 ; j < dim ; ++j)
- kstate->values[j] = path.states[i].vals[j];
- }
-
- m->collisionSpace->lock();
- bool valid = psetup->si->checkPath(kpath);
- m->collisionSpace->unlock();
-
- delete kpath;
-
- /* clear memory */
- cleanupPlanningData(psetup);
-
- return valid;
- }
-
- bool isTrivial(ModelMap &models, _R &req, double *distance = NULL)
- {
- // make sure the same motion planner instance is not used by other calls
- boost::mutex::scoped_lock(m_lock);
-
- if (!isRequestValid(models, req))
- return false;
-
- /* find the data we need */
- RKPModel *m = models[req.params.model_id];
- RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
-
- /* configure state space and starting state */
- setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
-
- std::vector<robot_msgs::PoseConstraint> cstrs;
- req.constraints.get_pose_vec(cstrs);
- setupPoseConstraints(psetup, cstrs);
-
- /* add goal state */
- setupGoalState(m, psetup, req);
-
- m->collisionSpace->lock();
- bool trivial = psetup->mp->isTrivial(NULL, distance);
- m->collisionSpace->unlock();
-
- /* clear memory */
- cleanupPlanningData(psetup);
-
- return trivial;
- }
-
- 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);
-
- if (!isRequestValid(models, req))
- return false;
-
- ROS_INFO("Selected motion planner: '%s'", req.params.planner_id.c_str());
-
- /* find the data we need */
- RKPModel *m = models[req.params.model_id];
- RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
-
- /* configure state space and starting state */
- setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
-
- std::vector<robot_msgs::PoseConstraint> cstrs;
- req.constraints.get_pose_vec(cstrs);
- setupPoseConstraints(psetup, cstrs);
-
- /* add goal state */
- setupGoalState(m, psetup, req);
-
- /* print some information */
- ROS_INFO("=======================================");
- std::stringstream ss;
- psetup->si->printSettings(ss);
- static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->getKinematicConstraintEvaluatorSet().print(ss);
- ROS_INFO("%s", ss.str().c_str());
- ROS_INFO("=======================================");
-
- /* compute actual motion plan */
- ompl::SpaceInformationKinematic::PathKinematic_t bestPath = NULL;
- double bestDifference = 0.0;
-
- m->collisionSpace->lock();
- trivial = computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
- m->collisionSpace->unlock();
-
- /* fill in the results */
- fillSolution(psetup, bestPath, bestDifference, path, distance);
-
- /* clear memory */
- cleanupPlanningData(psetup);
-
- return true;
- }
-
// the type of request (unique ID for each type)
- int type;
+ int m_type;
- protected:
-
- boost::mutex m_lock;
+ _R m_activeReq;
+ RKPPlannerSetup *m_activePSetup;
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -50,7 +50,7 @@
{
public:
- GoalToPosition(ompl::SpaceInformation_t si, RKPModel *model, const std::vector<robot_msgs::PoseConstraint> &pc) : ompl::SpaceInformationKinematic::GoalRegionKinematic(si)
+ GoalToPosition(ompl::SpaceInformation_t si, RKPModelBase *model, const std::vector<robot_msgs::PoseConstraint> &pc) : ompl::SpaceInformationKinematic::GoalRegionKinematic(si)
{
m_model = model;
for (unsigned int i = 0 ; i < pc.size() ; ++i)
@@ -129,7 +129,7 @@
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
}
- mutable RKPModel *m_model;
+ mutable RKPModelBase *m_model;
std::vector<PoseConstraintEvaluator*> m_pce;
};
@@ -137,7 +137,7 @@
template<>
RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest>::RKPBasicRequest(void)
{
- type = R_POSITION;
+ m_type = R_POSITION;
}
/** Validate request for planning towards a link position */
@@ -160,13 +160,13 @@
/** Set the goal using a destination link position */
template<>
- void RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest>::setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, robot_msgs::KinematicPlanLinkPositionRequest &req)
+ void RKPBasicRequest<robot_msgs::KinematicPlanLinkPositionRequest>::setupGoalState(RKPPlannerSetup *psetup, robot_msgs::KinematicPlanLinkPositionRequest &req)
{
/* set the goal */
std::vector<robot_msgs::PoseConstraint> pc;
req.get_goal_constraints_vec(pc);
- GoalToPosition *goal = new GoalToPosition(psetup->si, model, pc);
+ GoalToPosition *goal = new GoalToPosition(psetup->si, psetup->model, pc);
psetup->si->setGoal(goal);
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -49,7 +49,7 @@
template<>
RKPBasicRequest<robot_msgs::KinematicPlanStateRequest>::RKPBasicRequest(void)
{
- type = R_STATE;
+ m_type = R_STATE;
}
/** Validate request for planning towards a state */
@@ -79,7 +79,7 @@
/** Set the goal using a destination state */
template<>
- void RKPBasicRequest<robot_msgs::KinematicPlanStateRequest>::setupGoalState(RKPModel *model, RKPPlannerSetup *psetup, robot_msgs::KinematicPlanStateRequest &req)
+ void RKPBasicRequest<robot_msgs::KinematicPlanStateRequest>::setupGoalState(RKPPlannerSetup *psetup, robot_msgs::KinematicPlanStateRequest &req)
{
/* set the goal */
ompl::SpaceInformationKinematic::GoalStateKinematic_t goal = new ompl::SpaceInformationKinematic::GoalStateKinematic(psetup->si);
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPModel.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -67,65 +67,65 @@
delete i->second;
}
- void addRRT(std::map<std::string, std::string> &options)
+ void addRRT(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *rrt = new RKPRRTSetup();
- if (rrt->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *rrt = new RKPRRTSetup(dynamic_cast<RKPModelBase*>(this));
+ if (rrt->setup(options))
planners[rrt->name] = rrt;
else
delete rrt;
}
- void addLazyRRT(std::map<std::string, std::string> &options)
+ void addLazyRRT(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *rrt = new RKPLazyRRTSetup();
- if (rrt->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *rrt = new RKPLazyRRTSetup(dynamic_cast<RKPModelBase*>(this));
+ if (rrt->setup(options))
planners[rrt->name] = rrt;
else
delete rrt;
}
- void addEST(std::map<std::string, std::string> &options)
+ void addEST(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *est = new RKPESTSetup();
- if (est->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *est = new RKPESTSetup(dynamic_cast<RKPModelBase*>(this));
+ if (est->setup(options))
planners[est->name] = est;
else
delete est;
}
- void addSBL(std::map<std::string, std::string> &options)
+ void addSBL(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *sbl = new RKPSBLSetup();
- if (sbl->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *sbl = new RKPSBLSetup(dynamic_cast<RKPModelBase*>(this));
+ if (sbl->setup(options))
planners[sbl->name] = sbl;
else
delete sbl;
}
- void addIKSBL(std::map<std::string, std::string> &options)
+ void addIKSBL(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *sbl = new RKPIKSBLSetup();
- if (sbl->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *sbl = new RKPIKSBLSetup(dynamic_cast<RKPModelBase*>(this));
+ if (sbl->setup(options))
planners[sbl->name] = sbl;
else
delete sbl;
}
- void addKPIECE(std::map<std::string, std::string> &options)
+ void addKPIECE(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *kpiece = new RKPKPIECESetup();
- if (kpiece->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *kpiece = new RKPKPIECESetup(dynamic_cast<RKPModelBase*>(this));
+ if (kpiece->setup(options))
planners[kpiece->name] = kpiece;
else
delete kpiece;
}
- void addIKKPIECE(std::map<std::string, std::string> &options)
+ void addIKKPIECE(const std::map<std::string, std::string> &options)
{
- RKPPlannerSetup *kpiece = new RKPIKKPIECESetup();
- if (kpiece->setup(dynamic_cast<RKPModelBase*>(this), options))
+ RKPPlannerSetup *kpiece = new RKPIKKPIECESetup(dynamic_cast<RKPModelBase*>(this));
+ if (kpiece->setup(options))
planners[kpiece->name] = kpiece;
else
delete kpiece;
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPESTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPESTSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPESTSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPESTSetup(void);
+ RKPESTSetup(RKPModelBase *m);
virtual ~RKPESTSetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKKPIECESetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKKPIECESetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKKPIECESetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPIKKPIECESetup(void);
+ RKPIKKPIECESetup(RKPModelBase *m);
virtual ~RKPIKKPIECESetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKSBLSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKSBLSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPIKSBLSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPIKSBLSetup(void);
+ RKPIKSBLSetup(RKPModelBase *m);
virtual ~RKPIKSBLSetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPKPIECESetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPKPIECESetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPKPIECESetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPKPIECESetup(void);
+ RKPKPIECESetup(RKPModelBase *m);
virtual ~RKPKPIECESetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPLazyRRTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPLazyRRTSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPLazyRRTSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPLazyRRTSetup(void);
+ RKPLazyRRTSetup(RKPModelBase *m);
virtual ~RKPLazyRRTSetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -58,22 +58,23 @@
{
public:
- RKPPlannerSetup(void);
+ RKPPlannerSetup(RKPModelBase *m);
virtual ~RKPPlannerSetup(void);
/** For each planner definition, define the set of distance metrics it can use */
virtual void setupDistanceEvaluators(void);
- virtual ompl::ProjectionEvaluator* getProjectionEvaluator(RKPModelBase *model,
- const std::map<std::string, std::string> &options) const;
+ virtual ompl::ProjectionEvaluator* getProjectionEvaluator(const std::map<std::string, std::string> &options) const;
- virtual void preSetup(RKPModelBase *model, std::map<std::string, std::string> &options);
- virtual void postSetup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual void preSetup(const std::map<std::string, std::string> &options);
+ virtual void postSetup(const std::map<std::string, std::string> &options);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options) = 0;
+ virtual bool setup(const std::map<std::string, std::string> &options) = 0;
- std::string name;
+ RKPModelBase *model;
+
+ std::string name;
ompl::Planner *mp;
ompl::GAIK *gaik;
ompl::SpaceInformationKinematic *si;
@@ -83,7 +84,10 @@
protected:
- double parseDouble(const std::string &value, double def);
+ double parseDouble(const std::string &value, double def) const;
+ bool hasOption(const std::map<std::string, std::string> &options, const std::string &opt) const;
+ double optionAsDouble(const std::map<std::string, std::string> &options, const std::string &opt, double def) const;
+ std::string optionAsString(const std::map<std::string, std::string> &options, const std::string &opt, const::std::string &def) const;
};
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPRRTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPRRTSetup.h 2009-02-21 19:11:37 UTC (rev 11521)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPRRTSetup.h 2009-02-21 21:24:37 UTC (rev 11522)
@@ -47,9 +47,9 @@
{
public:
- RKPRRTSetup(void);
+ RKPRRTSetup(RKPModelBase *m);
virtual ~RKPRRTSetup(void);
- virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options);
+ virtual bool setup(const std::map<std::string, std::string> &options);
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/ompl_planner/RKPSBLSetup.h
===================================================================
--- pkg/...
[truncated message content] |
|
From: <ge...@us...> - 2009-02-23 00:52:52
|
Revision: 11550
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11550&view=rev
Author: gerkey
Date: 2009-02-23 00:52:49 +0000 (Mon, 23 Feb 2009)
Log Message:
-----------
movearm is basically working now
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/sendarmgoal.py
pkg/trunk/demos/tabletop_manipulation/sim.launch
pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
Modified: pkg/trunk/demos/tabletop_manipulation/sendarmgoal.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/sendarmgoal.py 2009-02-23 00:52:16 UTC (rev 11549)
+++ pkg/trunk/demos/tabletop_manipulation/sendarmgoal.py 2009-02-23 00:52:49 UTC (rev 11550)
@@ -39,31 +39,36 @@
from pr2_msgs.msg import MoveArmGoal, MoveArmState
from robot_msgs.msg import JointState
+import sys
+
def callback(data):
- print 'Got state: %d:%d:%d:%d:%d'%(data.active,
- data.valid,
- data.done,
- data.aborted,
- data.preempted)
+ print 'Got status: %d'%(data.status)
-def go():
+def go(v1, v2):
pub = rospy.Publisher('right_arm_goal', MoveArmGoal)
rospy.Subscriber('right_arm_state', MoveArmState, callback)
rospy.init_node('talker', anonymous=True)
# HACK
- rospy.sleep(3.0)
+ import time
+ time.sleep(3.0)
- while not rospy.is_shutdown():
- msg = MoveArmGoal()
- msg.configuration = []
- msg.configuration.append(JointState('r_shoulder_pan_joint',-0.5,0.0,0.0,0.0,0))
- msg.enable = 1
- msg.timeout = 0.0
+ msg = MoveArmGoal()
+ msg.configuration = []
+ msg.configuration.append(JointState('r_shoulder_lift_joint',v1,0.0,0.0,0.0,0))
+ msg.configuration.append(JointState('r_shoulder_pan_joint',v2,0.0,0.0,0.0,0))
+ msg.enable = 1
+ msg.timeout = 0.0
- pub.publish(msg)
- print 'Publishing: ' + `msg.configuration`
- rospy.sleep(3.0)
+ pub.publish(msg)
+ print 'Publishing: ' + `msg.configuration`
if __name__ == '__main__':
- go()
+ if len(sys.argv) < 3:
+ print 'Using defaults'
+ v1 = -0.5
+ v2 = -1.0
+ else:
+ v1 = float(sys.argv[1])
+ v2 = float(sys.argv[2])
+ go(v1,v2)
Modified: pkg/trunk/demos/tabletop_manipulation/sim.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/sim.launch 2009-02-23 00:52:16 UTC (rev 11549)
+++ pkg/trunk/demos/tabletop_manipulation/sim.launch 2009-02-23 00:52:49 UTC (rev 11550)
@@ -4,8 +4,8 @@
<include file="$(find tabletop_manipulation)/arm.launch"/>
<include file="$(find tabletop_manipulation)/perception_sim.launch"/>
<include file="$(find tabletop_manipulation)/planning.launch"/>
- <include file="$(find tabletop_manipulation)/debug_plot.launch"/>
- <node pkg="plan_kinematic_path" type="plan_kinematic_path">
+ <!--include file="$(find tabletop_manipulation)/debug_plot.launch"/-->
+ <!--node pkg="plan_kinematic_path" type="plan_kinematic_path">
<remap from="robot_description" to="robotdesc/pr2"/>
- </node>
+ </node-->
</launch>
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp 2009-02-23 00:52:16 UTC (rev 11549)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp 2009-02-23 00:52:49 UTC (rev 11550)
@@ -81,7 +81,7 @@
#include <pr2_msgs/MoveArmState.h>
#include <pr2_msgs/MoveArmGoal.h>
-#include <robot_srvs/KinematicPlanState.h>
+#include <robot_srvs/KinematicReplanState.h>
#include <robot_msgs/DisplayKinematicPath.h>
#include <robot_msgs/KinematicPlanStatus.h>
@@ -122,14 +122,18 @@
private:
robot_msgs::MechanismState mechanism_state_msg_;
+ robot_msgs::KinematicPlanStatus plan_status_;
const std::string kinematic_model_;
const std::string controller_topic_;
const std::string controller_name_;
planning_models::KinematicModel* robot_model_;
- bool trajectory_changed_;
+ int plan_id_;
+ bool new_goal_;
+ bool new_trajectory_;
+ bool trajectory_stopped_;
+ bool plan_valid_;
robot_msgs::KinematicPath current_trajectory_;
- bool trajectory_stopped_;
// HighlevelController interface that we must implement
virtual void updateGoalMsg();
@@ -146,6 +150,8 @@
robot_msgs::JointTraj &traj);
void printKinematicPath(robot_msgs::KinematicPath &path);
void mechanismStateCallback();
+ void receiveStatus();
+ void getCurrentStateAsTraj(robot_msgs::JointTraj& traj);
};
MoveArm::~MoveArm() {
@@ -164,8 +170,9 @@
kinematic_model_(kinematic_model),
controller_topic_(controller_topic),
controller_name_(controller_name),
- robot_model_(NULL), trajectory_changed_(false),
- trajectory_stopped_(false)
+ robot_model_(NULL), plan_id_(-1), new_goal_(false),
+ new_trajectory_(false), trajectory_stopped_(false),
+ plan_valid_(false)
{
//Create robot model.
@@ -190,17 +197,35 @@
}
- ros::Node::instance()->ros::Node::subscribe("mechanism_state",
- mechanism_state_msg_,
- &MoveArm::mechanismStateCallback,
- this, 1);
+ ros::Node::instance()->subscribe("kinematic_planning_status",
+ plan_status_,
+ &MoveArm::receiveStatus,
+ this,
+ 1);
+ ros::Node::instance()->subscribe("mechanism_state",
+ mechanism_state_msg_,
+ &MoveArm::mechanismStateCallback,
+ this,
+ 1);
ros::Node::instance()->advertise<robot_msgs::JointTraj>(controller_topic_, 1);
}
+void MoveArm::receiveStatus(void)
+{
+ if((plan_id_ >= 0) &&
+ (plan_id_ == plan_status_.id) &&
+ !plan_status_.path.states.empty())
+ {
+ current_trajectory_ = plan_status_.path;
+ new_trajectory_ = true;
+ }
+}
+
void MoveArm::updateGoalMsg()
{
lock();
stateMsg.goal = goalMsg.configuration;
+ new_goal_ = true;
unlock();
}
@@ -241,6 +266,32 @@
stateMsg.unlock();
}
+void MoveArm::getCurrentStateAsTraj(robot_msgs::JointTraj& traj)
+{
+ mechanism_state_msg_.lock();
+
+ std::vector<std::string> joints;
+ robot_model_->getJointsInGroup(joints, robot_model_->getGroupID(kinematic_model_));
+
+ traj.set_points_size(1);
+ traj.points[0].set_positions_size(joints.size());
+
+ for (unsigned int i = 0; i < joints.size(); i++)
+ {
+ unsigned int j;
+ for (j = 0; j < mechanism_state_msg_.joint_states.size(); j++)
+ {
+ if(mechanism_state_msg_.joint_states[j].name == joints[i])
+ break;
+ }
+ ROS_ASSERT(j<mechanism_state_msg_.joint_states.size());
+
+ traj.points[0].positions[i] = mechanism_state_msg_.joint_states[j].position;
+ }
+
+ mechanism_state_msg_.unlock();
+}
+
void stateParamsToMsg(planning_models::KinematicModel::StateParams *state,
planning_models::KinematicModel* model,
std::string group, std::vector<double>& conf) {
@@ -252,15 +303,21 @@
bool MoveArm::makePlan()
{
+ if(!new_goal_)
+ return true;
+
+ new_goal_ = false;
+
ROS_INFO("Starting to plan...");
- robot_srvs::KinematicPlanState::Request req;
- robot_srvs::KinematicPlanState::Response res;
+ robot_srvs::KinematicReplanState::Request req;
+ robot_srvs::KinematicReplanState::Response res;
req.value.params.model_id = kinematic_model_;
req.value.params.distance_metric = "L2Square";
- req.value.params.planner_id = "SBL";
+ //req.value.params.planner_id = "SBL";
+ req.value.params.planner_id = "KPIECE";
req.value.threshold = 0.1;
req.value.interpolate = 1;
req.value.times = 1;
@@ -297,38 +354,19 @@
//req.params.volume* are left empty, because we're not planning for the base.
-
- bool ret = ros::service::call("plan_kinematic_path_state", req, res);
+ bool ret = ros::service::call("replan_kinematic_path_state", req, res);
if (!ret)
+ {
ROS_ERROR("Service call on plan_kinematic_path_state failed");
+ plan_id_ = -1;
+ }
else
- setDone(res.value.done);
-
- ROS_INFO("Plan created.");
-
- current_trajectory_ = res.value.path;
- trajectory_changed_ = true;
-
- puts("Plan:");
- for(unsigned int i=0;i<current_trajectory_.states.size();i++)
{
- printf("%d: ", i);
- for(unsigned int j=0;j<current_trajectory_.states[i].vals.size();j++)
- printf("%.3f ", current_trajectory_.states[i].vals[j]);
- puts("");
+ plan_id_ = res.id;
+ ROS_INFO("Requested plan, got id %d\n", plan_id_);
}
- //Erase the first element, because the trajectory controllers could
- //try to use it as a waypoint, slowing things down.
- //if (!current_trajectory_.states.empty() )
- // current_trajectory_.states.erase(current_trajectory_.states.begin());
-
-
- if (!res.value.valid)
- ROS_ERROR("Plan is invalid.");
-
- delete state;
- return ret && res.value.valid;
+ return true;
}
bool MoveArm::goalReached()
@@ -338,14 +376,46 @@
bool MoveArm::dispatchCommands()
{
- puts("in dispatchCommands");
- lock();
- if (getDone() || !isValid())
+ plan_status_.lock();
+
+ if (plan_id_ < 0 ||
+ plan_status_.id != plan_id_ ||
+ !new_trajectory_)
+ {
+ // NOOP
+ }
+ else if(plan_status_.valid &&
+ !plan_status_.unsafe &&
+ !current_trajectory_.states.empty())
+ {
+ sendArmCommand(current_trajectory_, kinematic_model_);
+ plan_valid_ = true;
+ new_trajectory_ = false;
+ }
+ else if(plan_status_.done)
+ {
+ ROS_INFO("Plan completed.");
+ plan_id_ = -1;
+ trajectory_stopped_ = true;
+ ROS_INFO("Execution is complete");
+ stateMsg.status = pr2_msgs::MoveArmState::INACTIVE;
+ plan_valid_ = true;
+ }
+ else
+ {
+ ROS_INFO("Plan invalid %d %d %d %d %d %d\n",
+ plan_id_ >= 0,
+ plan_status_.id == plan_id_,
+ plan_status_.valid,
+ !plan_status_.unsafe,
+ !current_trajectory_.states.empty(),
+ new_trajectory_ == true);
stopArm();
- else if (trajectory_changed_)
- sendArmCommand(current_trajectory_, kinematic_model_);
- unlock();
- return isValid(); //Do not change.
+ plan_valid_ = false;
+ }
+ plan_status_.unlock();
+
+ return plan_valid_;
}
void MoveArm::printKinematicPath(robot_msgs::KinematicPath &path)
@@ -384,32 +454,29 @@
void MoveArm::sendArmCommand(robot_msgs::KinematicPath &path,
const std::string &model)
{
+ ROS_INFO("Sending %d-step trajectory\n", path.states.size());
robot_msgs::JointTraj traj;
getTrajectoryMsg(path, traj);
ros::Node::instance()->publish(controller_topic_, traj);
trajectory_stopped_ = false;
- trajectory_changed_ = false;
+ new_trajectory_ = false;
}
void MoveArm::stopArm()
{
+ ROS_INFO("Stopping arm.");
if(!trajectory_stopped_)
{
- pr2_mechanism_controllers::TrajectoryCancel::Request stop_traj_start_req;
- pr2_mechanism_controllers::TrajectoryCancel::Response stop_traj_start_res;
- stop_traj_start_req.trajectoryid = 0; // make sure we stop all trajectories
- // /TrajectoryCancel apparently isn't yet implemented!
- trajectory_stopped_ = true;
/*
- if(!ros::service::call(controller_name_ + "/TrajectoryCancel",
- stop_traj_start_req, stop_traj_start_res))
- ROS_ERROR("Failed to cancel trajectory");
- else
- {
- ROS_INFO("Cancelled trajectory");
- trajectory_stopped_ = true;
- }
+ // Send the current state
+ robot_msgs::JointTraj traj;
+ getCurrentStateAsTraj(traj);
*/
+
+ // Send an empty trajectory
+ robot_msgs::JointTraj traj;
+ ros::Node::instance()->publish(controller_topic_, traj);
+ trajectory_stopped_ = true;
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-02-23 06:34:41
|
Revision: 11565
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11565&view=rev
Author: rob_wheeler
Date: 2009-02-23 06:34:39 +0000 (Mon, 23 Feb 2009)
Log Message:
-----------
Cleanup compiler warnings
Modified Paths:
--------------
pkg/trunk/drivers/imu/3dmgx2_driver/3dmgx2.h
pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
pkg/trunk/util/laser_scan/src/median_filter.cpp
Modified: pkg/trunk/drivers/imu/3dmgx2_driver/3dmgx2.h
===================================================================
--- pkg/trunk/drivers/imu/3dmgx2_driver/3dmgx2.h 2009-02-23 06:26:24 UTC (rev 11564)
+++ pkg/trunk/drivers/imu/3dmgx2_driver/3dmgx2.h 2009-02-23 06:34:39 UTC (rev 11565)
@@ -95,7 +95,7 @@
//! Maximum bytes allowed to be skipped when seeking a message
static const int MAX_BYTES_SKIPPED = 1000;
//! Number of KF samples to sum over
- static const int KF_NUM_SUM = 100;
+ static const unsigned int KF_NUM_SUM= 100;
//! First KF term
static const double KF_K_1 = 0.00995031;
//! Second KF term
Modified: pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2009-02-23 06:26:24 UTC (rev 11564)
+++ pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp 2009-02-23 06:34:39 UTC (rev 11565)
@@ -180,7 +180,7 @@
for (unsigned int i = 0; i < js.size(); ++i)
{
double err = scaled_positions[i] - mean;
- double pid_effort = pids_[i].updatePid(err, 0.001);
+ /*double pid_effort =*/ pids_[i].updatePid(err, 0.001);
js[i]->commanded_effort_ =
/*pid_effort / ereductions_[i] + */as[0]->command_.effort_ * ereductions_[i];
Modified: pkg/trunk/util/laser_scan/src/median_filter.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/median_filter.cpp 2009-02-23 06:26:24 UTC (rev 11564)
+++ pkg/trunk/util/laser_scan/src/median_filter.cpp 2009-02-23 06:34:39 UTC (rev 11565)
@@ -53,6 +53,8 @@
intensity_filter_ = new FilterChain<std_vector_float >();
intensity_filter_->configure(num_ranges_, xml_doc);
+
+ return true;
};
LaserMedianFilter::~LaserMedianFilter()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-02-23 07:01:23
|
Revision: 11566
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11566&view=rev
Author: rob_wheeler
Date: 2009-02-23 07:01:19 +0000 (Mon, 23 Feb 2009)
Log Message:
-----------
Cleanup compiler warnings
Modified Paths:
--------------
pkg/trunk/motion_planning/navfn/include/navwin.h
pkg/trunk/motion_planning/navfn/src/navtest.cpp
pkg/trunk/motion_planning/navfn/src/navwin.cpp
pkg/trunk/motion_planning/sbpl/src/discrete_space_information/navxythetalat/environment_navxythetalat.cpp
pkg/trunk/motion_planning/sbpl/src/utils/heap.cpp
pkg/trunk/motion_planning/sbpl/src/utils/utils.cpp
pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp
Modified: pkg/trunk/motion_planning/navfn/include/navwin.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navwin.h 2009-02-23 06:34:39 UTC (rev 11565)
+++ pkg/trunk/motion_planning/navfn/include/navwin.h 2009-02-23 07:01:19 UTC (rev 11566)
@@ -18,7 +18,7 @@
: public Fl_Double_Window
{
public:
- NavWin(int w, int h, char *name);
+ NavWin(int w, int h, const char *name);
~NavWin();
int nw,nh; // width and height of image
Modified: pkg/trunk/motion_planning/navfn/src/navtest.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navtest.cpp 2009-02-23 06:34:39 UTC (rev 11565)
+++ pkg/trunk/motion_planning/navfn/src/navtest.cpp 2009-02-23 07:01:19 UTC (rev 11566)
@@ -40,7 +40,7 @@
// <raw> is true for ROS-generated raw cost maps
-COSTTYPE *readPGM(char *fname, int *width, int *height, bool raw = false);
+COSTTYPE *readPGM(const char *fname, int *width, int *height, bool raw = false);
int main(int argc, char **argv)
{
@@ -240,11 +240,11 @@
#define unknown_gray 0xCC // seems to be the value of "unknown" in maps
COSTTYPE *
-readPGM(char *fname, int *width, int *height, bool raw)
+readPGM(const char *fname, int *width, int *height, bool raw)
{
int fake_argc(1);
- char * fake_arg("foo");
- pgm_init(&fake_argc, &fake_arg);
+ char fake_arg[] = "foo";
+ pgm_init(&fake_argc, (char **)&fake_arg);
FILE *pgmfile;
pgmfile = fopen(fname,"r");
Modified: pkg/trunk/motion_planning/navfn/src/navwin.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navwin.cpp 2009-02-23 06:34:39 UTC (rev 11565)
+++ pkg/trunk/motion_planning/navfn/src/navwin.cpp 2009-02-23 07:01:19 UTC (rev 11566)
@@ -6,7 +6,7 @@
#include <string.h>
-NavWin::NavWin(int w, int h, char *name)
+NavWin::NavWin(int w, int h, const char *name)
: Fl_Double_Window(w,h,name)
{
nw = w;
Modified: pkg/trunk/motion_planning/sbpl/src/discrete_space_information/navxythetalat/environment_navxythetalat.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl/src/discrete_space_information/navxythetalat/environment_navxythetalat.cpp 2009-02-23 06:34:39 UTC (rev 11565)
+++ pkg/trunk/motion_planning/sbpl/src/discrete_space_information/navxythetalat/environment_navxythetalat.cpp 2009-02-23 07:01:19 UTC (rev 11566)
@@ -663,7 +663,7 @@
sourcepose.theta = DiscTheta2Cont(tind, NAVXYTHETALAT_THETADIRS);
//iterate over motion primitives
- for(int aind = 0; aind < motionprimitiveV->size(); aind++)
+ for(size_t aind = 0; aind < motionprimitiveV->size(); aind++)
{
EnvNAVXYTHETALATCfg.ActionsV[tind][aind].starttheta = tind;
double mp_endx_m = motionprimitiveV->at(aind).intermptV[motionprimitiveV->at(aind).intermptV.size()-1].x;
@@ -859,7 +859,7 @@
//at this point we don't allow nonuniform number of actions
- if(motionprimitiveV->size() != (int)(NAVXYTHETALAT_THETADIRS*maxnumofactions))
+ if(motionprimitiveV->size() != (size_t)(NAVXYTHETALAT_THETADIRS*maxnumofactions))
{
printf("ERROR: nonuniform number of actions is not supported (maxnumofactions=%d while motprims=%d thetas=%d\n",
maxnumofactions, motionprimitiveV->size(), NAVXYTHETALAT_THETADIRS);
Modified: pkg/trunk/motion_planning/sbpl/src/utils/heap.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl/src/utils/heap.cpp 2009-02-23 06:34:39 UTC (rev 11565)
+++ pkg/trunk/motion_planning/sbpl/src/utils/heap.cpp 2009-02-23 07:01:19 UTC (rev 11566)
@@ -29,7 +29,7 @@
#include "../sbpl/headers.h"
-void heaperror(char* ErrorString)
+void heaperror(const char* ErrorString)
{
//need to send a message from here somehow
printf("%s\n", ErrorString);
Modified: pkg/trunk/motion_planning/sbpl/src/utils/utils.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl/src/utils/utils.cpp 2009-02-23 06:34:39 UTC (rev 11565)
+++ pkg/trunk/motion_planning/sbpl/src/utils/utils.cpp 2009-02-23 07:01:19 UTC (rev 11566)
@@ -811,4 +811,4 @@
}//over y
}//direction
}//over x
-}
\ No newline at end of file
+}
Modified: pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp 2009-02-23 06:34:39 UTC (rev 11565)
+++ pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp 2009-02-23 07:01:19 UTC (rev 11566)
@@ -70,4 +70,6 @@
{
ROS_INFO("service call failed");
}
+
+ delete node;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-02-23 22:59:08
|
Revision: 11603
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11603&view=rev
Author: sfkwc
Date: 2009-02-23 22:58:57 +0000 (Mon, 23 Feb 2009)
Log Message:
-----------
filters is now part of prcore
Added Paths:
-----------
pkg/trunk/prcore/filters/
Removed Paths:
-------------
pkg/trunk/util/filters/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-02-24 07:01:44
|
Revision: 11650
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11650&view=rev
Author: gerkey
Date: 2009-02-24 07:01:42 +0000 (Tue, 24 Feb 2009)
Log Message:
-----------
Added table polygon to Table.msg
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/prcore/robot_msgs/msg/Table.msg
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py 2009-02-24 06:45:13 UTC (rev 11649)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py 2009-02-24 07:01:42 UTC (rev 11650)
@@ -38,10 +38,18 @@
import rospy
from robot_srvs.srv import FindTable, FindTableRequest
from robot_msgs.msg import Planner2DGoal
+from deprecated_msgs.msg import RobotBase2DOdom
-import sys
+import sys, math
+robot_pose = None
+
+def odomCallback(msg):
+ global robot_pose
+ robot_pose = msg
+
def go():
+ rospy.Subscriber('odom', RobotBase2DOdom, odomCallback)
pub_goal = rospy.Publisher("goal", Planner2DGoal)
rospy.init_node('test_node', anonymous=True)
@@ -52,10 +60,10 @@
resp = s.call(FindTableRequest())
print 'Result:'
print 'Table (frame %s): %f %f %f %f' % (resp.table.header.frame_id,
- resp.table.min_x,
- resp.table.max_x,
- resp.table.min_y,
- resp.table.max_y)
+ resp.table.table_min.x,
+ resp.table.table_max.x,
+ resp.table.table_min.y,
+ resp.table.table_max.y)
print '%d objects detected on table' % len(resp.table.objects)
for o in resp.table.objects:
print ' (%f %f %f): %f %f %f' % \
@@ -64,18 +72,52 @@
o.max_bound.y - o.min_bound.y,
o.max_bound.z - o.min_bound.z)
+ global robot_pose
+ print 'robot_pose: ' + `robot_pose`
+ computeApproachPose(robot_pose, resp.table.table,0.5)
- g = Planner2DGoal()
- g.goal.x = resp.table.min_x - 1.5
- g.goal.y = resp.table.min_y - 1.5
- g.goal.th = 0.0
- g.enable = 1
- g.header.frame_id = resp.table.header.frame_id
- g.timeout = 0.0
- print 'Sending the robot to (%f,%f,%f)'%(g.goal.x,g.goal.y,g.goal.th)
- pub_goal.publish(g)
+def computeApproachPose(pose, poly, d):
+ if pose == None:
+ print 'No robot pose!'
+ return
+
+ # Find the two closest vertices
+ min_sqd = [-1.0, -1.0]
+ closestp = [None, None]
+ for p in poly.points:
+ sqd = (pose.pos.x - p.x)*(pose.pos.x - p.x) + \
+ (pose.pos.y - p.y)*(pose.pos.y - p.y)
+ if min_sqd[0] < 0.0 or sqd < min_sqd[0]:
+ min_sqd[0] = sqd
+ closestp[0] = p
+ elif min_sqd[1] < 0.0 or sqd < min_sqd[1]:
+ min_sqd[1] = sqd
+ closestp[1] = p
+
+ if closestp[0] == None or closestp[0] == None:
+ print 'No pair of closest points!'
+ return
+
+ print 'Closest points: (%f,%f,%f),(%f,%f,%f)'%(closestp[0].x,closestp[0].y,closestp[0].z,closestp[1].x,closestp[1].y,closestp[1].z)
+
+
+ #g = Planner2DGoal()
+ #g.goal.x = resp.table.min_x - 1.5
+ #g.goal.y = resp.table.min_y - 1.5
+ #g.goal.th = 0.0
+ #g.enable = 1
+ #g.header.frame_id = resp.table.header.frame_id
+ #g.timeout = 0.0
+
+ #print 'Sending the robot to (%f,%f,%f)'%(g.goal.x,g.goal.y,g.goal.th)
+ #pub_goal.publish(g)
+
+ #print 'Poly:'
+ #for p in resp.table.table.points:
+ # print ' %f %f %f'%(p.x,p.y,p.z)
+
if __name__ == '__main__':
go()
Modified: pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
===================================================================
--- pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-02-24 06:45:13 UTC (rev 11649)
+++ pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-02-24 07:01:42 UTC (rev 11650)
@@ -314,10 +314,10 @@
{
tf_.transformPoint(global_frame_, minPstamped_local, minPstamped_global);
tf_.transformPoint(global_frame_, maxPstamped_local, maxPstamped_global);
- resp.table.min_x = minPstamped_global.point.x;
- resp.table.min_y = minPstamped_global.point.y;
- resp.table.max_x = maxPstamped_global.point.x;
- resp.table.max_y = maxPstamped_global.point.y;
+ resp.table.table_min.x = minPstamped_global.point.x;
+ resp.table.table_min.y = minPstamped_global.point.y;
+ resp.table.table_max.x = maxPstamped_global.point.x;
+ resp.table.table_max.y = maxPstamped_global.point.y;
}
catch (tf::ConnectivityException)
{
@@ -366,6 +366,7 @@
return false;
}
+ resp.table.table = pmap_.polygons[0];
// Reserve enough space
if (publish_debug_)
@@ -387,7 +388,7 @@
gettimeofday (&t2, NULL);
time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
ROS_INFO ("Table found. Bounds: [%f, %f] -> [%f, %f]. Number of objects: %d. Total time: %f.",
- resp.table.min_x, resp.table.min_y, resp.table.max_x, resp.table.max_y, resp.table.objects.size (), time_spent);
+ resp.table.table_min.x, resp.table.table_min.y, resp.table.table_max.x, resp.table.table_max.y, resp.table.objects.size (), time_spent);
return (true);
}
Modified: pkg/trunk/prcore/robot_msgs/msg/Table.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Table.msg 2009-02-24 06:45:13 UTC (rev 11649)
+++ pkg/trunk/prcore/robot_msgs/msg/Table.msg 2009-02-24 07:01:42 UTC (rev 11650)
@@ -1,6 +1,5 @@
Header header
-float32 min_x
-float32 max_x
-float32 min_y
-float32 max_y
+Polygon3D table
+Point table_min
+Point table_max
ObjectOnTable[] objects
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-02-24 19:36:39
|
Revision: 11670
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11670&view=rev
Author: rob_wheeler
Date: 2009-02-24 19:36:31 +0000 (Tue, 24 Feb 2009)
Log Message:
-----------
Merge nrt branch back to trunk
Modified Paths:
--------------
pkg/trunk/3rdparty/eml/Makefile
pkg/trunk/3rdparty/eml/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ek1122.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_device.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg014.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/src/ek1122.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg014.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
pkg/trunk/prcore/filters/include/filters/filter_base.h
pkg/trunk/prcore/filters/test/test_chain.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/CMakeLists.txt
pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.machine
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.machine
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf_manip.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.machine
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/sim.machine
Added Paths:
-----------
pkg/trunk/drivers/motor/ethercat_hardware/msg/MotorModel.msg
pkg/trunk/robot_control_loops/pr2_etherCAT/msg/
pkg/trunk/robot_control_loops/pr2_etherCAT/msg/RealtimeStats.msg
Removed Paths:
-------------
pkg/trunk/3rdparty/rtnet/
pkg/trunk/3rdparty/xenomai/
pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/launch
Modified: pkg/trunk/3rdparty/eml/Makefile
===================================================================
--- pkg/trunk/3rdparty/eml/Makefile 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/3rdparty/eml/Makefile 2009-02-24 19:36:31 UTC (rev 11670)
@@ -1,7 +1,7 @@
all: installed
-TARBALL = build/eml-r36-patched.tar.bz2
-TARBALL_URL = http://pr.willowgarage.com/downloads/eml-r36-patched.tar.bz2
+TARBALL = build/eml-nrt-r36-patched.tar.bz2
+TARBALL_URL = http://pr.willowgarage.com/downloads/eml-nrt-r36-patched.tar.bz2
SOURCE_DIR = build/eml-svn
UNPACK_CMD = tar xjf
TARBALL_PATCH = unistd.patch
@@ -11,7 +11,7 @@
cd $(SOURCE_DIR) && \
mkdir -p build && \
cd build && \
- cmake -D RTNET_INSTALL_PATH=`rospack find rtnet`/rtnet -D XENOMAI_INSTALL_PATH=`rospack find xenomai`/xenomai -D CMAKE_INSTALL_PREFIX=`rospack find eml`/eml .. && \
+ cmake -D CMAKE_INSTALL_PREFIX=`rospack find eml`/eml .. && \
make install
touch installed
Modified: pkg/trunk/3rdparty/eml/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/eml/manifest.xml 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/3rdparty/eml/manifest.xml 2009-02-24 19:36:31 UTC (rev 11670)
@@ -9,7 +9,6 @@
<license>GPL</license>
<review status="3rdparty" notes=""/>
<url>http://ethercatmaster.berlios.de/</url>
-<depend package="rtnet"/>
<export>
<cpp lflags="-L${prefix}/eml/lib -leml" cflags="-I${prefix}/eml/include"/>
<doxymaker external="http://ethercatmaster.berlios.de/doxygen/html/index.html"/>
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ek1122.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ek1122.h 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ek1122.h 2009-02-24 19:36:31 UTC (rev 11670)
@@ -42,7 +42,7 @@
public:
EK1122() : EthercatDevice() {}
EthercatDevice *configure(int &start_address, EtherCAT_SlaveHandler *sh);
- int initialize(Actuator *, bool);
+ int initialize(Actuator *, bool, bool);
void convertCommand(ActuatorCommand &command, unsigned char *buffer) {}
void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer) {}
void computeCurrent(ActuatorCommand &command) {}
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_device.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_device.h 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_device.h 2009-02-24 19:36:31 UTC (rev 11670)
@@ -59,7 +59,7 @@
virtual ~EthercatDevice() {}
virtual EthercatDevice *configure(int &startAddress, EtherCAT_SlaveHandler *sh) = 0;
- virtual int initialize(Actuator *, bool allow_unprogrammed=0) = 0;
+ virtual int initialize(Actuator *, bool allow_unprogrammed=0, bool motor_model=0) = 0;
virtual void initXml(TiXmlElement *) {}
virtual void convertCommand(ActuatorCommand &command, unsigned char *buffer) = 0;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2009-02-24 19:36:31 UTC (rev 11670)
@@ -68,7 +68,7 @@
/*!
* \brief Initialize the EtherCAT Master Library.
*/
- void init(char *interface, bool allow_unprogrammed);
+ void init(char *interface, bool allow_unprogrammed, bool motor_model);
/*!
* \brief Register actuators with mechanism control
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg014.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg014.h 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg014.h 2009-02-24 19:36:31 UTC (rev 11670)
@@ -43,7 +43,7 @@
WG014() : EthercatDevice() {}
~WG014();
EthercatDevice *configure(int &start_address, EtherCAT_SlaveHandler *sh);
- int initialize(Actuator *, bool);
+ int initialize(Actuator *, bool, bool);
void convertCommand(ActuatorCommand &command, unsigned char *buffer) {}
void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer) {}
void computeCurrent(ActuatorCommand &command) {}
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h 2009-02-24 19:36:31 UTC (rev 11670)
@@ -39,6 +39,7 @@
#include <realtime_tools/realtime_publisher.h>
#include <ethercat_hardware/PressureState.h>
+#include <ethercat_hardware/MotorModel.h>
struct WG0XMbxHdr
{
@@ -203,6 +204,7 @@
strings_.reserve(10);
values_.reserve(10);
reason_ = "OK";
+ level_ = 0;
voltage_error_ = max_voltage_error_ = 0;
current_error_ = max_current_error_ = 0;
@@ -214,11 +216,12 @@
drops_ = 0;
consecutive_drops_ = 0;
max_consecutive_drops_ = 0;
+ motor_publisher_ = 0;
}
~WG0X();
EthercatDevice *configure(int &start_address, EtherCAT_SlaveHandler *sh);
- int initialize(Actuator *, bool);
+ int initialize(Actuator *, bool allow_unprogrammed=true, bool motor_model=false);
void initXml(TiXmlElement *);
void convertCommand(ActuatorCommand &command, unsigned char *buffer);
@@ -291,6 +294,7 @@
vector<robot_msgs::DiagnosticString> strings_;
vector<robot_msgs::DiagnosticValue> values_;
string reason_;
+ int level_;
double voltage_error_, max_voltage_error_;
double current_error_, max_current_error_;
double voltage_estimate_;
@@ -301,6 +305,8 @@
int drops_;
int consecutive_drops_;
int max_consecutive_drops_;
+
+ realtime_tools::RealtimePublisher<ethercat_hardware::MotorModel> *motor_publisher_;
};
class WG05 : public WG0X
Modified: pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2009-02-24 19:36:31 UTC (rev 11670)
@@ -13,7 +13,6 @@
<depend package='loki' />
<depend package='realtime_tools' />
<depend package='robot_msgs' />
-<depend package='xenomai' />
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lethercat_hardware -Wl,-rpath,${prefix}/lib"/>
</export>
Added: pkg/trunk/drivers/motor/ethercat_hardware/msg/MotorModel.msg
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/msg/MotorModel.msg (rev 0)
+++ pkg/trunk/drivers/motor/ethercat_hardware/msg/MotorModel.msg 2009-02-24 19:36:31 UTC (rev 11670)
@@ -0,0 +1,12 @@
+Header header
+
+float64 measured_current
+float64 commanded_current
+float64 current_error
+int32 consecutive_current_errors
+
+float64 measured_voltage
+float64 expected_voltage
+float64 voltage_error
+int32 consecutive_voltage_errors
+
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ek1122.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ek1122.cpp 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ek1122.cpp 2009-02-24 19:36:31 UTC (rev 11670)
@@ -47,7 +47,7 @@
return this;
}
-int EK1122::initialize(Actuator *, bool)
+int EK1122::initialize(Actuator *, bool, bool)
{
ROS_INFO("Device #%02d: EK1122 (%#08x)", sh_->get_ring_position(), sh_->get_product_code());
return 0;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2009-02-24 19:36:31 UTC (rev 11670)
@@ -71,7 +71,7 @@
publisher_.stop();
}
-void EthercatHardware::init(char *interface, bool allow_unprogrammed)
+void EthercatHardware::init(char *interface, bool allow_unprogrammed, bool motor_model)
{
// Initialize network interface
interface_ = interface;
@@ -81,7 +81,7 @@
ROS_BREAK();
}
-#if 1
+#if 0
if (set_socket_timeout(ni_, 1000*500))
{
ROS_FATAL("Unable to change socket timeout");
@@ -146,6 +146,7 @@
else
{
ROS_FATAL("Unable to configure slave #%d, product code: %d, revision: %d", slave, sh->get_product_code(), sh->get_revision());
+ ROS_FATAL("Perhaps you should power-cycle the MCBs");
ROS_BREAK();
}
}
@@ -166,7 +167,7 @@
// Initialize slaves
for (unsigned int slave = 0, a = 0; slave < num_slaves_; ++slave)
{
- if (slaves_[slave]->initialize(slaves_[slave]->has_actuator_ ? hw_->actuators_[a] : NULL, allow_unprogrammed) < 0)
+ if (slaves_[slave]->initialize(slaves_[slave]->has_actuator_ ? hw_->actuators_[a] : NULL, allow_unprogrammed, motor_model) < 0)
{
ROS_FATAL("Unable to initialize slave #%d", slave);
ROS_BREAK();
@@ -307,6 +308,7 @@
{
reset_state_ = 2 * num_slaves_;
halt_motors_ = false;
+ diagnostics_.max_roundtrip_ = 0;
}
for (unsigned int s = 0, a = 0; s < num_slaves_; ++s)
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg014.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg014.cpp 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg014.cpp 2009-02-24 19:36:31 UTC (rev 11670)
@@ -53,7 +53,7 @@
return this;
}
-int WG014::initialize(Actuator *, bool)
+int WG014::initialize(Actuator *, bool, bool)
{
ROS_INFO("Device #%02d: WG014 (%#08x)", sh_->get_ring_position(), sh_->get_product_code());
return 0;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2009-02-24 19:36:31 UTC (rev 11670)
@@ -101,6 +101,7 @@
WG0X::~WG0X()
{
+ if (motor_publisher_) delete motor_publisher_;
delete sh_->get_fmmu_config();
delete sh_->get_pd_config();
}
@@ -179,7 +180,7 @@
return this;
}
-int WG0X::initialize(Actuator *actuator, bool allow_unprogrammed)
+int WG0X::initialize(Actuator *actuator, bool allow_unprogrammed, bool motor_model)
{
unsigned int revision = sh_->get_revision();
unsigned int major = (revision >> 8) & 0xff;
@@ -243,6 +244,8 @@
actuator->name_ = actuator_info_.name_;
backemf_constant_ = 1.0 / (actuator_info_.speed_constant_ * 2 * M_PI * 1.0/60);
ROS_INFO(" Name: %s", actuator_info_.name_);
+ string topic = "/motor_model/" + string(actuator_info_.name_);
+ motor_publisher_ = motor_model ? new realtime_tools::RealtimePublisher<ethercat_hardware::MotorModel>(topic, 1) : 0;
#if 0
ROS_INFO(" major: %d", actuator_info_.major_); // Major revision
ROS_INFO(" minor: %d", actuator_info_.minor_); // Minor revision
@@ -386,13 +389,16 @@
bool WG0X::verifyState(ActuatorState &state, unsigned char *this_buffer, unsigned char *prev_buffer)
{
bool rv = true;
- double expected_voltage = state.last_measured_current_ * actuator_info_.resistance_ + state.velocity_ * actuator_info_.encoder_reduction_ * backemf_constant_;
+ WG0XStatus *this_status, *prev_status;
+ double expected_voltage;
+ int level = 0;
+ string reason = "OK";
if (!(state.is_enabled_)) {
goto end;
}
- WG0XStatus *this_status, *prev_status;
+ expected_voltage = state.last_measured_current_ * actuator_info_.resistance_ + state.velocity_ * actuator_info_.encoder_reduction_ * backemf_constant_;
this_status = (WG0XStatus *)this_buffer;
prev_status = (WG0XStatus *)prev_buffer;
@@ -411,17 +417,19 @@
if (consecutive_drops_ > 10)
{
rv = false;
- reason_ = "Too many dropped packets";
+ reason = "Too many dropped packets";
+ level = 2;
goto end;
}
if (this_status->mode_ & MODE_SAFETY_LOCKOUT)
{
rv = false;
- reason_ = "Safety Lockout";
+ reason = "Safety Lockout";
+ level = 2;
if (readMailbox(sh_, WG0XConfigInfo::CONFIG_INFO_BASE_ADDR, &config_info_, sizeof(config_info_)) != 0)
{
- reason_ += ": unable to read mailbox too";
+ reason += ": unable to read mailbox too";
}
goto end;
}
@@ -449,22 +457,26 @@
//motor_velocity == 0 -> encoder failure likely
if (fabs(state.velocity_) < epsilon)
{
- reason_ = "Encoder failure likely";
+ reason = "Encoder failure likely";
+ level = 1;
}
//measured_current_ ~= 0 -> motor open-circuit likely
else if (fabs(state.last_measured_current_) < epsilon)
{
- reason_ = "Motor open-circuit likely";
+ reason = "Motor open-circuit likely";
+ level = 1;
}
//motor_voltage_ ~= 0 -> motor short-circuit likely
else if (fabs(voltage_estimate_) < epsilon)
{
- reason_ = "Motor short-circuit likely";
+ reason = "Motor short-circuit likely";
+ level = 1;
}
//else -> current-sense failure likely
else
{
- reason_ = "Current-sense failure likely";
+ reason = "Current-sense failure likely";
+ level = 1;
}
}
}
@@ -479,30 +491,47 @@
current_error_ = fabs(state.last_measured_current_ - last_commanded_current);
max_current_error_ = max(current_error_, max_current_error_);
-#if 0
- if (current_error_ > 0.1 &&
+
+ if (current_error_ > 0.05 &&
(last_commanded_current > 0 ?
prev_status->programmed_pwm_value_ < 8400 :
prev_status->programmed_pwm_value_ > -8400))
{
-printf("current_error: %f [%d], cmd: %f, mes: %f, pwm: %d, %s\n", current_error_, consecutive_current_errors_, last_commanded_current, state.last_measured_current_, prev_status->programmed_pwm_value_, actuator_info_.name_);
+//printf("current_error: %f [%d], cmd: %f, mes: %f, pwm: %d, %s\n", current_error_, consecutive_current_errors_, last_commanded_current, state.last_measured_current_, prev_status->programmed_pwm_value_, actuator_info_.name_);
++consecutive_current_errors_;
- if (consecutive_current_errors_ > 1000)
+ if (consecutive_current_errors_ > 100)
{
//complain and shut down
- rv = false;
- reason_ = "Current loop error too large";
+ //rv = false;
+ reason = "Current loop error too large";
+ level = 2;
}
}
else
{
consecutive_current_errors_ = 0;
}
-#endif
- if (rv) reason_ = "OK";
+ if (motor_publisher_ && motor_publisher_->trylock())
+ {
+ motor_publisher_->msg_.measured_current = state.last_measured_current_;
+ motor_publisher_->msg_.commanded_current = last_commanded_current;
+ motor_publisher_->msg_.current_error = current_error_;
+ motor_publisher_->msg_.consecutive_current_errors = consecutive_current_errors_;
+ motor_publisher_->msg_.measured_voltage = voltage_estimate_;
+ motor_publisher_->msg_.expected_voltage = expected_voltage;
+ motor_publisher_->msg_.voltage_error = voltage_error_;
+ motor_publisher_->msg_.consecutive_voltage_errors = consecutive_voltage_errors_;
+ motor_publisher_->unlockAndPublish();
+ }
+
end:
+ if (level_ != 1)
+ {
+ level_ = level;
+ reason_ = reason;
+ }
return rv;
}
@@ -841,7 +870,7 @@
str << "EtherCAT Device (" << actuator_info_.name_ << ")";
d.name = str.str();
d.message = reason_;
- d.level = reason_ == "OK" ? 0 : 2;
+ d.level = level_;
ADD_STRING("Configuration", config_info_.configuration_status_ ? "good" : "error loading configuration");
ADD_STRING("Name", actuator_info_.name_);
Modified: pkg/trunk/prcore/filters/include/filters/filter_base.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/filter_base.h 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/prcore/filters/include/filters/filter_base.h 2009-02-24 19:36:31 UTC (rev 11670)
@@ -35,38 +35,10 @@
#include <loki/Factory.h>
#include "ros/assert.h"
-typedef std::vector<float> std_vector_float;
-typedef std::vector<double> std_vector_double;
-
-
namespace filters
{
-std::string getFilterID(const std::string & filter_name, const std::string & typestring)
-{
- if (typestring == "int")
- return filter_name + "i";
- else if (typestring == "float")
- return filter_name + "f";
-
- else if (typestring == "double")
- return filter_name + "d";
-
- else if (typestring == "std_vector_float")
- return filter_name + "St6vectorIfSaIfEE";
-
- else if (typestring == "std_vector_double")
- return filter_name + "St6vectorIfSaIdEE";
-
- else
- printf("%s\n", typestring.c_str());
- ROS_ASSERT(typestring == "NOT A SUPPORTED TYPE");
- return "";
-
-}
-
-
/** \brief A Base filter class to provide a standard interface for all filters
*
*/
@@ -101,8 +73,7 @@
#define ROS_REGISTER_FILTER(c,t) \
filters::FilterBase<t> * Filters_New_##c##__##t() {return new c< t >;}; \
bool ROS_FILTER_## c ## _ ## t = \
- filters::FilterFactory<t>::Instance().Register(filters::getFilterID(#c , #t ), Filters_New_##c##__##t);
-///\todo make this use templating to get the data type, the user doesn't ever set the data type at runtime
+ filters::FilterFactory<t>::Instance().Register(std::string(#c) + typeid(t).name(), Filters_New_##c##__##t);
}
Modified: pkg/trunk/prcore/filters/test/test_chain.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_chain.cpp 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/prcore/filters/test/test_chain.cpp 2009-02-24 19:36:31 UTC (rev 11670)
@@ -70,7 +70,7 @@
TEST(FilterChain, configuring){
double epsilon = 1e-9;
printf("Chain test starting\n");
- filters::FilterChain<std_vector_float > chain;
+ filters::FilterChain<std::vector<float> > chain;
//filters::FilterChain<float> chain;
// EXPECT_TRUE(chain.add(mean_filter_5));
@@ -97,7 +97,7 @@
}
TEST(FilterChain, MisconfiguredNumberOfChannels){
- filters::FilterChain<std_vector_float > chain;
+ filters::FilterChain<std::vector<float> > chain;
// EXPECT_TRUE(chain.add(mean_filter_5));
@@ -120,7 +120,7 @@
}
TEST(FilterChain, OverlappingNames){
- filters::FilterChain<std_vector_float > chain;
+ filters::FilterChain<std::vector<float> > chain;
std::string bad_xml = "<filters> <filter type=\"MeanFilter\" name=\"mean_test\"> <params number_of_observations=\"5\"/></filter><filter type=\"MedianFilter\" name=\"mean_test\"> <params number_of_observations=\"5\"/></filter></filters>";
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/CMakeLists.txt 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/CMakeLists.txt 2009-02-24 19:36:31 UTC (rev 11670)
@@ -1,4 +1,8 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(pr2_etherCAT)
+genmsg()
+rospack_add_boost_directories()
rospack_add_executable(pr2_etherCAT src/main.cpp)
+rospack_link_boost(pr2_etherCAT thread)
+rospack_remove_compile_flags(pr2_etherCAT -W)
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml 2009-02-24 19:36:31 UTC (rev 11670)
@@ -14,7 +14,6 @@
<depend package="pr2_mechanism_controllers"/>
<depend package="joint_qualification_controllers"/>
<depend package="realtime_tools"/>
- <depend package="xenomai"/>
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
</package>
Added: pkg/trunk/robot_control_loops/pr2_etherCAT/msg/RealtimeStats.msg
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/msg/RealtimeStats.msg (rev 0)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/msg/RealtimeStats.msg 2009-02-24 19:36:31 UTC (rev 11670)
@@ -0,0 +1,3 @@
+float64 jitter
+float64 avg_jitter
+float64 max_jitter
Deleted: pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/launch
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/launch 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/launch 2009-02-24 19:36:31 UTC (rev 11670)
@@ -1,14 +0,0 @@
-#!/bin/sh
-
-ifconfig eth0 > /dev/null 2>&1
-if [ "$?" = "0" ]; then
- echo "In non-realtime mode"
- echo "Switching to realtime"
- /usr/local/rtnet/sbin/rtup
- sleep 5
-else
- echo "In realtime mode"
-fi
-
-echo "Launching pr2_etherCAT with options $*"
-`$ROS_ROOT/bin/rospack find pr2_etherCAT`/pr2_etherCAT $*
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2009-02-24 19:27:21 UTC (rev 11669)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2009-02-24 19:36:31 UTC (rev 11670)
@@ -41,8 +41,6 @@
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
-#include <native/task.h>
-#include <native/timer.h>
#include <mechanism_control/mechanism_control.h>
#include <ethercat_hardware/ethercat_hardware.h>
@@ -50,9 +48,16 @@
#include <ros/node.h>
#include <std_srvs/Empty.h>
+#include <pr2_etherCAT/RealtimeStats.h>
#include <realtime_tools/realtime_publisher.h>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/max.hpp>
+#include <boost/accumulators/statistics/mean.hpp>
+using namespace boost::accumulators;
+
static struct
{
char *program_;
@@ -61,6 +66,8 @@
bool allow_override_;
bool allow_unprogrammed_;
bool quiet_;
+ bool motor_model_;
+ bool stats;
} g_options;
void Usage(string msg = "")
@@ -70,8 +77,10 @@
fprintf(stderr, " -i, --interface <interface> Connect to EtherCAT devices on this interface\n");
fprintf(stderr, " -x, --xml <file|param> Load the robot description from this file or parameter name\n");
fprintf(stderr, " -u, --allow_unprogrammed Allow control loop to run with unprogrammed devices\n");
+ fprintf(stderr, " -m, --motor_model Publish motor model values\n");
+ fprintf(stderr, " -s, --stats Publish realtime statistics\n");
fprintf(stderr, " -q, --quiet Don't print warning messages when switching to secondary mode\n");
- fprintf(stderr, " -h, --help Print this message and exit\n");
+ fprintf(stderr, " -h, --help Print this message and exit\n");
if (msg != "")
{
fprintf(stderr, "Error: %s\n", msg.c_str());
@@ -111,9 +120,9 @@
for (int i = 0; i < 1000; ++i)
{
total_ec += diagnostics.ec[i];
- max_ec = max(max_ec, diagnostics.ec[i]);
+ max_ec = std::max(max_ec, diagnostics.ec[i]);
total_mc += diagnostics.mc[i];
- max_mc = max(max_mc, diagnostics.mc[i]);
+ max_mc = std::max(max_mc, diagnostics.mc[i]);
}
#define ADD_VALUE(lab, val) \
@@ -149,39 +158,28 @@
static inline double now()
{
- RTIME n;
- n = rt_timer_read();
- return double(n) / NSEC_PER_SEC;
+ struct timespec n;
+ clock_gettime(CLOCK_MONOTONIC, &n);
+ return double(n.tv_nsec) / NSEC_PER_SEC + n.tv_sec;
}
+void *controlLoop(void *)
+{
+ realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher("/diagnostics", 2);
+ realtime_tools::RealtimePublisher<pr2_etherCAT::RealtimeStats> *rtpublisher = 0;
+ accumulator_set<double, stats<tag::max, tag::mean> > acc;
-static void syncClocks(void *)
-{
- while (!g_quit)
+ if (g_options.stats)
{
- struct timespec ts;
- struct timeval tv;
- gettimeofday(&tv, NULL);
- ts.tv_sec = tv.tv_sec;
- ts.tv_nsec = tv.tv_usec * 1000;
- rt_timer_set(tv.tv_sec * 1000000000LL + tv.tv_usec * 1000LL);
- usleep(200000);
+ rtpublisher = new realtime_tools::RealtimePublisher<pr2_etherCAT::RealtimeStats> ("/realtime", 2);
}
-}
-
-static RT_TASK clockTask, controlTask;
-
-void controlLoop(void *)
-{
- realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher("/diagnostics", 2);
-
// Publish one-time before entering real-time to pre-allocate message vectors
publishDiagnostics(publisher);
// Initialize the hardware interface
EthercatHardware ec;
- ec.init(g_options.interface_, g_options.allow_unprogrammed_);
+ ec.init(g_options.interface_, g_options.allow_unprogrammed_, g_options.motor_model_);
// Create mechanism control
MechanismControl mc(ec.hw_);
@@ -207,13 +205,14 @@
exit(1);
}
}
- urdf::normalizeXml(xml.RootElement());
+ TiXmlElement *root_element = xml.RootElement();
TiXmlElement *root = xml.FirstChildElement("robot");
- if (!root)
+ if (!root || !root_element)
{
- ROS_FATAL("Could not load the xml file: %s\n", g_options.xml_);
- exit(1);
+ ROS_FATAL("Could not parse the xml from %s\n", g_options.xml_);
+ exit(1);
}
+ urdf::normalizeXml(root_element);
// Register actuators with mechanism control
ec.initXml(root, g_options.allow_override_);
@@ -229,20 +228,24 @@
mc.spawnController(elt->Attribute("type"), elt->Attribute("name"), elt);
}
- // Switch to hard real-time
-#if defined(__XENO__)
- rt_task_set_mode(0, T_PRIMARY|T_WARNSW, NULL);
-#endif
+ struct timespec tick;
+ clock_gettime(CLOCK_MONOTONIC, &tick);
+ int period = 1e+6; // 1 ms in nanoseconds
- rt_task_set_periodic(NULL, TM_NOW, 1e+6);
+ // Up priority for this thread
+ struct sched_param thread_param;
+ int policy = SCHED_FIFO;
+ thread_param.sched_priority = sched_get_priority_max(policy);
+ pthread_setschedparam(pthread_self(), policy, &thread_param);
static int count = 0;
while (!g_quit)
{
- rt_task_wait_period(NULL);
double start = now();
if (g_reset_motors)
{
+ accumulator_set<double, stats<tag::max, tag::mean> > zero;
+ acc = zero;
ec.update(true);
g_reset_motors = false;
}
@@ -263,6 +266,28 @@
count = 0;
}
+ tick.tv_nsec += period;
+ while (tick.tv_nsec >= NSEC_PER_SEC)
+ {
+ tick.tv_nsec -= NSEC_PER_SEC;
+ tick.tv_sec++;
+ }
+ clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &tick, NULL);
+
+ if (rtpublisher)
+ {
+ struct timespec after;
+ clock_gettime(CLOCK_MONOTONIC, &after);
+ double jitter = (after.tv_sec - tick.tv_sec + double(after.tv_nsec-tick.tv_nsec)/1e9)*1e6;
+ acc(jitter);
+ if (rtpublisher->trylock())
+ {
+ rtpublisher->msg_.jitter = jitter;
+ rtpublisher->m...
[truncated message content] |
|
From: <tf...@us...> - 2009-02-24 23:34:45
|
Revision: 11693
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11693&view=rev
Author: tfoote
Date: 2009-02-24 23:34:41 +0000 (Tue, 24 Feb 2009)
Log Message:
-----------
standardizing on passing of TiXmlElement * and getting laser median filter working as a full filter
Modified Paths:
--------------
pkg/trunk/prcore/filters/include/filters/filter_chain.h
pkg/trunk/prcore/filters/test/test_chain.cpp
pkg/trunk/util/laser_scan/CMakeLists.txt
pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
pkg/trunk/util/laser_scan/src/median_filter_node.cpp
Removed Paths:
-------------
pkg/trunk/util/laser_scan/src/median_filter.cpp
Modified: pkg/trunk/prcore/filters/include/filters/filter_chain.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/filter_chain.h 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/prcore/filters/include/filters/filter_chain.h 2009-02-24 23:34:41 UTC (rev 11693)
@@ -49,10 +49,10 @@
/** \brief Configure the filter chain
* This will call configure on all filters which have been added
* as well as allocate the buffers*/
- bool configure(unsigned int size, TiXmlDocument& doc)
+ bool configure(unsigned int size, TiXmlElement* config_arg)
{
/*************************** Parse the XML ***********************************/
- TiXmlElement *config = doc.RootElement();
+ TiXmlElement *config = config_arg;
//Verify incoming xml for proper naming and structure
if (!config)
@@ -109,7 +109,7 @@
bool result = true;
- config = doc.RootElement();
+ config = config_arg;
//Step into the filter list if necessary
if (config->ValueStr() == "filters")
Modified: pkg/trunk/prcore/filters/test/test_chain.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_chain.cpp 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/prcore/filters/test/test_chain.cpp 2009-02-24 23:34:41 UTC (rev 11693)
@@ -78,7 +78,8 @@
// EXPECT_TRUE(chain.add(median_filter_5));
TiXmlDocument chain_def = TiXmlDocument();
chain_def.Parse(median_filter_5.c_str());
- EXPECT_TRUE(chain.configure(5, chain_def));
+ TiXmlElement * config = chain_def.RootElement();
+ EXPECT_TRUE(chain.configure(5, config));
float input1[] = {1,2,3,4,5};
float input1a[] = {9,9,9,9,9};//seed w/incorrect values
@@ -104,7 +105,8 @@
//EXPECT_TRUE(chain.add(median_filter_5));
TiXmlDocument chain_def = TiXmlDocument();
chain_def.Parse(median_filter_5.c_str());
- EXPECT_TRUE(chain.configure(10, chain_def));
+ TiXmlElement * config = chain_def.RootElement();
+ EXPECT_TRUE(chain.configure(10, config));
// EXPECT_TRUE(chain.configure(10));
@@ -127,8 +129,10 @@
TiXmlDocument chain_def = TiXmlDocument();
chain_def.Parse(bad_xml.c_str());
- EXPECT_FALSE(chain.configure(5, chain_def));
+ TiXmlElement * config = chain_def.RootElement();
+ EXPECT_FALSE(chain.configure(5, config));
+
}
Modified: pkg/trunk/util/laser_scan/CMakeLists.txt
===================================================================
--- pkg/trunk/util/laser_scan/CMakeLists.txt 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/util/laser_scan/CMakeLists.txt 2009-02-24 23:34:41 UTC (rev 11693)
@@ -8,11 +8,8 @@
rospack_add_library(laser_scan src/laser_scan.cc )
rospack_link_boost(laser_scan thread)
-rospack_add_library(laser_median_filter src/median_filter.cpp )
-rospack_link_boost(laser_median_filter thread)
rospack_add_executable(median_node src/median_filter_node.cpp)
-target_link_libraries(median_node laser_median_filter)
rospack_add_executable(scan_shadows_filter_node src/scan_shadows_filter.cpp)
target_link_libraries (scan_shadows_filter_node laser_scan)
Modified: pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
===================================================================
--- pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-24 23:34:41 UTC (rev 11693)
@@ -35,6 +35,7 @@
#include <sstream>
#include "boost/thread/mutex.hpp"
+#include "boost/scoped_ptr.hpp"
#include "laser_scan/LaserScan.h"
#include "filters/median.h"
@@ -45,16 +46,17 @@
namespace laser_scan{
/** \brief A class to provide median filtering of laser scans */
-class LaserMedianFilter
+template <typename T>
+class LaserMedianFilter : public filters::FilterBase<T>
{
public:
/** \brief Constructor
* \param averaging_length How many scans to average over.
*/
- LaserMedianFilter();//const std::string & xml_parameters);
+ LaserMedianFilter();
~LaserMedianFilter();
- bool configure(const std::string & xml);
+ bool configure(unsigned int number_of_channels, TiXmlElement *config); //const std::string & xml);
/** \brief Update the filter and get the response
* \param scan_in The new scan to filter
@@ -73,9 +75,77 @@
filters::FilterChain<std_vector_float > * range_filter_;
filters::FilterChain<std_vector_float > * intensity_filter_;
- std::string latest_xml_;
+ boost::scoped_ptr<TiXmlElement> latest_xml_;
};
+typedef laser_scan::LaserScan laser_scan_laser_scan;
+ROS_REGISTER_FILTER(LaserMedianFilter, laser_scan_laser_scan);
+
+template <typename T>
+LaserMedianFilter<T>::LaserMedianFilter():
+ num_ranges_(1)
+{
+
+};
+
+template <typename T>
+bool LaserMedianFilter<T>::configure(unsigned int number_of_channels, TiXmlElement * xml_doc)
+{
+ ROS_ASSERT(number_of_channels == 1);
+ latest_xml_.reset( xml_doc->Clone()->ToElement());
+
+ range_filter_ = new filters::FilterChain<std::vector<float> >();
+ range_filter_->configure(num_ranges_, latest_xml_.get());
+
+ intensity_filter_ = new filters::FilterChain<std::vector<float> >();
+ intensity_filter_->configure(num_ranges_, latest_xml_.get());
+
+ return true;
+};
+
+template <typename T>
+LaserMedianFilter<T>::~LaserMedianFilter()
+{
+ delete range_filter_;
+ delete intensity_filter_;
+};
+
+template <typename T>
+bool LaserMedianFilter<T>::update(const laser_scan::LaserScan& scan_in, laser_scan::LaserScan& scan_out)
+{
+ boost::mutex::scoped_lock lock(data_lock);
+ scan_out = scan_in; ///Quickly pass through all data \todo don't copy data too
+
+ if (scan_in.get_ranges_size() != num_ranges_) //Reallocating
+ {
+ ROS_INFO("Laser filter clearning and reallocating due to larger scan size");
+ delete range_filter_;
+ delete intensity_filter_;
+
+
+ num_ranges_ = scan_in.get_ranges_size();
+
+
+ range_filter_ = new filters::FilterChain<std::vector<float> >();
+ range_filter_->configure(num_ranges_, latest_xml_.get());
+
+ intensity_filter_ = new filters::FilterChain<std::vector<float> >();
+ intensity_filter_->configure(num_ranges_, latest_xml_.get());
+
+ }
+
+ /** \todo check for length of intensities too */
+ range_filter_->update(scan_in.ranges, scan_out.ranges);
+ intensity_filter_->update(scan_in.intensities, scan_out.intensities);
+
+
+ return true;
}
+
+
+
+}
+
+
#endif //LASER_SCAN_UTILS_LASERSCAN_H
Deleted: pkg/trunk/util/laser_scan/src/median_filter.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/median_filter.cpp 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/util/laser_scan/src/median_filter.cpp 2009-02-24 23:34:41 UTC (rev 11693)
@@ -1,99 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include "laser_scan/median_filter.h"
-#include <algorithm>
-
-using namespace filters ;
-
-
-namespace laser_scan{
-
-
-LaserMedianFilter::LaserMedianFilter():
- num_ranges_(1)
-{
-
-};
-
-bool LaserMedianFilter::configure(const std::string & xml_parameters)
-{
- latest_xml_ = xml_parameters;
- TiXmlDocument xml_doc;
- xml_doc.Parse(latest_xml_.c_str());
-
- range_filter_ = new FilterChain<std_vector_float >();
- range_filter_->configure(num_ranges_, xml_doc);
-
- intensity_filter_ = new FilterChain<std_vector_float >();
- intensity_filter_->configure(num_ranges_, xml_doc);
-
- return true;
-};
-
-LaserMedianFilter::~LaserMedianFilter()
-{
- delete range_filter_;
- delete intensity_filter_;
-};
-
-bool LaserMedianFilter::update(const laser_scan::LaserScan& scan_in, laser_scan::LaserScan& scan_out)
-{
- boost::mutex::scoped_lock lock(data_lock);
- scan_out = scan_in; ///Quickly pass through all data \todo don't copy data too
-
- if (scan_in.get_ranges_size() != num_ranges_) //Reallocating
- {
- ROS_INFO("Laser filter clearning and reallocating due to larger scan size");
- delete range_filter_;
- delete intensity_filter_;
-
-
- num_ranges_ = scan_in.get_ranges_size();
-
- TiXmlDocument xml_doc;
- xml_doc.Parse(latest_xml_.c_str());
-
- range_filter_ = new FilterChain<std_vector_float >();
- range_filter_->configure(num_ranges_, xml_doc);
-
- intensity_filter_ = new FilterChain<std_vector_float >();
- intensity_filter_->configure(num_ranges_, xml_doc);
-
- }
-
- /** \todo check for length of intensities too */
- range_filter_->update(scan_in.ranges, scan_out.ranges);
- intensity_filter_->update(scan_in.intensities, scan_out.intensities);
-
-
- return true;
-}
-
-}
Modified: pkg/trunk/util/laser_scan/src/median_filter_node.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/median_filter_node.cpp 2009-02-24 23:31:31 UTC (rev 11692)
+++ pkg/trunk/util/laser_scan/src/median_filter_node.cpp 2009-02-24 23:34:41 UTC (rev 11693)
@@ -46,7 +46,11 @@
node_.advertise<laser_scan::LaserScan>("~output", 1000);
node_.param("~filters", filter_xml, median_filter_xml);
printf("Got ~filters as: %s\n", filter_xml.c_str());
- filter.configure(filter_xml);
+ TiXmlDocument xml_doc;
+ xml_doc.Parse(filter_xml.c_str());
+ TiXmlElement * config = xml_doc.RootElement();
+
+ filter.configure(1, config);
node_.subscribe("scan_in", msg, &MedianFilterNode::callback,this, 3);
}
void callback()
@@ -56,7 +60,7 @@
}
protected:
- laser_scan::LaserMedianFilter filter;
+ laser_scan::LaserMedianFilter<laser_scan::LaserScan> filter;
ros::Node& node_;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-02-24 23:56:13
|
Revision: 11699
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11699&view=rev
Author: stuglaser
Date: 2009-02-24 23:56:09 +0000 (Tue, 24 Feb 2009)
Log Message:
-----------
Added service for atomic replacement of controllers.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
Added Paths:
-----------
pkg/trunk/prcore/robot_srvs/srv/KillAndSpawnControllers.srv
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-02-24 23:54:11 UTC (rev 11698)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-02-24 23:56:09 UTC (rev 11699)
@@ -56,6 +56,7 @@
#include <robot_srvs/ListControllers.h>
#include <robot_srvs/SpawnController.h>
#include <robot_srvs/KillController.h>
+#include <robot_srvs/KillAndSpawnControllers.h>
#include <robot_srvs/SwitchController.h>
#include <robot_msgs/MechanismState.h>
#include <robot_msgs/DiagnosticMessage.h>
@@ -161,6 +162,8 @@
robot_srvs::ListControllers::Response &resp);
bool spawnController(robot_srvs::SpawnController::Request &req,
robot_srvs::SpawnController::Response &resp);
+ bool killAndSpawnControllers(robot_srvs::KillAndSpawnControllers::Request &req,
+ robot_srvs::KillAndSpawnControllers::Response &resp);
bool switchController(robot_srvs::SwitchController::Request &req,
robot_srvs::SwitchController::Response &resp);
@@ -184,7 +187,8 @@
realtime_tools::RealtimePublisher<tf::tfMessage> transform_publisher_;
AdvertisedServiceGuard list_controllers_guard_, list_controller_types_guard_,
- spawn_controller_guard_, kill_controller_guard_, switch_controller_guard_;
+ spawn_controller_guard_, kill_controller_guard_, switch_controller_guard_,
+ kill_and_spawn_controllers_guard_;
};
#endif /* MECHANISM_CONTROL_H */
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-02-24 23:54:11 UTC (rev 11698)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-02-24 23:56:09 UTC (rev 11699)
@@ -465,6 +465,8 @@
kill_controller_guard_.set("kill_controller");
node_->advertiseService("switch_controller", &MechanismControlNode::switchController, this);
switch_controller_guard_.set("switch_controller");
+ node_->advertiseService("kill_and_spawn_controllers", &MechanismControlNode::killAndSpawnControllers, this);
+ kill_and_spawn_controllers_guard_.set("kill_and_spawn_controllers");
return true;
}
@@ -653,6 +655,80 @@
return true;
}
+bool MechanismControlNode::killAndSpawnControllers(
+ robot_srvs::KillAndSpawnControllers::Request &req,
+ robot_srvs::KillAndSpawnControllers::Response &resp)
+{
+ std::vector<MechanismControl::AddReq> add_reqs;
+ std::vector<MechanismControl::RemoveReq> remove_reqs;
+
+ TiXmlDocument doc;
+ doc.Parse(req.add_config.c_str());
+
+ TiXmlElement *config = doc.RootElement();
+ if (!config)
+ {
+ ROS_ERROR("The XML given to SpawnController could not be parsed");
+ return false;
+ }
+ if (config->ValueStr() != "controllers" &&
+ config->ValueStr() != "controller")
+ {
+ ROS_ERROR("The XML given to SpawnController must have either \"controller\" or \
+\"controllers\" as the root tag");
+ return false;
+ }
+
+ if (config->ValueStr() == "controllers")
+ {
+ config = config->FirstChildElement("controller");
+ }
+
+ for (; config; config = config->NextSiblingElement("controller"))
+ {
+ bool ok = true;
+
+ if (!config->Attribute("type"))
+ {
+ ROS_ERROR("Could not spawn a controller because no type was given");
+ ok = false;
+ }
+ else if (!config->Attribute("name"))
+ {
+ ROS_ERROR("Could not spawn a controller because no name was given");
+ ok = false;
+ }
+ else
+ {
+ int last = add_reqs.size();
+ add_reqs.resize(last + 1);
+ add_reqs[last].type = config->Attribute("type");
+ add_reqs[last].name = config->Attribute("name");
+ add_reqs[last].config = config;
+ }
+ }
+
+ remove_reqs.resize(req.kill_name.size());
+ for (size_t i = 0; i < req.kill_name.size(); ++i)
+ remove_reqs[i].name = req.kill_name[i];
+
+ mc_->changeControllers(remove_reqs, add_reqs);
+
+ resp.add_ok.resize(add_reqs.size());
+ resp.add_name.resize(add_reqs.size());
+ for (size_t i = 0; i < add_reqs.size(); ++i)
+ {
+ resp.add_name[i] = add_reqs[i].name;
+ resp.add_ok[i] = add_reqs[i].success;
+ }
+
+ resp.kill_ok.resize(remove_reqs.size());
+ for (size_t i = 0; i < remove_reqs.size(); ++i)
+ resp.kill_ok[i] = remove_reqs[i].success;
+
+ return true;
+}
+
bool MechanismControlNode::switchController(
robot_srvs::SwitchController::Request &req,
robot_srvs::SwitchController::Response &resp)
Added: pkg/trunk/prcore/robot_srvs/srv/KillAndSpawnControllers.srv
===================================================================
--- pkg/trunk/prcore/robot_srvs/srv/KillAndSpawnControllers.srv (rev 0)
+++ pkg/trunk/prcore/robot_srvs/srv/KillAndSpawnControllers.srv 2009-02-24 23:56:09 UTC (rev 11699)
@@ -0,0 +1,6 @@
+string add_config
+string[] kill_name
+---
+string[] add_name
+uint8[] add_ok
+uint8[] kill_ok
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-02-26 00:23:12
|
Revision: 11764
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11764&view=rev
Author: stuglaser
Date: 2009-02-25 22:11:14 +0000 (Wed, 25 Feb 2009)
Log Message:
-----------
Changed xacro to report file include errors better.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/prcore/xacro/xacro.py
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-02-25 21:59:11 UTC (rev 11763)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-02-25 22:11:14 UTC (rev 11764)
@@ -365,7 +365,7 @@
ROS_ERROR("Could spawn controller %s because controller type %s does not exist",
add_reqs[i].name.c_str(), add_reqs[i].type.c_str());
continue;
- }
+ }MONOTONIC
timespec init_start, init_end;
clock_gettime(CLOCK_REALTIME, &init_start);
bool initialized = c->initXmlRequest(state_, add_reqs[i].config, add_reqs[i].name);
@@ -403,11 +403,12 @@
}
// Success! Swaps in the new set of controllers.
+ int former_current_controllers_list_ = current_controllers_list_;
current_controllers_list_ = free_controllers_list;
clock_gettime(CLOCK_REALTIME, &end_time);
// Destroys the old controllers list when the realtime thread is finished with it.
- while (used_by_realtime_ != current_controllers_list_)
+ while (used_by_realtime_ == former_current_controllers_list_)
usleep(200);
from.clear();
Modified: pkg/trunk/prcore/xacro/xacro.py
===================================================================
--- pkg/trunk/prcore/xacro/xacro.py 2009-02-25 21:59:11 UTC (rev 11763)
+++ pkg/trunk/prcore/xacro/xacro.py 2009-02-25 22:11:14 UTC (rev 11764)
@@ -157,8 +157,12 @@
filename = os.path.join(base_dir, filename)
f = None
try:
- f = open(filename)
try:
+ f = open(filename)
+ except IOError, e:
+ print elt
+ raise XacroException("included file \"%s\" could not be opened: %s" % (filename, str(e)))
+ try:
included = parse(f)
except Exception, e:
raise XacroException("included file [%s] generated an error during XML parsing: %s"%(filename, str(e)))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-02-26 00:51:01
|
Revision: 11782
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11782&view=rev
Author: hsujohnhsu
Date: 2009-02-26 00:50:54 +0000 (Thu, 26 Feb 2009)
Log Message:
-----------
* changed mass settings from Geom to Body in Gazebo.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2009-02-26 00:28:35 UTC (rev 11781)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2009-02-26 00:50:54 UTC (rev 11782)
@@ -185,32 +185,6 @@
/// Pointer to the stereo data
public: StereoCameraData *data;
};
-Index: server/physics/SphereGeom.cc
-===================================================================
---- server/physics/SphereGeom.cc (revision 7168)
-+++ server/physics/SphereGeom.cc (working copy)
-@@ -66,11 +66,18 @@
- this->radiusP->SetValue( radius );
-
- // Initialize box mass matrix
-- dMassSetSphereTotal(&this->mass, this->massP->GetValue(),
-- this->radiusP->GetValue());
-+ this->SetGeom( dCreateSphere(0, this->radiusP->GetValue()), true);
-
- // Create the sphere geometry
-- this->SetGeom( dCreateSphere(0, this->radiusP->GetValue()), true);
-+ if (this->customMassMatrix)
-+ dMassSetParameters(&this->mass, this->massP->GetValue(),
-+ this->cx, this->cy, this->cz,
-+ this->ixx,this->iyy,this->izz,
-+ this->ixy,this->ixz,this->iyz);
-+ else
-+ dMassSetSphereTotal(&this->mass, this->massP->GetValue(), this->radiusP->GetValue());
-+
-+
- }
-
- ////////////////////////////////////////////////////////////////////////////////
Index: server/physics/MapGeom.hh
===================================================================
--- server/physics/MapGeom.hh (revision 7168)
@@ -237,42 +211,6 @@
private: QuadNode *root;
-Index: server/physics/BoxGeom.cc
-===================================================================
---- server/physics/BoxGeom.cc (revision 7168)
-+++ server/physics/BoxGeom.cc (working copy)
-@@ -66,9 +66,28 @@
- this->sizeP->SetValue( size );
-
- // Initialize box mass matrix
-- dMassSetBoxTotal(&this->mass, this->massP->GetValue(),
-- this->sizeP->GetValue().x, this->sizeP->GetValue().y,
-- this->sizeP->GetValue().z);
-+ // set mass matrix if user provides some info
-+ // pending a tag <massMatrix>true</massMatrix> in geom:
-+ //
-+ //
-+ // cx,cy,cz passed to ode is actually
-+ // com described by the model subtracted by the position of the collision geometry cg
-+ //
-+ //
-+ if (this->customMassMatrix)
-+ std::cout << " BoxGeom name: " << this->GetName()
-+ << " cxyz " << cx << "," << cy << "," << cz
-+ << std::endl;
-+ if (this->customMassMatrix)
-+ dMassSetParameters(&this->mass, this->massP->GetValue(),
-+ this->cx, this->cy, this->cz,
-+ this->ixx,this->iyy,this->izz,
-+ this->ixy,this->ixz,this->iyz);
-+ else
-+ // Initialize box mass matrix
-+ dMassSetBoxTotal(&this->mass, this->massP->GetValue(),
-+ this->sizeP->GetValue().x, this->sizeP->GetValue().y,
-+ this->sizeP->GetValue().z);
-
-
- // Create a box geometry with box mass matrix
Index: server/physics/Geom.hh
===================================================================
--- server/physics/Geom.hh (revision 7168)
@@ -295,27 +233,6 @@
/// The body this geom belongs to
protected: Body *body;
-@@ -188,6 +192,20 @@
- /// Mass as a double
- protected: ParamT<double> *massP;
-
-+ /// User specified Mass Matrix
-+ protected: ParamT<bool> *customMassMatrixP;
-+ protected: ParamT<double> *cxP ;
-+ protected: ParamT<double> *cyP ;
-+ protected: ParamT<double> *czP ;
-+ protected: ParamT<double> *ixxP;
-+ protected: ParamT<double> *iyyP;
-+ protected: ParamT<double> *izzP;
-+ protected: ParamT<double> *ixyP;
-+ protected: ParamT<double> *ixzP;
-+ protected: ParamT<double> *iyzP;
-+ protected: bool customMassMatrix;
-+ protected: double cx,cy,cz,ixx,iyy,izz,ixy,ixz,iyz;
-+
- private: ParamT<Vector3> *xyzP;
- private: ParamT<Quatern> *rpyP;
-
Index: server/physics/PlaneGeom.cc
===================================================================
--- server/physics/PlaneGeom.cc (revision 7168)
@@ -432,43 +349,30 @@
/// \brief Set the linear velocity of the body
public: void SetLinearVel(const Vector3 &vel);
-@@ -184,6 +200,8 @@
+@@ -184,6 +200,23 @@
private: ParamT<Vector3> *xyzP;
private: ParamT<Quatern> *rpyP;
+ private: ParamT<bool> *turnGravityOffP;
+ private: ParamT<bool> *selfCollideP;
++
++ /// User specified Mass Matrix
++ protected: ParamT<bool> *customMassMatrixP;
++ protected: ParamT<double> *cxP ;
++ protected: ParamT<double> *cyP ;
++ protected: ParamT<double> *czP ;
++ protected: ParamT<double> *bodyMassP;
++ protected: ParamT<double> *ixxP;
++ protected: ParamT<double> *iyyP;
++ protected: ParamT<double> *izzP;
++ protected: ParamT<double> *ixyP;
++ protected: ParamT<double> *ixzP;
++ protected: ParamT<double> *iyzP;
++ protected: bool customMassMatrix;
++ protected: double cx,cy,cz,bodyMass,ixx,iyy,izz,ixy,ixz,iyz;
};
/// \}
-Index: server/physics/CylinderGeom.cc
-===================================================================
---- server/physics/CylinderGeom.cc (revision 7168)
-+++ server/physics/CylinderGeom.cc (working copy)
-@@ -64,11 +64,21 @@
- this->sizeP->SetValue( size );
-
- // Initialize mass matrix
-- dMassSetCylinderTotal(&this->mass, this->massP->GetValue(), 3,
-- this->sizeP->GetValue().x, this->sizeP->GetValue().y);
-+ // pending a tag <massMatrix>true</massMatrix> in geom:
-+ if (this->customMassMatrix)
-+ dMassSetParameters(&this->mass, this->massP->GetValue(),
-+ this->cx, this->cy, this->cz,
-+ this->ixx,this->iyy,this->izz,
-+ this->ixy,this->ixz,this->iyz);
-+ else
-+ // Initialize mass matrix
-+ dMassSetCylinderTotal(&this->mass, this->massP->GetValue(), 3,
-+ this->sizeP->GetValue().x, this->sizeP->GetValue().y);
-
- this->SetGeom( dCreateCylinder( 0, this->sizeP->GetValue().x,
- this->sizeP->GetValue().y ), true );
-+
-+
- }
-
- //////////////////////////////////////////////////////////////////////////////
Index: server/physics/MapGeom.cc
===================================================================
--- server/physics/MapGeom.cc (revision 7168)
@@ -600,26 +504,7 @@
using namespace gazebo;
-@@ -73,6 +74,18 @@
-
- this->laserFiducialIdP = new ParamT<int>("laserFiducialId",-1,0);
- this->laserRetroP = new ParamT<float>("laserRetro",-1,0);
-+
-+ this->customMassMatrixP = new ParamT<bool>("massMatrix",false,0);
-+ this->cxP = new ParamT<double>("cx",0.0,0);
-+ this->cyP = new ParamT<double>("cy",0.0,0);
-+ this->czP = new ParamT<double>("cz",0.0,0);
-+ this->ixxP = new ParamT<double>("ixx",1e-6,0);
-+ this->iyyP = new ParamT<double>("iyy",1e-6,0);
-+ this->izzP = new ParamT<double>("izz",1e-6,0);
-+ this->ixyP = new ParamT<double>("ixy",0.0,0);
-+ this->ixzP = new ParamT<double>("ixz",0.0,0);
-+ this->iyzP = new ParamT<double>("iyz",0.0,0);
-+
- Param::End();
- }
-
-@@ -88,17 +101,30 @@
+@@ -88,11 +89,14 @@
if (this->transId)
dGeomDestroy(this->transId);
@@ -637,23 +522,7 @@
delete this->massP;
delete this->xyzP;
- delete this->rpyP;
- delete this->laserFiducialIdP;
- delete this->laserRetroP;
-+ delete this->customMassMatrixP;
-+ delete this->cxP ;
-+ delete this->cyP ;
-+ delete this->czP ;
-+ delete this->ixxP;
-+ delete this->iyyP;
-+ delete this->izzP;
-+ delete this->ixyP;
-+ delete this->ixzP;
-+ delete this->iyzP;
- }
-
- ////////////////////////////////////////////////////////////////////////////////
-@@ -112,6 +138,10 @@
+@@ -112,6 +116,10 @@
this->typeName = node->GetName();
this->nameP->Load(node);
@@ -664,72 +533,8 @@
this->massP->Load(node);
this->xyzP->Load(node);
this->rpyP->Load(node);
-@@ -123,6 +153,63 @@
- this->massP->SetValue( 0.001 );
- }
+@@ -136,13 +144,17 @@
-+ this->customMassMatrixP->Load(node);
-+ this->cxP ->Load(node);
-+ this->cyP ->Load(node);
-+ this->czP ->Load(node);
-+ this->ixxP->Load(node);
-+ this->iyyP->Load(node);
-+ this->izzP->Load(node);
-+ this->ixyP->Load(node);
-+ this->ixzP->Load(node);
-+ this->iyzP->Load(node);
-+
-+ // option to enter full maxx matrix
-+ this->customMassMatrix = this->customMassMatrixP->GetValue();
-+ this->cx = this->cxP ->GetValue();
-+ this->cy = this->cyP ->GetValue();
-+ this->cz = this->czP ->GetValue();
-+ this->ixx = this->ixxP->GetValue();
-+ this->iyy = this->iyyP->GetValue();
-+ this->izz = this->izzP->GetValue();
-+ this->ixy = this->ixyP->GetValue();
-+ this->ixz = this->ixzP->GetValue();
-+ this->iyz = this->iyzP->GetValue();
-+
-+ // setup this->mass as well
-+ // this->mass.c[0] = this->cx;
-+ // this->mass.c[1] = this->cy;
-+ // this->mass.c[2] = this->cz;
-+
-+ // this->mass.I[0] = this->ixx;
-+ // this->mass.I[1] = this->ixy;
-+ // this->mass.I[2] = this->ixz;
-+
-+ // this->mass.I[3] = this->ixy;
-+ // this->mass.I[4] = this->iyy;
-+ // this->mass.I[5] = this->iyz;
-+
-+ // this->mass.I[6] = this->ixz;
-+ // this->mass.I[7] = this->iyz;
-+ // this->mass.I[8] = this->izz;
-+
-+ // this->mass.I[9] = 1;
-+ // this->mass.I[10] = 1;
-+ // this->mass.I[11] = 1;
-+
-+ // std::cout << " c[0] " << this->mass.c[0] << std::endl;
-+ // std::cout << " c[1] " << this->mass.c[1] << std::endl;
-+ // std::cout << " c[2] " << this->mass.c[2] << std::endl;
-+ // std::cout << " I[0] " << this->mass.I[0] << std::endl;
-+ // std::cout << " I[1] " << this->mass.I[1] << std::endl;
-+ // std::cout << " I[2] " << this->mass.I[2] << std::endl;
-+ // std::cout << " I[3] " << this->mass.I[3] << std::endl;
-+ // std::cout << " I[4] " << this->mass.I[4] << std::endl;
-+ // std::cout << " I[5] " << this->mass.I[5] << std::endl;
-+ // std::cout << " I[6] " << this->mass.I[6] << std::endl;
-+ // std::cout << " I[7] " << this->mass.I[7] << std::endl;
-+ // std::cout << " I[8] " << this->mass.I[8] << std::endl;
-+
- this->contact->Load(node);
-
- this->LoadChild(node);
-@@ -136,13 +223,17 @@
-
// TODO: This should probably be true....but "true" breaks trimesh postions.
this->SetPose(pose, true);
- childNode = node->GetChild("visual");
@@ -752,7 +557,7 @@
}
/*if (this->IsStatic())
-@@ -159,8 +250,11 @@
+@@ -159,8 +171,11 @@
Vector3 min(aabb[0], aabb[2], aabb[4]);
Vector3 max(aabb[1], aabb[3], aabb[5]);
@@ -766,7 +571,7 @@
}
if (this->geomId && dGeomGetClass(this->geomId) != dPlaneClass &&
-@@ -200,10 +294,11 @@
+@@ -200,10 +215,11 @@
stream << prefix << " " << *(this->laserFiducialIdP) << "\n";
stream << prefix << " " << *(this->laserRetroP) << "\n";
@@ -782,7 +587,7 @@
stream << prefix << "</geom:" << this->typeName << ">\n";
}
-@@ -297,6 +392,11 @@
+@@ -297,6 +313,11 @@
// Transform into CoM relative Pose
localPose = pose - this->body->GetCoMPose();
@@ -794,7 +599,7 @@
q[0] = localPose.rot.u;
q[1] = localPose.rot.x;
q[2] = localPose.rot.y;
-@@ -307,11 +407,13 @@
+@@ -307,11 +328,13 @@
dGeomSetPosition(this->geomId, localPose.pos.x, localPose.pos.y, localPose.pos.z);
dGeomSetQuaternion(this->geomId, q);
@@ -809,7 +614,7 @@
}
}
}
-@@ -396,23 +498,24 @@
+@@ -396,23 +419,24 @@
Pose3d pose;
dQuaternion q;
dMatrix3 r;
@@ -838,7 +643,7 @@
if (dMassCheck(&this->bodyMass))
{
dMassRotate(&this->bodyMass, r);
-@@ -454,8 +557,9 @@
+@@ -454,8 +478,9 @@
/// Set the visibility of the Bounding box of this geometry
void Geom::ShowBoundingBox(bool show)
{
@@ -850,7 +655,7 @@
}
// FIXME: ShowJoints and ShowPhysics will mess with each other and with the user's defined transparency visibility
-@@ -465,20 +569,21 @@
+@@ -465,20 +490,21 @@
{
std::vector<OgreVisual*>::iterator iter;
@@ -882,7 +687,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -487,26 +592,27 @@
+@@ -487,26 +513,27 @@
{
std::vector<OgreVisual*>::iterator iter;
@@ -926,7 +731,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -521,15 +627,21 @@
+@@ -521,15 +548,21 @@
/// Get the number of visuals
unsigned int Geom::GetVisualCount() const
{
@@ -950,7 +755,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -538,11 +650,12 @@
+@@ -538,11 +571,12 @@
{
std::vector<OgreVisual*>::const_iterator iter;
@@ -968,61 +773,6 @@
return NULL;
}
-Index: server/physics/Joint.hh
-===================================================================
---- server/physics/Joint.hh (revision 7168)
-+++ server/physics/Joint.hh (working copy)
-@@ -156,6 +156,8 @@
- /// Name of this joint
- private: ParamT<double> *erpP;
- private: ParamT<double> *cfmP;
-+ private: ParamT<double> *stopKpP;
-+ private: ParamT<double> *stopKdP;
- private: ParamT<std::string> *body1NameP;
- private: ParamT<std::string> *body2NameP;
- private: ParamT<std::string> *anchorBodyNameP;
-@@ -163,6 +165,41 @@
- private: ParamT<bool> *provideFeedbackP;
- private: ParamT<double> *fudgeFactorP;
-
-+ /// Added for mimicing other joints
-+ private: ParamT<std::string> *mimicJointP;
-+ private: ParamT<double> *mimicMultP;
-+ private: ParamT<double> *mimicOffsetP;
-+ private: ParamT<double> *mimicKpP;
-+ private: ParamT<double> *mimicKdP;
-+ private: ParamT<double> *mimicFMaxP;
-+
-+ private: Joint *mimicJoint;
-+ private: bool enableMimic;
-+ private: double mimicMult;
-+ private: double mimicOffset;
-+ private: double mimicKp;
-+ private: double mimicKd;
-+ private: double mimicFMax;
-+ private: double current_time_mimic, last_time_mimic;
-+ private: double current_error_mimic, last_error_mimic;
-+
-+ private: ParamT<std::string> *latchJointP;
-+ private: ParamT<double> *latchAngleP;
-+ private: ParamT<double> *doorClosedAngleP;
-+ private: ParamT<double> *latchKpP;
-+ private: ParamT<double> *latchKdP;
-+ private: ParamT<double> *latchFMaxP;
-+
-+ private: Joint* latchJoint;
-+ private: bool enableLatch;
-+ private: double latchAngle;
-+ private: double doorClosedAngle;
-+ private: double latchKp;
-+ private: double latchKd;
-+ private: double latchFMax;
-+ private: double current_time_latch, last_time_latch;
-+ private: double current_error_latch, last_error_latch;
-+
- /// Feedback data for this joint
- private: dJointFeedback *feedback;
-
Index: server/physics/Body.cc
===================================================================
--- server/physics/Body.cc (revision 7168)
@@ -1043,25 +793,49 @@
using namespace gazebo;
////////////////////////////////////////////////////////////////////////////////
-@@ -70,6 +78,8 @@
+@@ -70,6 +78,21 @@
this->rpyP = new ParamT<Quatern>("rpy", Quatern(), 0);
this->rpyP->Callback( &Body::SetRotation, this );
+ this->turnGravityOffP = new ParamT<bool>("turnGravityOff", false, 0);
+ this->selfCollideP = new ParamT<bool>("selfCollide", false, 0);
++
++ this->customMassMatrixP = new ParamT<bool>("massMatrix",false,0);
++ this->cxP = new ParamT<double>("cx",0.0,0);
++ this->cyP = new ParamT<double>("cy",0.0,0);
++ this->czP = new ParamT<double>("cz",0.0,0);
++ this->bodyMassP = new ParamT<double>("mass",0.001,0);
++ this->ixxP = new ParamT<double>("ixx",1e-6,0);
++ this->iyyP = new ParamT<double>("iyy",1e-6,0);
++ this->izzP = new ParamT<double>("izz",1e-6,0);
++ this->ixyP = new ParamT<double>("ixy",0.0,0);
++ this->ixzP = new ParamT<double>("ixz",0.0,0);
++ this->iyzP = new ParamT<double>("iyz",0.0,0);
++
Param::End();
}
-@@ -95,6 +105,8 @@
+@@ -95,18 +118,73 @@
delete this->xyzP;
delete this->rpyP;
+ delete this->turnGravityOffP;
+ delete this->selfCollideP;
++ delete this->customMassMatrixP;
++ delete this->cxP ;
++ delete this->cyP ;
++ delete this->czP ;
++ delete this->bodyMassP;
++ delete this->ixxP;
++ delete this->iyyP;
++ delete this->izzP;
++ delete this->ixyP;
++ delete this->ixzP;
++ delete this->iyzP;
}
-@@ -102,11 +114,27 @@
+ ////////////////////////////////////////////////////////////////////////////////
// Load the body based on an XMLConfig node
void Body::Load(XMLConfigNode *node)
{
@@ -1078,6 +852,32 @@
+ }
+
+
++
++ this->customMassMatrixP->Load(node);
++ this->cxP ->Load(node);
++ this->cyP ->Load(node);
++ this->czP ->Load(node);
++ this->bodyMassP->Load(node);
++ this->ixxP->Load(node);
++ this->iyyP->Load(node);
++ this->izzP->Load(node);
++ this->ixyP->Load(node);
++ this->ixzP->Load(node);
++ this->iyzP->Load(node);
++
++ // option to enter full mass matrix
++ this->customMassMatrix = this->customMassMatrixP->GetValue();
++ this->cx = this->cxP ->GetValue();
++ this->cy = this->cyP ->GetValue();
++ this->cz = this->czP ->GetValue();
++ this->bodyMass = this->bodyMassP->GetValue();
++ this->ixx = this->ixxP->GetValue();
++ this->iyy = this->iyyP->GetValue();
++ this->izz = this->izzP->GetValue();
++ this->ixy = this->ixyP->GetValue();
++ this->ixz = this->ixzP->GetValue();
++ this->iyz = this->iyzP->GetValue();
++
+ // load child nodes
+
XMLConfigNode *childNode;
@@ -1089,7 +889,7 @@
Pose3d initPose;
initPose.pos = **(this->xyzP);
-@@ -134,8 +162,9 @@
+@@ -134,8 +212,9 @@
}
// If no geoms are attached, then don't let gravity affect the body.
@@ -1100,7 +900,7 @@
this->SetGravityMode(false);
}
-@@ -197,6 +226,13 @@
+@@ -197,6 +276,13 @@
}
////////////////////////////////////////////////////////////////////////////////
@@ -1114,7 +914,7 @@
// Initialize the body
void Body::Init()
{
-@@ -218,25 +254,53 @@
+@@ -218,25 +304,53 @@
std::vector< Sensor* >::iterator sensorIter;
std::map< std::string, Geom* >::iterator geomIter;
@@ -1169,7 +969,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -329,7 +393,8 @@
+@@ -329,7 +443,8 @@
dBodySetPosition(this->bodyId, pos.x, pos.y, pos.z);
// Set the position of the scene node
@@ -1179,7 +979,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -350,7 +415,8 @@
+@@ -350,7 +465,8 @@
}
// Set the orientation of the scene node
@@ -1189,7 +989,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -404,6 +470,88 @@
+@@ -404,6 +520,88 @@
}
////////////////////////////////////////////////////////////////////////////////
@@ -1278,15 +1078,69 @@
// Return the ID of this body
dBodyID Body::GetId() const
{
-@@ -506,7 +654,6 @@
+@@ -505,13 +703,57 @@
+ */
void Body::UpdateCoM()
{
++ if (!this->bodyId)
++ return;
++
++ if (this->customMassMatrix)
++ {
++
++ // comPose is zero in this case, we'll keep cx, cy, cz
++ this->comPose.Reset();
++
++ this->comPose.pos.x = this->cx;
++ this->comPose.pos.y = this->cy;
++ this->comPose.pos.z = this->cz;
++
++ // setup this->mass as well
++ dMassSetParameters(&this->mass, this->bodyMass,
++ this->cx, this->cy, this->cz,
++ //0,0,0,
++ this->ixx,this->iyy,this->izz,
++ this->ixy,this->ixz,this->iyz);
++
++ dMassTranslate( &this->mass, -this->cx, -this->cy, -this->cz);
++
++ // dMatrix3 rot;
++ // dMassRotate(&this->mass, rot);
++
++ // Set the mass matrix
++ if (this->mass.mass > 0)
++ dBodySetMass( this->bodyId, &this->mass );
++
++ // std::cout << " c[0] " << this->mass.c[0] << std::endl;
++ // std::cout << " c[1] " << this->mass.c[1] << std::endl;
++ // std::cout << " c[2] " << this->mass.c[2] << std::endl;
++ // std::cout << " I[0] " << this->mass.I[0] << std::endl;
++ // std::cout << " I[1] " << this->mass.I[1] << std::endl;
++ // std::cout << " I[2] " << this->mass.I[2] << std::endl;
++ // std::cout << " I[3] " << this->mass.I[3] << std::endl;
++ // std::cout << " I[4] " << this->mass.I[4] << std::endl;
++ // std::cout << " I[5] " << this->mass.I[5] << std::endl;
++ // std::cout << " I[6] " << this->mass.I[6] << std::endl;
++ // std::cout << " I[7] " << this->mass.I[7] << std::endl;
++ // std::cout << " I[8] " << this->mass.I[8] << std::endl;
++
++ }
++ else
++ {
++
++ // original gazebo subroutine that gathers mass from all geoms and sums into one single mass matrix
++
const dMass *lmass;
- Pose3d oldPose, newPose, pose;
std::map< std::string, Geom* >::iterator giter;
- if (!this->bodyId)
-@@ -521,12 +668,23 @@
+- if (!this->bodyId)
+- return;
+-
+ // Construct the mass matrix by combining all the geoms
+ dMassSetZero( &this->mass );
+
+@@ -521,12 +763,21 @@
if (giter->second->IsPlaceable() && giter->second->GetGeomId())
{
dMassAdd( &this->mass, lmass );
@@ -1295,8 +1149,6 @@
}
}
-+ //return; // Stop pose update, we have full com xyz, I control
-+
// Old pose for the CoM
+ Pose3d oldPose, newPose, tmpPose;
+
@@ -1310,7 +1162,7 @@
if (std::isnan(this->mass.c[0]))
this->mass.c[0] = 0;
-@@ -541,24 +699,37 @@
+@@ -541,24 +792,37 @@
newPose.pos.y = this->mass.c[1];
newPose.pos.z = this->mass.c[2];
@@ -1353,6 +1205,70 @@
// Settle on the new CoM pose
+@@ -570,6 +834,8 @@
+ // Set the mass matrix
+ if (this->mass.mass > 0)
+ dBodySetMass( this->bodyId, &this->mass );
++ }
++
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+Index: server/physics/Joint.hh
+===================================================================
+--- server/physics/Joint.hh (revision 7168)
++++ server/physics/Joint.hh (working copy)
+@@ -156,6 +156,8 @@
+ /// Name of this joint
+ private: ParamT<double> *erpP;
+ private: ParamT<double> *cfmP;
++ private: ParamT<double> *stopKpP;
++ private: ParamT<double> *stopKdP;
+ private: ParamT<std::string> *body1NameP;
+ private: ParamT<std::string> *body2NameP;
+ private: ParamT<std::string> *anchorBodyNameP;
+@@ -163,6 +165,41 @@
+ private: ParamT<bool> *provideFeedbackP;
+ private: ParamT<double> *fudgeFactorP;
+
++ /// Added for mimicing other joints
++ private: ParamT<std::string> *mimicJointP;
++ private: ParamT<double> *mimicMultP;
++ private: ParamT<double> *mimicOffsetP;
++ private: ParamT<double> *mimicKpP;
++ private: ParamT<double> *mimicKdP;
++ private: ParamT<double> *mimicFMaxP;
++
++ private: Joint *mimicJoint;
++ private: bool enableMimic;
++ private: double mimicMult;
++ private: double mimicOffset;
++ private: double mimicKp;
++ private: double mimicKd;
++ private: double mimicFMax;
++ private: double current_time_mimic, last_time_mimic;
++ private: double current_error_mimic, last_error_mimic;
++
++ private: ParamT<std::string> *latchJointP;
++ private: ParamT<double> *latchAngleP;
++ private: ParamT<double> *doorClosedAngleP;
++ private: ParamT<double> *latchKpP;
++ private: ParamT<double> *latchKdP;
++ private: ParamT<double> *latchFMaxP;
++
++ private: Joint* latchJoint;
++ private: bool enableLatch;
++ private: double latchAngle;
++ private: double doorClosedAngle;
++ private: double latchKp;
++ private: double latchKd;
++ private: double latchFMax;
++ private: double current_time_latch, last_time_latch;
++ private: double current_error_latch, last_error_latch;
++
+ /// Feedback data for this joint
+ private: dJointFeedback *feedback;
+
Index: server/physics/HingeJoint.cc
===================================================================
--- server/physics/HingeJoint.cc (revision 7168)
@@ -2027,7 +1943,7 @@
}
//////////////////////////////////////////////////////////////////////////////
-@@ -118,103 +121,115 @@
+@@ -118,103 +121,109 @@
this->meshNameP->Load(node);
this->scaleP->Load(node);
@@ -2196,13 +2112,7 @@
- memset(this->matrix_dblbuff,0,32*sizeof(dReal));
- this->last_matrix_index = 0;
-+ if (this->customMassMatrix)
-+ dMassSetParameters(&this->mass, this->massP->GetValue(),
-+ this->cx, this->cy, this->cz,
-+ this->ixx,this->iyy,this->izz,
-+ this->ixy,this->ixz,this->iyz);
-+ else
-+ dMassSetTrimesh(&this->mass, this->massP->GetValue(), this->geomId);
++ dMassSetTrimesh(&this->mass, this->massP->GetValue(), this->geomId);
+
+ // Create the trimesh geometry
+ this->SetGeom(this->geomId, true);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-02-26 00:28:35 UTC (rev 11781)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-02-26 00:50:54 UTC (rev 11782)
@@ -184,7 +184,23 @@
/* set body name */
elem->SetAttribute("name", link->name);
+
+
+ /* set mass properties */
+ addKeyValue(elem, "massMatrix", "true");
+ addKeyValue(elem, "mass", values2str(1, &link->inertial->mass));
+ static const char tagList1[6][4] = {"ixx", "ixy", "ixz", "iyy", "iyz", "izz"};
+ for (int j = 0 ; j < 6 ; ++j)
+ addKeyValue(elem, tagList1[j], values2str(1, link->inertial->inertia + j));
+
+ static const char tagList2[3][3] = {"cx", "cy", "cz"};
+ for (int j = 0 ; j < 3 ; ++j)
+ {
+ double tmp_value = (link->inertial->com)[j] - 0*(link->collision->xyz)[j];
+ addKeyValue(elem, tagList2[j], values2str(1, &tmp_value));
+ }
+
/* compute global transform */
btTransform localTransform;
setupTransform(localTransform, link->xyz, link->rpy);
@@ -205,22 +221,6 @@
addKeyValue(geom, "xyz", values2str(3, link->collision->xyz));
addKeyValue(geom, "rpy", values2str(3, link->collision->rpy, rad2deg));
- /* set mass properties */
- addKeyValue(geom, "massMatrix", "true");
- addKeyValue(geom, "mass", values2str(1, &link->inertial->mass));
-
- static const char tagList1[6][4] = {"ixx", "ixy", "ixz", "iyy", "iyz", "izz"};
- for (int j = 0 ; j < 6 ; ++j)
- addKeyValue(geom, tagList1[j], values2str(1, link->inertial->inertia + j));
-
- static const char tagList2[3][3] = {"cx", "cy", "cz"};
- for (int j = 0 ; j < 3 ; ++j)
- {
- // by doing this we support only 1 geom
- double tmp_value = (link->inertial->com)[j] - (link->collision->xyz)[j];
- addKeyValue(geom, tagList2[j], values2str(1, &tmp_value));
- }
-
if (link->collision->geometry->type == robot_desc::URDF::Link::Geometry::MESH)
{
robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->collision->geometry->shape);
@@ -347,7 +347,7 @@
for (int j = 0 ; j < 3 ; ++j)
{
// undo Gazebo's shift of object anchor to geom cg center, stay in body cs
- tmpAnchor[j] = (link->joint->anchor)[j] - (link->collision->xyz)[j];
+ tmpAnchor[j] = (link->joint->anchor)[j] - (link->inertial->com)[j] - 0*(link->collision->xyz)[j];
}
addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-02-26 01:08:15
|
Revision: 11786
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11786&view=rev
Author: tfoote
Date: 2009-02-26 01:08:09 +0000 (Thu, 26 Feb 2009)
Log Message:
-----------
refactored filters to optionally take standalone or vector types
Modified Paths:
--------------
pkg/trunk/prcore/filters/include/filters/filter_base.h
pkg/trunk/prcore/filters/include/filters/filter_chain.h
pkg/trunk/prcore/filters/include/filters/mean.h
pkg/trunk/prcore/filters/include/filters/median.h
pkg/trunk/prcore/filters/include/filters/transfer_function.h
pkg/trunk/prcore/filters/test/test_chain.cpp
pkg/trunk/prcore/filters/test/test_factory.cpp
pkg/trunk/prcore/filters/test/test_mean.cpp
pkg/trunk/prcore/filters/test/test_median.cpp
pkg/trunk/prcore/filters/test/test_transfer_function.cpp
pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
pkg/trunk/util/laser_scan/src/median_filter_node.cpp
Modified: pkg/trunk/prcore/filters/include/filters/filter_base.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/filter_base.h 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/include/filters/filter_base.h 2009-02-26 01:08:09 UTC (rev 11786)
@@ -64,8 +64,20 @@
/** \brief Update the filter and return the data seperately
+ * This is a lazy way to do this and can be overridden in the derived class
*/
- virtual bool update(const T& data_in, T& data_out)=0;
+ virtual bool update(const T& data_in, T& data_out)
+ {
+ std::vector<T> temp_in(1);
+ std::vector<T> temp_out(1);
+ temp_in[0] = data_in;
+ bool retval = update(temp_in, temp_out);
+ data_out = temp_out[0];
+ return retval;
+ };
+ /** \brief Update the filter and return the data seperately
+ */
+ virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out)=0;
std::string getType() {return typeid(T).name();};
};
@@ -85,7 +97,6 @@
filters::FilterBase<t> * Filters_New_##c##__##t() {return new c< t >;}; \
bool ROS_FILTER_## c ## _ ## t = \
filters::FilterFactory<t>::Instance().Register(filters::getFilterID<t>(std::string(#c)), Filters_New_##c##__##t);
-///\todo make this use templating to get the data type, the user doesn't ever set the data type at runtime
}
Modified: pkg/trunk/prcore/filters/include/filters/filter_chain.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/filter_chain.h 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/include/filters/filter_chain.h 2009-02-26 01:08:09 UTC (rev 11786)
@@ -164,6 +164,7 @@
/** \brief process data through each of the filters added sequentially */
bool update(const T& data_in, T& data_out);
+ bool update(const std::vector<T>& data_in, std::vector<T>& data_out);
~FilterChain()
@@ -175,8 +176,8 @@
private:
std::vector<boost::shared_ptr<filters::FilterBase<T> > > reference_pointers_;
- T buffer0_; ///<! A temporary intermediate buffer
- T buffer1_; ///<! A temporary intermediate buffer
+ std::vector<T> buffer0_; ///<! A temporary intermediate buffer
+ std::vector<T> buffer1_; ///<! A temporary intermediate buffer
bool configured_; ///<! whether the system is configured
};
@@ -184,6 +185,17 @@
template <typename T>
bool FilterChain<T>::update (const T& data_in, T& data_out)
{
+ std::vector<T> temp_in(1);
+ std::vector<T> temp_out(1);
+ temp_in[0] = data_in;
+ bool retval = update(temp_in, temp_out);
+ data_out = temp_out[0];
+
+ return retval;
+};
+template <typename T>
+bool FilterChain<T>::update (const std::vector<T>& data_in, std::vector<T>& data_out)
+{
unsigned int list_size = reference_pointers_.size();
bool result;
if (list_size == 0)
Modified: pkg/trunk/prcore/filters/include/filters/mean.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/mean.h 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/include/filters/mean.h 2009-02-26 01:08:09 UTC (rev 11786)
@@ -63,18 +63,23 @@
* \param data_out T array with length width
*/
virtual bool update( const T & data_in, T& data_out);
+ virtual bool update( const std::vector<T> & data_in, std::vector<T>& data_out);
std::string name_; //Name of the filter.
protected:
- RealtimeVectorCircularBuffer<T>* data_storage_; ///< Storage for data between updates
+ RealtimeVectorCircularBuffer<std::vector<T> >* data_storage_; ///< Storage for data between updates
uint32_t last_updated_row_; ///< The last row to have been updated by the filter
+ std::vector<T> temp; //used for preallocation and copying from non vector source
+
uint32_t number_of_observations_; ///< Number of observations over which to filter
uint32_t number_of_channels_; ///< Number of elements per observation
bool configured_;
+
+
};
@@ -112,9 +117,8 @@
number_of_observations_ = atof(p->Attribute("number_of_observations"));
number_of_channels_ = number_of_channels;
- T temp;
temp.resize(number_of_channels);
- data_storage_ = new RealtimeVectorCircularBuffer<T>(number_of_observations_, temp);
+ data_storage_ = new RealtimeVectorCircularBuffer<std::vector<T> >(number_of_observations_, temp);
configured_ = true;
return true;
@@ -132,6 +136,40 @@
{
// ROS_ASSERT(data_in.size() == width_);
//ROS_ASSERT(data_out.size() == width_);
+ if (number_of_channels_ != 1)
+ return false;
+
+ temp[0] = data_in;
+
+ //update active row
+ if (last_updated_row_ >= number_of_observations_ - 1)
+ last_updated_row_ = 0;
+ else
+ last_updated_row_++;
+
+ data_storage_->push_back(temp);
+
+
+ unsigned int length = data_storage_->size();
+
+ //Return each value
+ for (uint32_t i = 0; i < number_of_channels_; i++)
+ {
+ data_out = 0;
+ for (uint32_t row = 0; row < length; row ++)
+ {
+ data_out += data_storage_->at(row)[i];
+ }
+ data_out /= length;
+ }
+
+ return true;
+};
+template <typename T>
+bool MeanFilter<T>::update(const std::vector<T> & data_in, std::vector<T>& data_out)
+{
+ // ROS_ASSERT(data_in.size() == width_);
+ //ROS_ASSERT(data_out.size() == width_);
if (data_in.size() != number_of_channels_ || data_out.size() != number_of_channels_)
return false;
@@ -160,8 +198,8 @@
return true;
};
-ROS_REGISTER_FILTER(MeanFilter, std_vector_double)
-ROS_REGISTER_FILTER(MeanFilter, std_vector_float)
+ROS_REGISTER_FILTER(MeanFilter, double)
+ROS_REGISTER_FILTER(MeanFilter, float)
}
Modified: pkg/trunk/prcore/filters/include/filters/median.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/median.h 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/include/filters/median.h 2009-02-26 01:08:09 UTC (rev 11786)
@@ -104,14 +104,17 @@
* \param data_out double array with length width
*/
virtual bool update(const T& data_in, T& data_out);
+ virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out);
std::string name_; //Name of the filter.
protected:
- T temp_storage_; ///< Preallocated storage for the list to sort
- RealtimeVectorCircularBuffer<T>* data_storage_; ///< Storage for data between updates
+ std::vector<T> temp_storage_; ///< Preallocated storage for the list to sort
+ RealtimeVectorCircularBuffer<std::vector<T> >* data_storage_; ///< Storage for data between updates
+ std::vector<T> temp; //used for preallocation and copying from non vector source
+
uint32_t number_of_observations_; ///< Number of observations over which to filter
uint32_t number_of_channels_; ///< Number of elements per observation
@@ -145,7 +148,7 @@
const char *name = config->Attribute("name");
if (!name)
{
- fprintf(stderr, "Error: TransferFunctionFilter was not given a name.\n");
+ fprintf(stderr, "Error: MedianFilter was not given a name.\n");
return false;
}
name_ = std::string(name);
@@ -154,16 +157,15 @@
TiXmlElement *p = config->FirstChildElement("params");
if (!p)
{
- fprintf(stderr, "Error: TransferFunctionFilter was not given params.\n");
+ fprintf(stderr, "Error: MedianFilter was not given params.\n");
return false;
}
number_of_observations_ = atoi(p->Attribute("number_of_observations"));
number_of_channels_ = number_of_channels;
- T temp;
temp.resize(number_of_channels_);
- data_storage_ = new RealtimeVectorCircularBuffer<T>(number_of_observations_, temp);
+ data_storage_ = new RealtimeVectorCircularBuffer<std::vector<T> >(number_of_observations_, temp);
temp_storage_.resize(number_of_observations_);
configured_ = true;
@@ -171,7 +173,7 @@
};
template <typename T>
-bool MedianFilter<T>::update(const T& data_in, T& data_out)
+bool MedianFilter<T>::update(const std::vector<T>& data_in, std::vector<T>& data_out)
{
// printf("Expecting width %d, got %d and %d\n", width_, data_in.size(),data_out.size());
if (data_in.size() != number_of_channels_ || data_out.size() != number_of_channels_)
@@ -197,8 +199,35 @@
return true;
};
-ROS_REGISTER_FILTER(MedianFilter, std_vector_double)
-ROS_REGISTER_FILTER(MedianFilter, std_vector_float)
+template <typename T>
+bool MedianFilter<T>::update(const T& data_in, T& data_out)
+{
+ // printf("Expecting width %d, got %d and %d\n", width_, data_in.size(),data_out.size());
+ if (number_of_channels_ != 1)
+ return false;
+ if (!configured_)
+ return false;
+
+ temp[0] = data_in;
+
+ data_storage_->push_back(temp);
+ ///\todo standardize on calling to vector class
+
+ unsigned int length = data_storage_->size();
+
+
+ for (uint32_t row = 0; row < length; row ++)
+ {
+ temp_storage_[row] = (*data_storage_)[row][0];
+ }
+ data_out = median(&temp_storage_[0], length);
+
+
+ return true;
+};
+
+ROS_REGISTER_FILTER(MedianFilter, double)
+ROS_REGISTER_FILTER(MedianFilter, float)
}
Modified: pkg/trunk/prcore/filters/include/filters/transfer_function.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/transfer_function.h 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/include/filters/transfer_function.h 2009-02-26 01:08:09 UTC (rev 11786)
@@ -94,6 +94,7 @@
* \param data_out vector<T> with number_of_channels elements
*/
virtual bool update(const T & data_in, T& data_out) ;
+ virtual bool update(const std::vector<T> & data_in, std::vector<T>& data_out) ;
std::string name_; //Name of the filter.
@@ -102,9 +103,11 @@
unsigned int number_of_channels_; //The number of inputs filtered.
- RealtimeVectorCircularBuffer<T>* input_buffer_; //The input sample history.
- RealtimeVectorCircularBuffer<T>* output_buffer_; //The output sample history.
+ RealtimeVectorCircularBuffer<std::vector<T> >* input_buffer_; //The input sample history.
+ RealtimeVectorCircularBuffer<std::vector<T> >* output_buffer_; //The output sample history.
+ std::vector<T> temp; //used for storage and preallocation
+
std::vector<double> a_; //Transfer functon coefficients (output).
std::vector<double> b_; //Transfer functon coefficients (input).
@@ -112,8 +115,8 @@
};
-ROS_REGISTER_FILTER(TransferFunctionFilter, std_vector_double)
-ROS_REGISTER_FILTER(TransferFunctionFilter, std_vector_float)
+ROS_REGISTER_FILTER(TransferFunctionFilter, double)
+ROS_REGISTER_FILTER(TransferFunctionFilter, float)
template <typename T>
TransferFunctionFilter<T>::TransferFunctionFilter():
@@ -174,10 +177,9 @@
number_of_channels_ = number_of_channels;
// Create the input and output buffers of the correct size.
- T temp;
temp.resize(number_of_channels);
- input_buffer_ = new RealtimeVectorCircularBuffer<T>(b_.size()-1, temp);
- output_buffer_ = new RealtimeVectorCircularBuffer<T>(a_.size()-1, temp);
+ input_buffer_ = new RealtimeVectorCircularBuffer<std::vector<T> >(b_.size()-1, temp);
+ output_buffer_ = new RealtimeVectorCircularBuffer<std::vector<T> >(a_.size()-1, temp);
// Prevent divide by zero while normalizing coeffs.
if ( a_[0] == 0)
@@ -209,15 +211,47 @@
bool TransferFunctionFilter<T>::update(const T & data_in, T& data_out)
{
// Ensure the correct number of inputs
+ if (number_of_channels_ != 1)
+ return false;
+
+ // Copy data to prevent mutation if in and out are the same ptr
+ temp[0] = data_in;
+ std::vector<T> temp_in(1);
+ temp_in[0] = data_in;
+ std::vector<T> temp_out(1);
+
+ for (uint32_t i = 0; i < temp_in.size(); i++)
+ {
+ temp_out[i]=(b_[0]) * (temp_in[i]);
+
+ for (uint32_t row = 1; row <= input_buffer_->size(); row++)
+ {
+ (temp_out)[i] += b_[row] * (*input_buffer_)[row-1][i];
+ }
+ for (uint32_t row = 1; row <= output_buffer_->size(); row++)
+ {
+ (temp_out)[i] -= a_[row] * (*output_buffer_)[row-1][i];
+ }
+ }
+ input_buffer_->push_front(temp_in);
+ output_buffer_->push_front(temp_out);
+
+ data_out = temp_out[0];
+ return true;
+}
+template <typename T>
+bool TransferFunctionFilter<T>::update(const std::vector<T> & data_in, std::vector<T> & data_out)
+{
+ // Ensure the correct number of inputs
if (data_in.size() != number_of_channels_ || data_out.size() != number_of_channels_ )
return false;
// Copy data to prevent mutation if in and out are the same ptr
- T current_input = data_in;
+ temp = data_in;
- for (uint32_t i = 0; i < current_input.size(); i++)
+ for (uint32_t i = 0; i < temp.size(); i++)
{
- data_out[i]=b_[0] * current_input[i];
+ data_out[i]=b_[0] * temp[i];
for (uint32_t row = 1; row <= input_buffer_->size(); row++)
{
@@ -228,7 +262,7 @@
(data_out)[i] -= a_[row] * (*output_buffer_)[row-1][i];
}
}
- input_buffer_->push_front(current_input);
+ input_buffer_->push_front(temp);
output_buffer_->push_front(data_out);
return true;
Modified: pkg/trunk/prcore/filters/test/test_chain.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_chain.cpp 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/test/test_chain.cpp 2009-02-26 01:08:09 UTC (rev 11786)
@@ -52,6 +52,11 @@
printf("Update called\n");
return true;
};
+ virtual bool update(const std::vector<T> & data_in, std::vector<T>& data_out)
+ {
+ printf("Update called\n");
+ return true;
+ };
@@ -70,7 +75,7 @@
TEST(FilterChain, configuring){
double epsilon = 1e-9;
printf("Chain test starting\n");
- filters::FilterChain<std::vector<float> > chain;
+ filters::FilterChain<float> chain;
//filters::FilterChain<float> chain;
// EXPECT_TRUE(chain.add(mean_filter_5));
@@ -98,7 +103,7 @@
}
TEST(FilterChain, MisconfiguredNumberOfChannels){
- filters::FilterChain<std::vector<float> > chain;
+ filters::FilterChain<float> chain;
// EXPECT_TRUE(chain.add(mean_filter_5));
@@ -122,7 +127,7 @@
}
TEST(FilterChain, OverlappingNames){
- filters::FilterChain<std::vector<float> > chain;
+ filters::FilterChain<float> chain;
std::string bad_xml = "<filters> <filter type=\"MeanFilter\" name=\"mean_test\"> <params number_of_observations=\"5\"/></filter><filter type=\"MedianFilter\" name=\"mean_test\"> <params number_of_observations=\"5\"/></filter></filters>";
Modified: pkg/trunk/prcore/filters/test/test_factory.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_factory.cpp 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/test/test_factory.cpp 2009-02-26 01:08:09 UTC (rev 11786)
@@ -49,6 +49,11 @@
printf("Update called\n");
return true;
};
+ virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out)
+ {
+ printf("Update called\n");
+ return true;
+ };
@@ -57,15 +62,15 @@
ROS_REGISTER_FILTER(TestFilter, double)
ROS_REGISTER_FILTER(TestFilter, float)
ROS_REGISTER_FILTER(TestFilter, int)
-ROS_REGISTER_FILTER(TestFilter, std_vector_float)
+
TEST(FilterChain, configuring)
{
std::string name = filters::getFilterID<float>("TestFilter");
filters::FilterBase<float> * a_filter = filters::FilterFactory<float>::Instance().CreateObject(name);
- name = filters::getFilterID<std::vector<float> >("TestFilter");
- filters::FilterBase<std::vector<float> > * a1_filter = filters::FilterFactory<std::vector<float> >::Instance().CreateObject(name);
+ name = filters::getFilterID<float>("TestFilter");
+ filters::FilterBase<float> * a1_filter = filters::FilterFactory<float>::Instance().CreateObject(name);
//filters::FilterBase<int> * a1_filter = filters::FilterFactory<int>::Instance().CreateObject("TestFilter<int>");
name = filters::getFilterID<double>("TestFilter");
filters::FilterBase<double> * b_filter = filters::FilterFactory<double>::Instance().CreateObject(name);
Modified: pkg/trunk/prcore/filters/test/test_mean.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_mean.cpp 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/test/test_mean.cpp 2009-02-26 01:08:09 UTC (rev 11786)
@@ -63,7 +63,7 @@
doc.Parse("<filter type=\"MeanFilter\" name=\"mean_test\"> <params number_of_observations=\"5\"/></filter>");
TiXmlElement *config = doc.RootElement();
- FilterBase<std::vector<double> > * filter = new MeanFilter<std::vector<double> > ();
+ FilterBase<double > * filter = new MeanFilter<double > ();
filter->configure(rows, config );
double input1[] = {1,2,3,4,5};
@@ -93,7 +93,7 @@
doc.Parse("<filter type=\"MeanFilter\" name=\"mean_test\"> <params number_of_observations=\"5\"/></filter>");
TiXmlElement *config = doc.RootElement();
- FilterBase<std::vector<double> > * filter = new MeanFilter<std::vector<double> > ();
+ FilterBase<double > * filter = new MeanFilter<double > ();
filter->configure(rows, config);
double input1[] = {0,1,2,3,4};
Modified: pkg/trunk/prcore/filters/test/test_median.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_median.cpp 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/test/test_median.cpp 2009-02-26 01:08:09 UTC (rev 11786)
@@ -64,7 +64,7 @@
doc.Parse("<filter type=\"MedianFilter\" name=\"median_test\"> <params number_of_observations=\"5\"/></filter>");
TiXmlElement *config = doc.RootElement();
- FilterBase<std::vector<float> > * filter = new MedianFilter<std_vector_float>();
+ FilterBase<float > * filter = new MedianFilter<float>();
filter->configure(rows, config );
@@ -95,7 +95,7 @@
doc.Parse("<filter type=\"MedianFilter\" name=\"median_test\"> <params number_of_observations=\"5\"/></filter>");
TiXmlElement *config = doc.RootElement();
- FilterBase<std::vector<float> > * filter = new MedianFilter<std_vector_float>();
+ FilterBase<float > * filter = new MedianFilter<float>();
filter->configure(rows, config );
float input1[] = {0,1,2,3,4};
Modified: pkg/trunk/prcore/filters/test/test_transfer_function.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_transfer_function.cpp 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/prcore/filters/test/test_transfer_function.cpp 2009-02-26 01:08:09 UTC (rev 11786)
@@ -43,7 +43,7 @@
doc.Parse("<filter type=\"TransferFunctionFilter\" name=\"transferfunction_test\"> <params a=\"1.0 -0.509525449494429\" b=\" 0.245237275252786 0.245237275252786\"/></filter>");
TiXmlElement *config = doc.RootElement();
- FilterBase<std::vector<double> > * filter = new TransferFunctionFilter<std::vector<double> > ();
+ FilterBase<double> * filter = new TransferFunctionFilter<double> ();
filter->configure(1, config );
@@ -77,7 +77,7 @@
doc.Parse("<filter type=\"TransferFunctionFilter\" name=\"transferfunction_test\"> <params a=\"2.0 -0.509525449494429\" b=\" 0.245237275252786 0.245237275252786\"/></filter>");
TiXmlElement *config = doc.RootElement();
- FilterBase<std::vector<double> > * filter = new TransferFunctionFilter<std::vector<double> > ();
+ FilterBase<double> * filter = new TransferFunctionFilter<double> ();
filter->configure(1, config );
std::vector<double> in1,in2,in3,in4,in5,in6,in7;
@@ -110,7 +110,7 @@
doc.Parse("<filter type=\"TransferFunctionFilter\" name=\"transferfunction_test\"> <params a=\"1.0 -1.760041880343169 1.182893262037831 -0.278059917634546\" b=\"0.018098933007514 0.245237275252786 0.054296799022543 0.018098933007514\"/></filter>");
TiXmlElement *config = doc.RootElement();
- FilterBase<std::vector<double> > * filter = new TransferFunctionFilter<std::vector<double> > ();
+ FilterBase<double> * filter = new TransferFunctionFilter<double> ();
filter->configure(3, config );
std::vector<double> in1,in2,in3,in4,in5,in6,in7;
@@ -169,7 +169,7 @@
doc.Parse("<filter type=\"TransferFunctionFilter\" name=\"transferfunction_test\"> <params a=\"1.0 -1.760041880343169 1.182893262037831 \" b=\"0.018098933007514 0.054296799022543 0.054296799022543 0.018098933007514\"/></filter>");
TiXmlElement *config = doc.RootElement();
- FilterBase<std::vector<double> > * filter = new TransferFunctionFilter<std::vector<double> > ();
+ FilterBase<double> * filter = new TransferFunctionFilter<double> ();
filter->configure(3, config );
std::vector<double> in1,in2,in3,in4,in5,in6,in7;
Modified: pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
===================================================================
--- pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-26 01:08:09 UTC (rev 11786)
@@ -62,7 +62,7 @@
* \param scan_in The new scan to filter
* \param scan_out The filtered scan
*/
- bool update(const laser_scan::LaserScan& scan_in, laser_scan::LaserScan& scan_out);
+ bool update(const std::vector<laser_scan::LaserScan>& scan_in, std::vector<laser_scan::LaserScan>& scan_out);
private:
@@ -72,8 +72,8 @@
boost::mutex data_lock; /// Protection from multi threaded programs
laser_scan::LaserScan temp_scan_; /** \todo cache only shallow info not full scan */
- filters::FilterChain<std_vector_float > * range_filter_;
- filters::FilterChain<std_vector_float > * intensity_filter_;
+ filters::FilterChain<float> * range_filter_;
+ filters::FilterChain<float> * intensity_filter_;
boost::scoped_ptr<TiXmlElement> latest_xml_;
};
@@ -94,13 +94,16 @@
{
ROS_ASSERT(number_of_channels == 1);
latest_xml_.reset( xml_doc->Clone()->ToElement());
+ TiXmlElement * child = latest_xml_.get()->FirstChild("filters")->ToElement();
+ if (!child)
+ return false;
+ range_filter_ = new filters::FilterChain<float>();
+ if (!range_filter_->configure(num_ranges_, child))
+ return false;
- range_filter_ = new filters::FilterChain<std::vector<float> >();
- range_filter_->configure(num_ranges_, latest_xml_.get());
-
- intensity_filter_ = new filters::FilterChain<std::vector<float> >();
- intensity_filter_->configure(num_ranges_, latest_xml_.get());
-
+ intensity_filter_ = new filters::FilterChain<float>();
+ if (!intensity_filter_->configure(num_ranges_, child))
+ return false;
return true;
};
@@ -112,8 +115,16 @@
};
template <typename T>
-bool LaserMedianFilter<T>::update(const laser_scan::LaserScan& scan_in, laser_scan::LaserScan& scan_out)
+bool LaserMedianFilter<T>::update(const std::vector<laser_scan::LaserScan>& data_in, std::vector<laser_scan::LaserScan>& data_out)
{
+ if (data_in.size() != 1 || data_out.size() != 1)
+ {
+ ROS_ERROR("LaserMedianFilter is not vectorized");
+ return false;
+ }
+ const laser_scan::LaserScan & scan_in = data_in[0];
+ laser_scan::LaserScan & scan_out = data_out[0];
+
boost::mutex::scoped_lock lock(data_lock);
scan_out = scan_in; ///Quickly pass through all data \todo don't copy data too
@@ -126,12 +137,13 @@
num_ranges_ = scan_in.get_ranges_size();
-
- range_filter_ = new filters::FilterChain<std::vector<float> >();
- range_filter_->configure(num_ranges_, latest_xml_.get());
+ TiXmlElement * child = latest_xml_.get()->FirstChild("filters")->ToElement();
+
+ range_filter_ = new filters::FilterChain<float>();
+ range_filter_->configure(num_ranges_, child);
- intensity_filter_ = new filters::FilterChain<std::vector<float> >();
- intensity_filter_->configure(num_ranges_, latest_xml_.get());
+ intensity_filter_ = new filters::FilterChain<float>();
+ intensity_filter_->configure(num_ranges_, child);
}
Modified: pkg/trunk/util/laser_scan/src/median_filter_node.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/median_filter_node.cpp 2009-02-26 01:04:17 UTC (rev 11785)
+++ pkg/trunk/util/laser_scan/src/median_filter_node.cpp 2009-02-26 01:08:09 UTC (rev 11786)
@@ -40,7 +40,7 @@
laser_scan::LaserScan msg;
- MedianFilterNode(ros::Node& anode) : filter(), node_(anode)
+ MedianFilterNode(ros::Node& anode) : filter_chain_(), node_(anode)
{
std::string filter_xml;
node_.advertise<laser_scan::LaserScan>("~output", 1000);
@@ -50,17 +50,17 @@
xml_doc.Parse(filter_xml.c_str());
TiXmlElement * config = xml_doc.RootElement();
- filter.configure(1, config);
+ filter_chain_.configure(1, config);
node_.subscribe("scan_in", msg, &MedianFilterNode::callback,this, 3);
}
void callback()
{
- filter.update (msg, msg);
+ filter_chain_.update (msg, msg);
node_.publish("~output", msg);
}
protected:
- laser_scan::LaserMedianFilter<laser_scan::LaserScan> filter;
+ filters::FilterChain<laser_scan::LaserScan> filter_chain_;
ros::Node& node_;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-02-26 09:19:23
|
Revision: 11809
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11809&view=rev
Author: rdiankov
Date: 2009-02-26 09:19:19 +0000 (Thu, 26 Feb 2009)
Log Message:
-----------
bullet installation needed to copy more headers
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/prcore/bullet/Makefile
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-02-26 07:17:55 UTC (rev 11808)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-02-26 09:19:19 UTC (rev 11809)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 686
+SVN_REVISION = -r 693
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/prcore/bullet/Makefile
===================================================================
--- pkg/trunk/prcore/bullet/Makefile 2009-02-26 07:17:55 UTC (rev 11808)
+++ pkg/trunk/prcore/bullet/Makefile 2009-02-26 09:19:19 UTC (rev 11809)
@@ -17,7 +17,8 @@
$(SVN_DIR)/src/LinearMath/libLinearMath.a \
$(SVN_DIR)/src/BulletSoftBody/libBulletSoftBody.a
-BULLET_INC_DIRS = BulletCollision/CollisionShapes \
+BULLET_INC_DIRS = . \
+ BulletCollision/CollisionShapes \
BulletCollision/BroadphaseCollision \
BulletCollision/NarrowPhaseCollision \
BulletCollision/CollisionDispatch \
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-02-26 18:21:10
|
Revision: 11821
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11821&view=rev
Author: gerkey
Date: 2009-02-26 18:21:08 +0000 (Thu, 26 Feb 2009)
Log Message:
-----------
Added header to robot_srvs/IKService, to allow EE pose to be given in
non-torso_link frame. Added logic to pr2_ik_node to try to transform from
given frame to torso_link for a given timeout. Modified "test" program to
match.
Modified Paths:
--------------
pkg/trunk/prcore/robot_srvs/srv/IKService.srv
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/libKinematics/test/test_pr2_ik_node.cpp
Modified: pkg/trunk/prcore/robot_srvs/srv/IKService.srv
===================================================================
--- pkg/trunk/prcore/robot_srvs/srv/IKService.srv 2009-02-26 18:12:48 UTC (rev 11820)
+++ pkg/trunk/prcore/robot_srvs/srv/IKService.srv 2009-02-26 18:21:08 UTC (rev 11821)
@@ -1,4 +1,6 @@
+Header header
robot_msgs/Pose pose
robot_msgs/JointTrajPoint joint_pos
+duration timeout
---
robot_msgs/JointTraj traj
Modified: pkg/trunk/util/kinematics/libKinematics/include/libKinematics/pr2_ik_node.h
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/include/libKinematics/pr2_ik_node.h 2009-02-26 18:12:48 UTC (rev 11820)
+++ pkg/trunk/util/kinematics/libKinematics/include/libKinematics/pr2_ik_node.h 2009-02-26 18:21:08 UTC (rev 11821)
@@ -39,6 +39,7 @@
#include <robot_srvs/IKService.h>
#include <tf/tf.h>
+#include <tf/transform_listener.h>
#include <LinearMath/btTransform.h>
using namespace kinematics;
@@ -66,6 +67,12 @@
bool init();
double init_solution_theta3_;
+
+ // Transform incoming EE pose to the frame that we're hardwired for,
+ // obeying timeout in the request. Return true on success, false
+ // otherwise.
+ bool transformPose(robot_srvs::IKService::Request& req,
+ tf::Stamped<tf::Pose>& pose_out);
bool processIKRequest(robot_srvs::IKService::Request &req, robot_srvs::IKService::Response &resp);
@@ -73,6 +80,8 @@
private:
+ static std::string getTargetFrame() { return std::string("torso_link"); }
+
int closestJointSolution(const std::vector<double> current_joint_pos, const std::vector<std::vector<double> > new_positions);
std::string robot_description_model_;
@@ -95,5 +104,7 @@
double root_z_;
+ tf::TransformListener tf_;
+
};
}
Modified: pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp 2009-02-26 18:12:48 UTC (rev 11820)
+++ pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp 2009-02-26 18:21:08 UTC (rev 11821)
@@ -34,7 +34,7 @@
#include <libKinematics/pr2_ik_node.h>
#include <angles/angles.h>
-LibKinematicsNode::LibKinematicsNode(std::string node_name,std::string arm_name):ros::Node(node_name),arm_name_(arm_name),increment_(0.01),root_x_(0.0),root_y_(0.0), root_z_(0.0)
+LibKinematicsNode::LibKinematicsNode(std::string node_name,std::string arm_name):ros::Node(node_name),arm_name_(arm_name),increment_(0.01),root_x_(0.0),root_y_(0.0), root_z_(0.0), tf_(*this)
{
advertiseService("perform_pr2_ik", &LibKinematicsNode::processIKRequest);
advertiseService("perform_pr2_ik_closest", &LibKinematicsNode::processIKClosestRequest);
@@ -189,13 +189,54 @@
return true;
}
+// Transform incoming EE pose to the frame that we're hardwired for,
+// obeying timeout in the request. Return true on success, false
+// otherwise.
+bool LibKinematicsNode::transformPose(robot_srvs::IKService::Request& req,
+ tf::Stamped<tf::Pose>& pose_out)
+{
+ tf::Stamped<tf::Pose> tf_pose_in;
+ tf::PoseMsgToTF(req.pose, tf_pose_in);
+
+ tf_pose_in.stamp_ = req.header.stamp;
+ tf_pose_in.frame_id_ = req.header.frame_id;
+
+ // Transform into the desired frame
+ ros::Time start = ros::Time::now();
+ bool transformed = false;
+ // Heuristically chosen value
+ ros::Duration sleeptime = ros::Duration().fromSec(0.01);
+ while((req.timeout.toSec() == 0.0) ||
+ ((ros::Time::now() - start) < req.timeout))
+ {
+ try
+ {
+ tf_.transformPose(getTargetFrame(), tf_pose_in, pose_out);
+ transformed = true;
+ break;
+ }
+ catch(tf::TransformException e)
+ {
+ // Try, try, again
+ sleeptime.sleep();
+ }
+ }
+
+ return transformed;
+}
+
bool LibKinematicsNode::processIKRequest(robot_srvs::IKService::Request &req, robot_srvs::IKService::Response &resp)
{
- robot_msgs::Pose pose;
- pose = req.pose;
+ tf::Stamped<tf::Pose> tf_pose;
- tf::Pose tf_pose;
- tf::PoseMsgToTF(pose,tf_pose);
+ if(!transformPose(req, tf_pose))
+ {
+ ROS_WARN("Failed to transform from %s to %s within timeout (%f)",
+ req.header.frame_id.c_str(),
+ getTargetFrame().c_str(),
+ req.timeout.toSec());
+ return false;
+ }
NEWMAT::Matrix g0(4,4);
btScalar m[16];
@@ -236,11 +277,17 @@
bool LibKinematicsNode::processIKClosestRequest(robot_srvs::IKService::Request &req, robot_srvs::IKService::Response &resp)
{
- robot_msgs::Pose pose;
- pose = req.pose;
+ tf::Stamped<tf::Pose> tf_pose;
- tf::Pose tf_pose;
- tf::PoseMsgToTF(pose,tf_pose);
+ if(!transformPose(req, tf_pose))
+ {
+ ROS_WARN("Failed to transform from %s to %s within timeout (%f)",
+ req.header.frame_id.c_str(),
+ getTargetFrame().c_str(),
+ req.timeout.toSec());
+ return false;
+ }
+
int num_joints = req.joint_pos.positions.size();
if(num_joints != 7)
Modified: pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp 2009-02-26 18:12:48 UTC (rev 11820)
+++ pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp 2009-02-26 18:21:08 UTC (rev 11821)
@@ -58,6 +58,12 @@
robot_srvs::IKService::Request req;
robot_srvs::IKService::Response res;
+ req.header.frame_id = "torso_link";
+ // Timeout is how long we're willing to wait for the transform from
+ // our frame to the ik_node's hardcoded working frame. 0.0 is forever
+ // (dangerous!).
+ req.timeout.fromSec(0.1);
+
tf::Pose pose(btQuaternion(0,0,0),btVector3(0.75,-0.288,0));
tf::PoseTFToMsg(pose,req.pose);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-02-26 19:18:35
|
Revision: 11826
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11826&view=rev
Author: isucan
Date: 2009-02-26 19:18:31 +0000 (Thu, 26 Feb 2009)
Log Message:
-----------
publishing new_id
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-26 19:01:01 UTC (rev 11825)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-26 19:18:31 UTC (rev 11826)
@@ -203,6 +203,7 @@
m_currentRequestType = R_NONE;
m_currentPlanStatus.id = -1;
+ m_currentPlanStatus.new_id = 0;
m_currentPlanStatus.distance = -1.0;
m_currentPlanStatus.done = 1;
m_currentPlanStatus.approximate = 0;
@@ -345,6 +346,7 @@
m_currentRequestType = R_STATE;
m_currentPlanStatus.id = ++m_replanID;
+ m_currentPlanStatus.new_id = 1;
m_currentPlanStatus.valid = 1;
m_currentPlanStatus.path.set_states_size(0);
m_currentPlanStatus.done = 0;
@@ -386,6 +388,7 @@
m_currentRequestType = R_POSITION;
m_currentPlanStatus.id = ++m_replanID;
+ m_currentPlanStatus.new_id = 1;
m_currentPlanStatus.valid = 1;
m_currentPlanStatus.path.set_states_size(0);
m_currentPlanStatus.done = 0;
@@ -428,6 +431,7 @@
}
res.value.id = -1;
+ res.value.new_id = 0;
res.value.done = trivial ? 1 : 0;
res.value.valid = res.value.path.get_states_size() > 0;
res.value.approximate = approximate ? 1 : 0;
@@ -456,6 +460,7 @@
}
res.value.id = -1;
+ res.value.new_id = 0;
res.value.done = trivial ? 1 : 0;
res.value.valid = res.value.path.get_states_size() > 0;
res.value.approximate = approximate ? 1 : 0;
@@ -590,6 +595,10 @@
m_currentlyExecutedPath.set_states_size(0);
}
+ // make sure new_id becomes 0, if it was 1
+ if (m_currentPlanStatus.new_id == 1)
+ m_currentPlanStatus.new_id = 0;
+
m_statusLock.unlock();
if (replan_inactive)
Modified: pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg 2009-02-26 19:01:01 UTC (rev 11825)
+++ pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg 2009-02-26 19:18:31 UTC (rev 11826)
@@ -4,6 +4,9 @@
# replanning is not active
int32 id
+# If the ID just changed, this value will be 1. Subsequent messages
+# will have the value equal to 0
+byte new_id
# The result of a motion plan: a kinematic path. If the motion plan is
# not succesful, this path has 0 states. If the motion plan is
@@ -19,7 +22,6 @@
# 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 done
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-02-26 23:17:27
|
Revision: 11839
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11839&view=rev
Author: tfoote
Date: 2009-02-26 23:17:15 +0000 (Thu, 26 Feb 2009)
Log Message:
-----------
standardizaing filter naming, with helper functions to load from xml
Modified Paths:
--------------
pkg/trunk/prcore/filters/include/filters/filter_base.h
pkg/trunk/prcore/filters/include/filters/mean.h
pkg/trunk/prcore/filters/include/filters/median.h
pkg/trunk/prcore/filters/include/filters/transfer_function.h
pkg/trunk/util/iir_filters/include/iir_filters/iir.h
pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
Modified: pkg/trunk/prcore/filters/include/filters/filter_base.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/filter_base.h 2009-02-26 22:54:08 UTC (rev 11838)
+++ pkg/trunk/prcore/filters/include/filters/filter_base.h 2009-02-26 23:17:15 UTC (rev 11839)
@@ -80,6 +80,18 @@
virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out)=0;
std::string getType() {return typeid(T).name();};
+
+ bool setName(TiXmlElement * config)
+ {
+ const char *name = config->Attribute("name");
+ if (!name) return false;
+ filter_name_ = std::string(name);
+ return true;
+ };
+ inline const std::string& getName(){return filter_name_;};
+
+private:
+ std::string filter_name_;
};
Modified: pkg/trunk/prcore/filters/include/filters/mean.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/mean.h 2009-02-26 22:54:08 UTC (rev 11838)
+++ pkg/trunk/prcore/filters/include/filters/mean.h 2009-02-26 23:17:15 UTC (rev 11839)
@@ -65,8 +65,6 @@
virtual bool update( const T & data_in, T& data_out);
virtual bool update( const std::vector<T> & data_in, std::vector<T>& data_out);
- std::string name_; //Name of the filter.
-
protected:
RealtimeVectorCircularBuffer<std::vector<T> >* data_storage_; ///< Storage for data between updates
uint32_t last_updated_row_; ///< The last row to have been updated by the filter
@@ -98,13 +96,11 @@
return false;
// Parse the name of the filter from the xml.
- const char *name = config->Attribute("name");
- if (!name)
+ if (!FilterBase<T>::setName(config))
{
fprintf(stderr, "Error: TransferFunctionFilter was not given a name.\n");
return false;
}
- name_ = std::string(name);
// Parse the params of the filter from the xml.
TiXmlElement *p = config->FirstChildElement("params");
Modified: pkg/trunk/prcore/filters/include/filters/median.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/median.h 2009-02-26 22:54:08 UTC (rev 11838)
+++ pkg/trunk/prcore/filters/include/filters/median.h 2009-02-26 23:17:15 UTC (rev 11839)
@@ -106,8 +106,6 @@
virtual bool update(const T& data_in, T& data_out);
virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out);
- std::string name_; //Name of the filter.
-
protected:
std::vector<T> temp_storage_; ///< Preallocated storage for the list to sort
RealtimeVectorCircularBuffer<std::vector<T> >* data_storage_; ///< Storage for data between updates
@@ -145,13 +143,11 @@
return false;
// Parse the name of the filter from the xml.
- const char *name = config->Attribute("name");
- if (!name)
+ if (!FilterBase<T>::setName(config))
{
fprintf(stderr, "Error: MedianFilter was not given a name.\n");
return false;
}
- name_ = std::string(name);
// Parse the params of the filter from the xml.
TiXmlElement *p = config->FirstChildElement("params");
Modified: pkg/trunk/prcore/filters/include/filters/transfer_function.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/transfer_function.h 2009-02-26 22:54:08 UTC (rev 11838)
+++ pkg/trunk/prcore/filters/include/filters/transfer_function.h 2009-02-26 23:17:15 UTC (rev 11839)
@@ -97,7 +97,6 @@
virtual bool update(const std::vector<T> & data_in, std::vector<T>& data_out) ;
- std::string name_; //Name of the filter.
protected:
@@ -143,33 +142,32 @@
}
// Parse the name of the filter from the xml.
- const char *name = config->Attribute("name");
- if (!name)
+ if (!FilterBase<T>::setName(config))
{
ROS_ERROR("TransferFunctionFilter was not given a name.");
return false;
}
- name_ = std::string(name);
- ROS_INFO("Configuring TransferFunctionFilter with name \"%s\".", name_.c_str());
+ ROS_INFO("Configuring TransferFunctionFilter with name \"%s\".", FilterBase<T>::getName().c_str());
+
// Parse the params of the filter from the xml.
TiXmlElement *p = config->FirstChildElement("params");
if (!p)
{
- ROS_ERROR("TransferFunctionFilter, \"%s\", was not given params.", name_.c_str());
+ ROS_ERROR("TransferFunctionFilter, \"%s\", was not given params.", FilterBase<T>::getName().c_str());
return false;
}
// Parse a and b into a std::vector<double>.
if (!urdf::queryVectorAttribute(p, "a", &a_))
{
- ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute a.", name_.c_str());
+ ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute a.", FilterBase<T>::getName().c_str());
return false;
}
if (!urdf::queryVectorAttribute(p, "b", &b_))
{
- ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute b.", name_.c_str());
+ ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute b.", FilterBase<T>::getName().c_str());
return false;
}
Modified: pkg/trunk/util/iir_filters/include/iir_filters/iir.h
===================================================================
--- pkg/trunk/util/iir_filters/include/iir_filters/iir.h 2009-02-26 22:54:08 UTC (rev 11838)
+++ pkg/trunk/util/iir_filters/include/iir_filters/iir.h 2009-02-26 23:17:15 UTC (rev 11839)
@@ -102,8 +102,6 @@
virtual bool update(const std::vector<T> & data_in, std::vector<T>& data_out) ;
- std::string name_; //Name of the filter.
-
protected:
unsigned int number_of_channels_;
@@ -153,27 +151,25 @@
number_of_channels_=number_of_channels;
// Parse the name of the filter from the xml.
- const char *name = config->Attribute("name");
- if (!name)
+ if (!filters::FilterBase<T>::setName(config))
{
ROS_ERROR("IIRFilter was not given a name.");
return false;
}
- name_ = std::string(name);
- ROS_INFO("Configuring IIRFilter with name \"%s\".", name_.c_str());
+ ROS_INFO("Configuring IIRFilter with name \"%s\".", filters::FilterBase<T>::getName().c_str());
// Parse the params of the filter from the xml.
TiXmlElement *p = config->FirstChildElement("params");
if (!p)
{
- fprintf(stderr, "Error: IIRFilter, \"%s\", was not given params.", name_.c_str());
+ fprintf(stderr, "Error: IIRFilter, \"%s\", was not given params.", filters::FilterBase<T>::getName().c_str());
return false;
}
const char *t = p->Attribute("name");
if (!t)
{
- ROS_ERROR("IIRFilter, \"%s\", params has no attribute name.", name_.c_str());
+ ROS_ERROR("IIRFilter, \"%s\", params has no attribute name.", filters::FilterBase<T>::getName().c_str());
return false;
}
type_=std::string(t);
@@ -181,7 +177,7 @@
const char *s = p->Attribute("args");
if (!s)
{
- ROS_ERROR("IIRFilter, \"%s\", params has no attribute args.", name_.c_str());
+ ROS_ERROR("IIRFilter, \"%s\", params has no attribute args.", filters::FilterBase<T>::getName().c_str());
return false;
}
@@ -197,7 +193,7 @@
if (ros::service::call("filter_coeffs", req, res))
{
std::string xml_str("<filter type=\"TransferFunctionFilter\" name=\"");
- xml_str.append(name_);
+ xml_str.append(filters::FilterBase<T>::getName());
xml_str.append("\"> <params a=\"");
for(uint32_t i=0; i<res.a.size();i++)
{
@@ -222,7 +218,7 @@
}
else
{
- ROS_ERROR("IIRFilter, \"%s\", could not get filter coefficients to build filter.", name_.c_str());
+ ROS_ERROR("IIRFilter, \"%s\", could not get filter coefficients to build filter.", filters::FilterBase<T>::getName().c_str());
return false;
}
Modified: pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
===================================================================
--- pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-26 22:54:08 UTC (rev 11838)
+++ pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-26 23:17:15 UTC (rev 11839)
@@ -93,10 +93,17 @@
bool LaserMedianFilter<T>::configure(unsigned int number_of_channels, TiXmlElement * xml_doc)
{
ROS_ASSERT(number_of_channels == 1);
+ if (!filters::FilterBase<T>::setName(xml_doc))
+ {
+ ROS_ERROR("LaserMedianFilter configured without a name");
+ return false;
+ }
latest_xml_.reset( xml_doc->Clone()->ToElement());
TiXmlElement * child = latest_xml_.get()->FirstChild("filters")->ToElement();
if (!child)
return false;
+
+
range_filter_ = new filters::FilterChain<float>();
if (!range_filter_->configure(num_ranges_, child))
return false;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-02-27 02:26:51
|
Revision: 11853
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11853&view=rev
Author: gerkey
Date: 2009-02-27 02:26:46 +0000 (Fri, 27 Feb 2009)
Log Message:
-----------
Moving toward exposing move-to-EE-pose capability, using IK
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
pkg/trunk/highlevel/highlevel_controllers/src/control_cli.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/call_plan_to_state.cpp
pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-02-27 02:22:40 UTC (rev 11852)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-02-27 02:26:46 UTC (rev 11853)
@@ -37,7 +37,7 @@
roslib.load_manifest('tabletop_manipulation')
import rospy
from pr2_msgs.msg import MoveArmGoal, MoveArmState
-from robot_msgs.msg import JointState
+from robot_msgs.msg import JointState, PoseConstraint
import sys
@@ -53,15 +53,17 @@
self.goal_id = msg.goal_id
self.status = msg.status
- def moveArm(self, joints):
+ def moveArmState(self, joints):
msg = MoveArmGoal()
- msg.configuration = []
+ msg.implicit_goal = 0
+ msg.goal_configuration = []
+ msg.goal_constraints = []
for j in joints:
- msg.configuration.append(JointState(j,joints[j],0.0,0.0,0.0,0))
+ msg.goal_configuration.append(JointState(j,joints[j],0.0,0.0,0.0,0))
msg.enable = 1
msg.timeout = 0.0
self.pub.publish(msg)
- print '[MoveArm] Sending arm to: ' + `msg.configuration`
+ print '[MoveArm] Sending arm to: ' + `msg.goal_configuration`
# HACK to get around the lack of proper goal_id support
print '[MoveArm] Waiting for goal to be taken up...'
@@ -72,6 +74,62 @@
rospy.sleep(1.0)
return self.status == MoveArmState.INACTIVE
+
+ def moveArmEEPose(self, frame, position, orientation):
+ msg = MoveArmGoal()
+ msg.implicit_goal = 1
+
+ msg.goal_configuration = []
+ msg.goal_constraints = []
+
+ if position != None:
+ constraint = PoseConstraint()
+ constraint.type = constraint.POSITION_XYZ
+ constraint.robot_link = self.side[0] + '_gripper_palm_link'
+ constraint.x = position[0]
+ constraint.y = position[1]
+ constraint.z = position[2]
+ # Not used by the move_arm HLC
+ constraint.position_distance = 0.0
+ # Not used by the move_arm HLC
+ constraint.orientation_distance = 0.0
+ # Not used by the move_arm HLC
+ constraint.orientation_importance = 0.0
+ msg.goal_constraints.append(constraint)
+
+ if orientation != None:
+ constraint = PoseConstraint()
+ constraint.type = constraint.ORIENTATION_RPY
+ constraint.robot_link = self.side[0] + '_gripper_palm_link'
+ constraint.roll = orientation[0]
+ constraint.pitch = orientation[1]
+ constraint.yaw = orientation[2]
+ # Not used by the move_arm HLC
+ constraint.position_distance = 0.0
+ # Not used by the move_arm HLC
+ constraint.orientation_distance = 0.0
+ # Not used by the move_arm HLC
+ constraint.orientation_importance = 0.0
+ msg.goal_constraints.append(constraint)
+
+ if len(msg.goal_constraints) == 0:
+ print 'Must provide a position and/or orientation contraint!'
+ return False
+
+ msg.enable = 1
+ msg.timeout = 0.0
+ self.pub.publish(msg)
+ print '[MoveArm] Sending arm EE to: '
+
+ # HACK to get around the lack of proper goal_id support
+ print '[MoveArm] Waiting for goal to be taken up...'
+ rospy.sleep(2.0)
+
+ while self.status == None or self.status == MoveArmState.ACTIVE:
+ print '[MoveArm] Waiting for goal achievement...'
+ rospy.sleep(1.0)
+
+ return self.status == MoveArmState.INACTIVE
USAGE = 'movearm.py {left|right} <shoulder_lift> <shoulder_pan>'
if __name__ == '__main__':
@@ -92,7 +150,7 @@
import time
time.sleep(2.0)
- res = ma.moveArm(joints)
+ res = ma.moveArmState(joints)
if res:
print 'Success!'
Modified: pkg/trunk/highlevel/highlevel_controllers/src/control_cli.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/control_cli.cpp 2009-02-27 02:22:40 UTC (rev 11852)
+++ pkg/trunk/highlevel/highlevel_controllers/src/control_cli.cpp 2009-02-27 02:26:46 UTC (rev 11853)
@@ -187,19 +187,19 @@
pr2_msgs::MoveArmGoal goalMsg;
goalMsg.enable = 1;
- goalMsg.set_configuration_size(names.size());
+ goalMsg.set_goal_configuration_size(names.size());
for (unsigned int i = 0; i < names.size(); i++) {
- goalMsg.configuration[i].name = names[i];
- goalMsg.configuration[i].position = 0;
+ goalMsg.goal_configuration[i].name = names[i];
+ goalMsg.goal_configuration[i].position = 0;
}
- goalMsg.configuration[0].position = -1;
+ goalMsg.goal_configuration[0].position = -1;
printf("Publishing:");
for (unsigned int i = 0; i < names.size(); i++) {
- printf(" %f", goalMsg.configuration[i].position);
+ printf(" %f", goalMsg.goal_configuration[i].position);
}
printf("\n");
@@ -218,18 +218,18 @@
pr2_msgs::MoveArmGoal goalMsg;
goalMsg.enable = 1;
- goalMsg.set_configuration_size(names.size());
+ goalMsg.set_goal_configuration_size(names.size());
for (unsigned int i = 0; i < names.size(); i++) {
printf("Enter the angle for joint %s in radians:\n",
names[i].c_str());
- goalMsg.configuration[i].name = names[i];
- goalMsg.configuration[i].position = enterValue(-2 * M_PI, 2 * M_PI);
+ goalMsg.goal_configuration[i].name = names[i];
+ goalMsg.goal_configuration[i].position = enterValue(-2 * M_PI, 2 * M_PI);
}
printf("Publishing:");
for (unsigned int i = 0; i < names.size(); i++) {
- printf(" %f", goalMsg.configuration[i].position);
+ printf(" %f", goalMsg.goal_configuration[i].position);
}
printf("\n");
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp 2009-02-27 02:22:40 UTC (rev 11852)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp 2009-02-27 02:26:46 UTC (rev 11853)
@@ -81,7 +81,13 @@
#include <pr2_msgs/MoveArmState.h>
#include <pr2_msgs/MoveArmGoal.h>
+// service for (re)planning to a state
#include <robot_srvs/KinematicReplanState.h>
+// service for (re)planning to a link position
+#include <robot_srvs/KinematicReplanLinkPosition.h>
+// service for getting IK solution for an EE pose
+#include <robot_srvs/IKService.h>
+
#include <robot_msgs/DisplayKinematicPath.h>
#include <robot_msgs/KinematicPlanStatus.h>
@@ -207,6 +213,8 @@
void MoveArm::receiveStatus(void)
{
+ ROS_DEBUG("Got status: %d %d %d",
+ plan_id_, plan_status_.id, plan_status_.path.states.empty());
// HACK: sometimes we get the status for the new plan before we receive
// the response to the service call that gives us the new plan id. It
// happens that the planner increments the id by 1 with each request.
@@ -218,15 +226,12 @@
ROS_DEBUG("Got new trajectory from planner");
new_trajectory_ = true;
}
- else
- ROS_DEBUG("Got irrelevant status: %d %d %d",
- plan_id_, plan_status_.id, plan_status_.path.states.empty());
}
void MoveArm::updateGoalMsg()
{
lock();
- stateMsg.goal = goalMsg.configuration;
+ stateMsg.goal = goalMsg.goal_configuration;
ROS_DEBUG("Received new goal");
new_goal_ = true;
unlock();
@@ -312,10 +317,12 @@
new_goal_ = false;
ROS_DEBUG("Starting to plan...");
+
+ goalMsg.lock();
robot_srvs::KinematicReplanState::Request req;
robot_srvs::KinematicReplanState::Response res;
-
+
req.value.params.model_id = kinematic_model_;
req.value.params.distance_metric = "L2Square";
//req.value.params.planner_id = "SBL";
@@ -328,32 +335,76 @@
//First create stateparams for the group of intrest.
planning_models::KinematicModel::StateParams *state = robot_model_->newStateParams();
ROS_ASSERT(state);
-
+
// Skip setting the start state, so the planner will use the mech state.
-
+
+ stateParamsToMsg(state, robot_model_, kinematic_model_, req.value.goal_state.vals);
+
//Set the goal state.
- goalMsg.lock();
- for (unsigned int i = 0; i < goalMsg.configuration.size(); i++)
+ std::vector<robot_msgs::JointState> goal_configuration;
+ // If it's an implicit goal, first call out to IK to get the
+ // configuration
+ if(goalMsg.implicit_goal)
{
- planning_models::KinematicModel::Joint* j = robot_model_->getJoint(goalMsg.configuration[i].name);
+ robot_srvs::IKService::Request ikreq;
+ robot_srvs::IKService::Response ikres;
+
+ ikreq.header = goalMsg.header;
+ // Timeout is how long we're willing to wait for the transform from
+ // our frame to the ik_node's hardcoded working frame. 0.0 is forever
+ // (dangerous!).
+ ikreq.timeout.fromSec(0.1);
+
+ // Where does the 7 come from?
+ ikreq.joint_pos.set_positions_size(7);
+
+ // Replace with latest mechanism_state data
+ ikreq.joint_pos.positions[0] = 0.0;
+ ikreq.joint_pos.positions[1] = 0.0;
+ ikreq.joint_pos.positions[2] = 0.0;
+ ikreq.joint_pos.positions[3] = 0.0;
+ ikreq.joint_pos.positions[4] = 0.0;
+ ikreq.joint_pos.positions[5] = 0.0;
+ ikreq.joint_pos.positions[6] = 0.0;
+
+ if(ros::service::call("perform_pr2_ik_closest", ikreq, ikres))
+ {
+ for(int j=0; j < (int) ikres.traj.points[0].positions.size(); j++)
+ {
+ // HACK: we're assuming that the ordering of the angles returned by
+ // the IKService is the same as that expected by the planner
+ goal_configuration[j].position = ikres.traj.points[0].positions[j];
+ }
+ }
+ else
+ {
+ ROS_WARN("service call failed");
+ }
+ }
+ else
+ {
+ goal_configuration = goalMsg.goal_configuration;
+ }
+
+ for (unsigned int i = 0; i < goal_configuration.size(); i++)
+ {
+ planning_models::KinematicModel::Joint* j = robot_model_->getJoint(goal_configuration[i].name);
ROS_ASSERT(j);
unsigned int axes = j->usedParams;
ROS_ASSERT(axes == 1);
double* param = new double[axes];
- param[0] = goalMsg.configuration[i].position;
- state->setParamsJoint(param, goalMsg.configuration[i].name);
+ param[0] = goalMsg.goal_configuration[i].position;
+ state->setParamsJoint(param, goalMsg.goal_configuration[i].name);
delete[] param;
}
- goalMsg.unlock();
- stateParamsToMsg(state, robot_model_, kinematic_model_, req.value.goal_state.vals);
//ROS_DEBUG("Going for:");
//state->print();
-
+
//FIXME: should be something?
req.value.allowed_time = 0.5;
-
+
//req.params.volume* are left empty, because we're not planning for the base.
bool ret = ros::service::call("replan_kinematic_path_state", req, res);
@@ -370,6 +421,8 @@
plan_status_.unlock();
}
+ goalMsg.unlock();
+
return true;
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/call_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/call_plan_to_state.cpp 2009-02-27 02:22:40 UTC (rev 11852)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/call_plan_to_state.cpp 2009-02-27 02:26:46 UTC (rev 11853)
@@ -65,9 +65,9 @@
ag.enable = 1;
ag.timeout = 10.0;
- ag.set_configuration_size(1);
- ag.configuration[0].name = "r_shoulder_pan_joint";
- ag.configuration[0].position = -0.5;
+ ag.set_goal_configuration_size(1);
+ ag.goal_configuration[0].name = "r_shoulder_pan_joint";
+ ag.goal_configuration[0].position = -0.5;
publish("right_arm_goal", ag);
}
Modified: pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-02-27 02:22:40 UTC (rev 11852)
+++ pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2009-02-27 02:26:46 UTC (rev 11853)
@@ -1,8 +1,17 @@
Header header
+# Is the goal implicit, defined by constraints? If so, then the goal is
+# constructed from goal_constraints. Otherwise, it's taken directly from
+# configuration
+byte implicit_goal
+
# The desired joint configuration
-robot_msgs/JointState[] configuration
+robot_msgs/JointState[] goal_configuration
+# The goal state for the model to plan for. The state is not explicit.
+# The goal is considered achieved if all the constraints are satisfied.
+robot_msgs/PoseConstraint[] goal_constraints
+
# Indicate if the goal is being enabled or disabled
byte enable
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-02-27 22:04:52
|
Revision: 11896
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11896&view=rev
Author: tfoote
Date: 2009-02-27 22:04:51 +0000 (Fri, 27 Feb 2009)
Log Message:
-----------
deleting ntp_monitor package and copying the one script into runtime_monitor
Modified Paths:
--------------
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf_manip.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
Added Paths:
-----------
pkg/trunk/hardware_test/runtime_monitor/scripts/ntp_monitor.py
Removed Paths:
-------------
pkg/trunk/util/ntp_monitor/
Copied: pkg/trunk/hardware_test/runtime_monitor/scripts/ntp_monitor.py (from rev 10497, pkg/trunk/util/ntp_monitor/ntp_monitor.py)
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/scripts/ntp_monitor.py (rev 0)
+++ pkg/trunk/hardware_test/runtime_monitor/scripts/ntp_monitor.py 2009-02-27 22:04:51 UTC (rev 11896)
@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+# 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.
+
+import roslib
+roslib.load_manifest('ntp_monitor')
+
+from robot_msgs.msg import *
+import sys
+import rospy
+import socket
+import subprocess
+
+import time
+
+import re
+
+NAME = 'ntp_monitor'
+
+def ntp_monitor():
+ if len(sys.argv) < 2:
+ print "Please pass host as argument"
+ return
+ else:
+ pub = rospy.Publisher("/diagnostics", DiagnosticMessage)
+ rospy.init_node(NAME, anonymous=True)
+
+ hostname = socket.gethostbyaddr(socket.gethostname())[0]
+
+ while not rospy.is_shutdown():
+ p = subprocess.Popen(["ntpdate", "-q", sys.argv[1]], stdout=subprocess.PIPE, stdin=subprocess.PIPE, stderr=subprocess.PIPE)
+ res = p.wait()
+ (o,e) = p.communicate()
+ if (res == 0):
+ offset = float(re.search("offset (.*),", o).group(1))*1000000
+ if (abs(offset) < 500):
+ stat = DiagnosticStatus(0,hostname + " NTP offset", "Acceptable synchronization", [DiagnosticValue(offset,"offset (us)")],[])
+ print offset
+ else:
+ stat = DiagnosticStatus(2,hostname + " NTP offset", "Offset too great", [DiagnosticValue(offset,"offset (us)")],[])
+ else:
+ stat = DiagnosticStatus(2,hostname + ": NTP offset", "Failure to run ntpdate -q", [],[])
+
+ pub.publish(DiagnosticMessage(None, [stat]))
+ time.sleep(1)
+
+if __name__ == "__main__":
+ ntp_monitor()
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/manifest.xml 2009-02-27 22:03:16 UTC (rev 11895)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/manifest.xml 2009-02-27 22:04:51 UTC (rev 11896)
@@ -24,6 +24,5 @@
<depend package="imu_node"/>
<depend package="dcam"/>
<depend package="robot_pose_ekf"/>
- <depend package="ntp_monitor"/>
<depend package="spacenav_node"/>
</package>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch 2009-02-27 22:03:16 UTC (rev 11895)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch 2009-02-27 22:04:51 UTC (rev 11896)
@@ -62,9 +62,9 @@
<node pkg="rosrecord" type="rosrecord" args="-f /hwlog/pre_runtime_automatic /diagnostics" />
<!-- NTP monitoring script Warns to console if sync error -->
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="pre2" machine="realtime"/>
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="pre2" machine="three"/>
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="pre2" machine="four"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="pre2" machine="realtime"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="pre2" machine="three"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="pre2" machine="four"/>
</launch>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-02-27 22:03:16 UTC (rev 11895)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-02-27 22:04:51 UTC (rev 11896)
@@ -64,9 +64,9 @@
<node pkg="rosrecord" type="rosrecord" args="-f /hwlog/prf_runtime_automatic /diagnostics" />
<!-- NTP monitoring script Warns to console if sync error -->
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="prf2" machine="realtime"/>
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="prf2" machine="three"/>
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="prf2" machine="four"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="prf2" machine="realtime"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="prf2" machine="three"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="prf2" machine="four"/>
</launch>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf_manip.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf_manip.launch 2009-02-27 22:03:16 UTC (rev 11895)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf_manip.launch 2009-02-27 22:04:51 UTC (rev 11896)
@@ -77,9 +77,9 @@
<node pkg="rosrecord" type="rosrecord" args="-f /hwlog/prf_runtime_automatic /diagnostics" />
<!-- NTP monitoring script Warns to console if sync error -->
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="prf2" machine="realtime"/>
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="prf2" machine="three"/>
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="prf2" machine="four"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="prf2" machine="realtime"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="prf2" machine="three"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="prf2" machine="four"/>
</launch>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-02-27 22:03:16 UTC (rev 11895)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-02-27 22:04:51 UTC (rev 11896)
@@ -52,9 +52,9 @@
<node pkg="rosrecord" type="rosrecord" args="-f /hwlog/prg_runtime_automatic /diagnostics" />
<!-- NTP monitoring script Warns to console if sync error -->
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="prg2" machine="realtime"/>
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="prg2" machine="three"/>
- <node pkg="ntp_monitor" type="ntp_monitor.py" args="prg2" machine="four"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="prg2" machine="realtime"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="prg2" machine="three"/>
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="prg2" machine="four"/>
<!-- Stereo cam -->
<group ns="stereo">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-02-27 22:18:59
|
Revision: 11899
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11899&view=rev
Author: hsujohnhsu
Date: 2009-02-27 22:18:57 +0000 (Fri, 27 Feb 2009)
Log Message:
-----------
updates to arm sim scripts.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/l_arm_no_x.launch
pkg/trunk/demos/arm_gazebo/r_arm_no_x.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.xml
Modified: pkg/trunk/demos/arm_gazebo/l_arm_no_x.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm_no_x.launch 2009-02-27 22:12:36 UTC (rev 11898)
+++ pkg/trunk/demos/arm_gazebo/l_arm_no_x.launch 2009-02-27 22:18:57 UTC (rev 11899)
@@ -15,10 +15,14 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- start arm controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /> <!-- load default arm controller -->
+ <!--
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" />
+ -->
<!-- send arm a command -->
- <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+ <!--
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" />
+ -->
<!-- for visualization -->
<!--node pkg="rviz" type="rviz" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/arm_gazebo/r_arm_no_x.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_no_x.launch 2009-02-27 22:12:36 UTC (rev 11898)
+++ pkg/trunk/demos/arm_gazebo/r_arm_no_x.launch 2009-02-27 22:18:57 UTC (rev 11899)
@@ -16,12 +16,14 @@
<!-- start arm controller -->
<!--
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/r_arm_trajectory_controller.xml" respawn="false" />
<node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/r_arm_default_controller.xml" respawn="false" />
-->
- <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/r_arm_trajectory_controller.xml" respawn="false" />
<!-- send arm a command -->
- <node pkg="robot_mechanism_controllers" type="control.py" args="set r_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+ <!--
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set r_gripper_controller 0.5" respawn="false" output="screen" />
+ -->
<!-- for visualization -->
<!--node pkg="rviz" type="rviz" respawn="false" output="screen" /-->
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.xml 2009-02-27 22:12:36 UTC (rev 11898)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.xml 2009-02-27 22:18:57 UTC (rev 11899)
@@ -3,6 +3,7 @@
<!-- send single_link.xml to param server -->
<include file="$(find arm_gazebo)/l_arm_no_x.launch" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" />
<test test-name="test_pr2_mechanism_gazebo_test_arm" pkg="test_pr2_mechanism_controllers_gazebo" type="test_arm.py" time-limit="110" />
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|