|
From: <mc...@us...> - 2008-11-18 21:50:15
|
Revision: 6910
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6910&view=rev
Author: mcgann
Date: 2008-11-18 21:50:12 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
Fixed error where not updating a time stamp because of double buffering and deferred update
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-18 21:37:24 UTC (rev 6909)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-18 21:50:12 UTC (rev 6910)
@@ -63,8 +63,8 @@
std_msgs::Point origin;
// Throw out point clouds that are old.
- if((last_updated_ - point_cloud.header.stamp) > max_transform_delay){
- ROS_INFO("Discarding stale point cloud\n");
+ if((local_cloud.header.stamp - point_cloud.header.stamp) > max_transform_delay){
+ ROS_DEBUG("Discarding stale point cloud\n");
point_clouds_.pop_front();
continue;
}
@@ -93,17 +93,18 @@
}
catch(libTF::TransformReference::LookupException& ex)
{
- ROS_ERROR("Lookup exception: %s\n", ex.what());
+ ROS_ERROR("Lookup exception for %s : %s\n", frame_id_.c_str(), ex.what());
break;
}
catch(libTF::TransformReference::ExtrapolateException& ex)
{
- ROS_DEBUG("No transform available yet - have to try later: %s . Buffer size is %d\n", ex.what(), point_clouds_.size());
+ ROS_INFO("No transform available yet for %s - have to try later: %s . Buffer size is %d\n",
+ frame_id_.c_str(), ex.what(), point_clouds_.size());
break;
}
catch(libTF::TransformReference::ConnectivityException& ex)
{
- ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ ROS_ERROR("Connectivity exception for %s: %s\n", frame_id_.c_str(), ex.what());
break;
}
catch(...)
@@ -723,7 +724,7 @@
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
- ROS_INFO("Cycle Time: %.3f\n", t_diff);
+ ROS_DEBUG("Cycle Time: %.3f\n", t_diff);
if(!planOk){
// Zero out the velocities
@@ -736,8 +737,6 @@
}
}
- ROS_INFO("Dispatching velocity vector: (%f, %f, %f)\n", cmdVel.vx, cmdVel.vy, cmdVel.vw);
-
publish("cmd_vel", cmdVel);
publishFootprint(global_pose_.x, global_pose_.y, global_pose_.yaw);
@@ -911,7 +910,7 @@
//Avoids laser race conditions.
if (isInitialized()) {
- ROS_INFO("Starting cost map update/n");
+ ROS_DEBUG("Starting cost map update/n");
lock();
// Aggregate buffered observations across 3 sources
std::vector<costmap_2d::Observation> observations;
@@ -919,7 +918,7 @@
tiltScanBuffer_->get_observations(observations);
stereoCloudBuffer_->get_observations(observations);
- ROS_INFO("Applying update with %d observations/n", observations.size());
+ ROS_DEBUG("Applying update with %d observations/n", observations.size());
// Apply to cost map
struct timeval curr;
gettimeofday(&curr,NULL);
@@ -931,7 +930,7 @@
t_diff = last_t - curr_t;
publishLocalCostMap();
unlock();
- ROS_INFO("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
+ ROS_DEBUG("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
}
d->sleep();
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-18 21:37:24 UTC (rev 6909)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-18 21:50:12 UTC (rev 6910)
@@ -8,14 +8,12 @@
<include file="$(find pr2_gazebo)/pr2_floorobj.launch" />
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map_blank.png 0.1" respawn="false" />
<remap from="base_pose_gazebo_ground_truth" to="base_pose_ground_truth"/>
<param name="max_publish_frequency" value="20.0"/>
- <node pkg="fake_localization" type="fake_localization" respawn="false" />
+ <!--node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" /-->
+ <node pkg="fake_localization" type="fake_localization" args="" respawn="false" />
-
- <!--include file="$(find highlevel_controllers)/test/launch_world_3d_map.xml"/-->
-
<!-- Load parameters for moving the base. -->
<param name="costmap_2d/inflation_radius" type="double" value="0.35" />
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-18 21:37:24 UTC (rev 6909)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-18 21:50:12 UTC (rev 6910)
@@ -133,7 +133,7 @@
*/
virtual void get_observations(std::vector<Observation>& observations);
- protected:
+ private:
std::list<Observation> buffer_;
const ros::Duration keep_alive_;
ros::Time last_updated_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2008-11-18 23:33:54
|
Revision: 6924
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6924&view=rev
Author: rdiankov
Date: 2008-11-18 23:33:48 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
added openrave planning structure and an IK solver plugin that wraps libKinematics
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/robot_descriptions/openrave_robot_description/manifest.xml
Added Paths:
-----------
pkg/trunk/openrave_planning/
pkg/trunk/openrave_planning/or_plugins/
pkg/trunk/openrave_planning/or_plugins/CMakeLists.txt
pkg/trunk/openrave_planning/or_plugins/Makefile
pkg/trunk/openrave_planning/or_plugins/manifest.xml
pkg/trunk/openrave_planning/or_rosplanning/
pkg/trunk/openrave_planning/or_rosplanning/CMakeLists.txt
pkg/trunk/openrave_planning/or_rosplanning/Makefile
pkg/trunk/openrave_planning/or_rosplanning/manifest.xml
pkg/trunk/openrave_planning/or_rosplanning/src/
pkg/trunk/openrave_planning/or_rosplanning/src/or_rosplanningmain.cpp
pkg/trunk/openrave_planning/or_rosplanning/src/plugindefs.h
pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp
pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h
pkg/trunk/openrave_planning/or_rosplanning/test/
pkg/trunk/openrave_planning/or_rosplanning/test/addopenrave.m
pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m
pkg/trunk/openrave_planning/setopenrave.sh
pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-11-18 23:32:52 UTC (rev 6923)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-11-18 23:33:48 UTC (rev 6924)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 465
+SVN_REVISION = -r 472
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Added: pkg/trunk/openrave_planning/or_plugins/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/or_plugins/CMakeLists.txt (rev 0)
+++ pkg/trunk/openrave_planning/or_plugins/CMakeLists.txt 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(or_plugins)
+
+find_ros_package(or_rosplanning)
+
+file(GLOB or_rosplanning_files ${or_rosplanning_PACKAGE_PATH}/lib/*)
+
+set(openrave_plugins )
+foreach(it ${or_rosplanning_files})
+get_filename_component(basename ${it} NAME)
+add_custom_command(
+ OUTPUT ${CMAKE_SOURCE_DIR}/${basename}
+ COMMAND ${CMAKE_COMMAND} -E create_symlink
+ ARGS ${it} ${CMAKE_SOURCE_DIR}/${basename}
+ DEPENDS ${it})
+
+ set(openrave_plugins ${openrave_plugins} ${CMAKE_SOURCE_DIR}/${basename})
+endforeach(it)
+
+add_custom_target(or_plugins ALL DEPENDS ${openrave_plugins})
Added: pkg/trunk/openrave_planning/or_plugins/Makefile
===================================================================
--- pkg/trunk/openrave_planning/or_plugins/Makefile (rev 0)
+++ pkg/trunk/openrave_planning/or_plugins/Makefile 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/openrave_planning/or_plugins/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/or_plugins/manifest.xml (rev 0)
+++ pkg/trunk/openrave_planning/or_plugins/manifest.xml 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,8 @@
+<package>
+ <description brief="OpenRAVE Plugins Maintainer">
+ This package depends on all OpenRAVE plugins. Once the plugins are compiled, they will be installed to this package. All an OpenRAVE user has to do is add this path to the OPENRAVE_PLUGINS environment variable.
+ </description>
+ <author>Rosen Diankov (rdi...@cs...)</author>
+ <license>BSD</license>
+ <depend package="or_rosplanning"/>
+</package>
Added: pkg/trunk/openrave_planning/or_rosplanning/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/CMakeLists.txt (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/CMakeLists.txt 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+add_definitions(-Wall)
+rospack(or_rosplanning)
+rospack_add_library(or_rosplanning src/or_rosplanningmain.cpp src/rosarmik.cpp)
Added: pkg/trunk/openrave_planning/or_rosplanning/Makefile
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/Makefile (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/Makefile 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/openrave_planning/or_rosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/manifest.xml (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/manifest.xml 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,12 @@
+<package>
+ <description brief="OpenRAVE Plugin for ROS Planning">
+ Contains robot ik solvers, planners, and commonly used functions that integrate with the ROS framework.
+ </description>
+ <author>Rosen Diankov (rdi...@cs...)</author>
+ <license>BSD</license>
+ <depend package="roscpp"/>
+ <depend package="openrave"/>
+ <depend package="libKinematics"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
+</package>
Added: pkg/trunk/openrave_planning/or_rosplanning/src/or_rosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/or_rosplanningmain.cpp (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/or_rosplanningmain.cpp 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,68 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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 Stanford University 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 "plugindefs.h"
+
+#include "rosarmik.h"
+
+typedef void (*CREATECALLBACK)(PluginType type, const wchar_t* pname);
+EnvironmentBase* g_pEnviron = NULL;
+
+// need c linkage
+extern "C" {
+
+InterfaceBase* DECL_STDCALL(ORCreate, (PluginType type, wchar_t* name))
+{
+ switch(type) {
+ case PT_InverseKinematicsSolver:
+ if( wcsicmp(name, L"ROSArmIK") == 0 )
+ return new ROSArmIK();
+ break;
+
+ default:
+ break;
+ }
+
+ return NULL;
+}
+
+bool DECL_STDCALL(GetPluginAttributes, (PLUGININFO* pinfo, int size))
+{
+ if( pinfo == NULL ) return false;
+ if( size != sizeof(PLUGININFO) ) {
+ printf("bad plugin info sizes %d != %d\n", size, sizeof(PLUGININFO));
+ return false;
+ }
+
+ pinfo->iksolvers.push_back(L"ROSArmIK");
+ return true;
+}
+
+void DECL_STDCALL(OpenPlugin, (void* pcallback, EnvironmentBase* penv))
+{
+ RaveSetEnvironment(penv);
+ g_pEnviron = penv;
+}
+
+}
Added: pkg/trunk/openrave_planning/or_rosplanning/src/plugindefs.h
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/plugindefs.h (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/plugindefs.h 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,165 @@
+// Copyright (C) 2006-2008 Rosen Diankov (rdi...@cs...)
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#ifndef RRT_PLANNER_STDAFX
+#define RRT_PLANNER_STDAFX
+
+#define _CRT_SECURE_NO_WARNINGS
+#define _CRT_SECURE_NO_DEPRECATE
+
+#include <assert.h>
+#include <cstdio>
+#include <cmath>
+#include <cstdlib>
+
+#ifdef _MSC_VER
+#include <boost/typeof/std/string.hpp>
+#include <boost/typeof/std/vector.hpp>
+#include <boost/typeof/std/list.hpp>
+#include <boost/typeof/std/map.hpp>
+#include <boost/typeof/std/string.hpp>
+
+#define FOREACH(it, v) for(BOOST_TYPEOF(v)::iterator it = (v).begin(); it != (v).end(); (it)++)
+#define FOREACHC(it, v) for(BOOST_TYPEOF(v)::const_iterator it = (v).begin(); it != (v).end(); (it)++)
+#define RAVE_REGISTER_BOOST
+#else
+
+#include <string>
+#include <vector>
+#include <list>
+#include <map>
+#include <string>
+
+#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
+#define FOREACHC FOREACH
+
+#endif
+
+#include <fstream>
+#include <iostream>
+#include <sstream>
+
+using namespace std;
+typedef unsigned int u32;
+
+#include <sys/timeb.h> // ftime(), struct timeb
+
+// declaring variables with stdcall can be a little complex
+#ifdef _MSC_VER
+
+#define PROT_STDCALL(name, paramlist) __stdcall name paramlist
+#define DECL_STDCALL(name, paramlist) __stdcall name paramlist
+
+#else
+
+#ifdef __x86_64__
+#define DECL_STDCALL(name, paramlist) name paramlist
+#else
+#define DECL_STDCALL(name, paramlist) __attribute__((stdcall)) name paramlist
+#endif
+
+#endif // _MSC_VER
+
+template<class T>
+inline T CLAMP_ON_RANGE(T value, T min, T max)
+{
+ if (value < min) return min;
+ if (value > max) return max;
+ return value;
+}
+
+inline unsigned long timeGetTime()
+{
+#ifdef _WIN32
+ _timeb t;
+ _ftime(&t);
+#else
+ timeb t;
+ ftime(&t);
+#endif
+
+ return (unsigned long)(t.time*1000+t.millitm);
+}
+
+inline float RANDOM_FLOAT()
+{
+#if defined(__IRIX__)
+ return drand48();
+#else
+ return rand()/((float)RAND_MAX);
+#endif
+}
+
+inline float RANDOM_FLOAT(float maximum)
+{
+#if defined(__IRIX__)
+ return (drand48() * maximum);
+#else
+ return (RANDOM_FLOAT() * maximum);
+#endif
+}
+
+inline int RANDOM_INT(int maximum)
+{
+#if defined(__IRIX__)
+ return (random() % maximum);
+#else
+ return (rand() % maximum);
+#endif
+}
+
+#ifndef ARRAYSIZE
+#define ARRAYSIZE(x) (sizeof(x)/(sizeof( (x)[0] )))
+#endif
+
+#define FORIT(it, v) for(it = (v).begin(); it != (v).end(); (it)++)
+
+#ifdef _WIN32
+
+#define WCSTOK(str, delim, ptr) wcstok(str, delim)
+
+// define wcsicmp for MAC OS X
+#elif defined(__APPLE_CC__)
+
+#define WCSTOK(str, delim, ptr) wcstok(str, delim, ptr);
+
+#define strnicmp strncasecmp
+#define stricmp strcasecmp
+
+inline int wcsicmp(const wchar_t* s1, const wchar_t* s2)
+{
+ char str1[128], str2[128];
+ sprintf(str1, "%S", s1);
+ sprintf(str2, "%S", s2);
+ return stricmp(str1, str2);
+}
+
+
+#else
+
+#define WCSTOK(str, delim, ptr) wcstok(str, delim, ptr)
+
+#define strnicmp strncasecmp
+#define stricmp strcasecmp
+#define wcsnicmp wcsncasecmp
+#define wcsicmp wcscasecmp
+
+#endif
+
+#include <rave/rave.h>
+using namespace OpenRAVE;
+
+extern EnvironmentBase* g_pEnviron;
+
+#endif
Added: pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,510 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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 Stanford University 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 "plugindefs.h"
+
+#include "rosarmik.h"
+
+#ifndef SQR
+template <class T>
+inline T SQR(T x) { return x * x; }
+#endif
+
+ROSArmIK::ROSArmIK() : IkSolverBase()
+{
+}
+
+bool ROSArmIK::Init(RobotBase* probot, const RobotBase::Manipulator* pmanip, int options)
+{
+ assert( probot != NULL );
+ _probot = probot;
+ if( _probot == NULL || pmanip == NULL )
+ return false;
+
+ // get the joint limits
+ const vector<int>& vjoints = pmanip->_vecarmjoints;
+
+ vector<dReal> qlower, qupper;
+ _probot->GetJointLimits(qlower, qupper);
+ _qlower.resize(vjoints.size()); _qupper.resize(vjoints.size());
+
+ for(int i = 0; i < (int)vjoints.size(); ++i) {
+ _qlower[i] = qlower[vjoints[i]];
+ _qupper[i] = qupper[vjoints[i]];
+ }
+
+ if( _qlower.size() > 0 )
+ fiFreeParam = 1.0f / (_qupper[0]-_qlower[0]);
+ else fiFreeParam = 1;
+
+ std::vector<NEWMAT::Matrix> axis;
+ std::vector<NEWMAT::Matrix> anchor;
+ std::vector<std::string> joint_type;
+
+ NEWMAT::Matrix aj(3,1);
+ NEWMAT::Matrix an(3,1);
+
+ joint_type.resize(7);
+
+ // Shoulder pan
+ aj << 0 << 0 << -1.0;
+ axis.push_back(aj);
+ an << 0 << 0 << 0;
+ anchor.push_back(an);
+
+ // Shoulder pitch
+ aj << 0 << -1 << 0;
+ axis.push_back(aj);
+ an << 0.1 << 0 << 0;
+ anchor.push_back(an);
+
+ // Shoulder roll
+ aj << -1 << 0 << 0;
+ axis.push_back(aj);
+ an << 0.1 << 0 << 0;
+ anchor.push_back(an);
+
+ // Elbow flex
+ aj << 0 << 1 << 0;
+ axis.push_back(aj);
+ an << 0.5 << 0 << 0;
+ anchor.push_back(an);
+
+ // Forearm roll
+ aj << 1 << 0 << 0;
+ axis.push_back(aj);
+ an << 0.5 << 0 << 0;
+ anchor.push_back(an);
+
+ // Wrist flex
+ aj << 0 << 1 << 0;
+ axis.push_back(aj);
+ an << 0.82025 << 0 << 0;
+ anchor.push_back(an);
+
+ // Gripper roll
+ aj << -1 << 0 << 0;
+ axis.push_back(aj);
+ an << 0.82025 << 0 << 0;
+ anchor.push_back(an);
+
+ for(int i=0; i < 7; i++)
+ joint_type[i] = std::string("ROTARY");
+
+ iksolver.reset(new kinematics::arm7DOF(anchor,axis,joint_type));
+
+ if( pmanip->_vecarmjoints.size() > 0 ) {
+ RobotBase::RobotStateSaver saver(_probot);
+ _probot->SetTransform(Transform());
+ vector<dReal> vjoints(_probot->GetDOF(),0);
+ _probot->SetJointValues(NULL,NULL,&vjoints[0]);
+ Transform tbase = pmanip->pBase->GetTransform();
+ KinBody::Joint* pjoint = _probot->GetJoint(pmanip->_vecarmjoints.front());
+ KinBody::Link* pother = pjoint->GetFirstAttached() == pmanip->pBase ? pjoint->GetSecondAttached() : pjoint->GetFirstAttached();
+ if( pother != NULL )
+ voffset = tbase.trans - pother->GetTransform().trans;
+ }
+
+ return true;
+}
+
+// end eff transform is the transform of the wrist with respect to the base arm link
+bool ROSArmIK::Solve(const Transform &_T, const dReal* q0, bool bCheckEnvCollision, dReal* qResult)
+{
+ assert( _probot != NULL );
+
+ const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
+ assert( pmanip != NULL );
+
+ // the world coordinate system is at the origin of the intersection of the first 3 joint axes
+ assert( pmanip->_vecarmjoints.size() == _qlower.size() );
+
+ RobotBase::RobotStateSaver saver(_probot);
+
+ _probot->SetActiveDOFs(pmanip->_vecarmjoints);
+
+ // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
+ dReal startphi = q0 != NULL ? q0[0] : 0;
+ dReal upperphi = _qupper[0], lowerphi = _qlower[0], deltaphi = 0;
+ int iter = 0;
+ bool bsuccess = false;
+
+ dReal bestdist = 1000; // only valid if q0 != NULL
+ NEWMAT::Matrix nmT = GetNewMat(_T);
+
+ while(1) {
+
+ dReal curphi = startphi;
+ if( iter & 1 ) { // increment
+ curphi += deltaphi;
+ if( curphi > upperphi ) {
+
+ if( startphi-deltaphi < lowerphi)
+ break; // reached limit
+ ++iter;
+ continue;
+ }
+ }
+ else { // decrement
+ curphi -= deltaphi;
+ if( curphi < lowerphi ) {
+
+ if( startphi+deltaphi > upperphi )
+ break; // reached limit
+ deltaphi += GetPhiInc(); // increment
+ ++iter;
+ continue;
+ }
+
+ deltaphi += GetPhiInc(); // increment
+ }
+
+ iter++;
+
+ iksolver->ComputeIK(nmT,curphi);
+ vector<dReal> vravesol(_probot->GetActiveDOF());
+ vector<double>* pbest = NULL;
+ FOREACH(itsol, iksolver->solution_ik_) {
+ vector<double>& sol = *itsol;
+ assert( (int)sol.size() == _probot->GetActiveDOF());
+
+ // find the first valid solution that satisfies joint constraints and collisions
+ wstringstream ss;
+ int j;
+ for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
+ if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ break;
+ }
+
+ if( j < (int)pmanip->_vecarmjoints.size() )
+ continue; // out of bounds
+
+ // check for self collisions
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ continue;
+
+ COLLISIONREPORT report;
+ if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot, &report) ) {
+ if( report.plink1 != NULL && report.plink2 != NULL ) {
+ RAVELOG(L"WAMIK: collision %S:%S with %S:%S\n", report.plink1->GetParent()->GetName(), report.plink1->GetName(), report.plink2->GetParent()->GetName(), report.plink2->GetName());
+ }
+ continue;
+ }
+
+ // solution is valid, check with q0
+ if( q0 != NULL ) {
+
+ dReal d = 0;
+ for(int k = 0; k < (int)pmanip->_vecarmjoints.size(); ++k)
+ d += SQR(sol[k]-q0[k]);
+
+ if( bestdist > d ) {
+ pbest = /
+ bestdist = d;
+ }
+ }
+ else {
+ pbest = /
+ break;
+ }
+ }
+
+ // return as soon as a solution is found, since we're visiting phis starting from q0[0], we are guaranteed
+ // that the solution will be close (ie, phi's dominate in the search). This is to speed things up
+ if( pbest != NULL ) {
+
+ if( qResult != NULL ) {
+ for(int i = 0; i < (int)pbest->size(); ++i)
+ qResult[i] = (*pbest)[i];
+ }
+ bsuccess = true;
+ break;
+ }
+ }
+
+ return bsuccess;
+}
+
+bool ROSArmIK::Solve(const Transform &_T, bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions)
+{
+ assert( _probot != NULL );
+ const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
+
+ assert( pmanip != NULL );
+
+ qSolutions.resize(0);
+
+ // the world coordinate system is at the origin of the intersection of the first 3 joint axes
+ assert( pmanip->_vecarmjoints.size() && _qlower.size() );
+
+ RobotBase::RobotStateSaver saver(_probot);
+
+ _probot->SetActiveDOFs(pmanip->_vecarmjoints);
+
+ // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
+ double startphi = 0;
+ double upperphi = _qupper[0], lowerphi = _qlower[0], deltaphi = 0;
+ int iter = 0;
+
+ NEWMAT::Matrix nmT = GetNewMat(_T);
+
+ while(1) {
+ double curphi = startphi;
+ if( iter & 1 ) { // increment
+ curphi += deltaphi;
+ if( curphi > upperphi ) {
+
+ if( startphi-deltaphi < lowerphi)
+ break; // reached limit
+ ++iter;
+ continue;
+ }
+ }
+ else { // decrement
+ curphi -= deltaphi;
+ if( curphi < lowerphi ) {
+
+ if( startphi+deltaphi > upperphi )
+ break; // reached limit
+ ++iter;
+ deltaphi += GetPhiInc(); // increment
+ continue;
+ }
+
+ deltaphi += GetPhiInc(); // increment
+ }
+
+ iter++;
+
+ iksolver->ComputeIK(nmT,curphi);
+ vector<dReal> vravesol(_probot->GetActiveDOF());
+
+ FOREACH(itsol, iksolver->solution_ik_) {
+ vector<double>& sol = *itsol;
+ assert( (int)sol.size() == _probot->GetActiveDOF());
+
+ // find the first valid solutino that satisfies joint constraints and collisions
+ int j;
+ for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
+ if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ break;
+ }
+
+ if( j < (int)pmanip->_vecarmjoints.size() )
+ continue; // out of bounds
+
+ // check for self collisions
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ continue;
+
+ if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot) )
+ continue;
+
+ qSolutions.push_back(vector<dReal>());
+ qSolutions.back().resize(pmanip->_vecarmjoints.size());
+ for(int i = 0; i < (int)sol.size(); ++i)
+ qSolutions.back()[i] = sol[i];
+ }
+ }
+
+ return qSolutions.size()>0;
+}
+
+bool ROSArmIK::Solve(const Transform &_T, const dReal* q0, const dReal* pFreeParameters,
+ bool bCheckEnvCollision, dReal* qResult)
+{
+ if( pFreeParameters == NULL )
+ return Solve(_T, q0, bCheckEnvCollision, qResult);
+
+ assert( _probot != NULL );
+
+ const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
+ assert( pmanip != NULL );
+
+ // the world coordinate system is at the origin of the intersection of the first 3 joint axes
+ assert( pmanip->_vecarmjoints.size() == _qlower.size() );
+
+ RobotBase::RobotStateSaver saver(_probot);
+
+ _probot->SetActiveDOFs(pmanip->_vecarmjoints);
+
+ // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
+ dReal bestdist = 1000; // only valid if q0 != NULL
+
+ NEWMAT::Matrix nmT = GetNewMat(_T);
+ iksolver->ComputeIK(nmT,_qlower[0] + (_qupper[0]-_qlower[0])*pFreeParameters[0]);
+
+ vector<dReal> vravesol(_probot->GetActiveDOF());
+ vector<double>* pbest = NULL;
+ FOREACH(itsol, iksolver->solution_ik_) {
+ vector<double>& sol = *itsol;
+ assert( (int)sol.size() == _probot->GetActiveDOF());
+
+ // find the first valid solutino that satisfies joint constraints and collisions
+ int j;
+ for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
+ if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ break;
+ }
+
+ if( j < (int)pmanip->_vecarmjoints.size() )
+ continue; // out of bounds
+
+ // check for self collisions (does WAM ever self-collide?)
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ continue;
+
+ COLLISIONREPORT report;
+ if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot, &report) ) {
+ if( report.plink1 != NULL && report.plink2 != NULL ) {
+ RAVELOG(L"WAMIK: collision %S:%S with %S:%S\n", report.plink1->GetParent()->GetName(), report.plink1->GetName(), report.plink2->GetParent()->GetName(), report.plink2->GetName());
+ }
+ continue;
+ }
+
+ // solution is valid, check with q0
+ if( q0 != NULL ) {
+
+ dReal d = 0;
+
+ for(int k = 0; k < (int)pmanip->_vecarmjoints.size(); ++k)
+ d += SQR(vravesol[k]-q0[k]);
+
+ if( bestdist > d ) {
+ pbest = /
+ bestdist = d;
+ }
+ }
+ else {
+ pbest = /
+ break;
+ }
+ }
+
+ // return as soon as a solution is found, since we're visiting phis starting from q0[0], we are guaranteed
+ // that the solution will be close (ie, phi's dominate in the search). This is to speed things up
+ if( pbest != NULL ) {
+ if( qResult != NULL ) {
+ for(int i = 0; i < (int)pbest->size(); ++i)
+ qResult[i] = (*pbest)[i];
+ }
+ return true;
+ }
+
+ return false;
+}
+
+bool ROSArmIK::Solve(const Transform &_T, const dReal* pFreeParameters,
+ bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions)
+{
+ if( pFreeParameters == NULL )
+ return Solve(_T, bCheckEnvCollision, qSolutions);
+
+ assert( _probot != NULL );
+ const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
+
+ assert( pmanip != NULL );
+
+ qSolutions.resize(0);
+
+ // the world coordinate system is at the origin of the intersection of the first 3 joint axes
+ assert( pmanip->_vecarmjoints.size() && _qlower.size() );
+
+ RobotBase::RobotStateSaver saver(_probot);
+
+ _probot->SetActiveDOFs(pmanip->_vecarmjoints);
+ vector<dReal> vravesol(_probot->GetActiveDOF());
+
+ // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
+ NEWMAT::Matrix nmT = GetNewMat(_T);
+ iksolver->ComputeIK(nmT,_qlower[0] + (_qupper[0]-_qlower[0])*pFreeParameters[0]);
+
+ FOREACH(itsol, iksolver->solution_ik_) {
+ vector<double>& sol = *itsol;
+ assert( (int)sol.size() == _probot->GetActiveDOF());
+
+ // find the first valid solutino that satisfies joint constraints and collisions
+ int j;
+ for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
+ if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ break;
+ }
+
+ if( j < (int)pmanip->_vecarmjoints.size() )
+ continue; // out of bounds
+
+ // check for self collisions
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ continue;
+
+ if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot) )
+ continue;
+
+ qSolutions.push_back(vravesol);
+ }
+
+ return qSolutions.size()>0;
+}
+
+bool ROSArmIK::GetFreeParameters(dReal* pFreeParameters) const
+{
+ assert( _probo...
[truncated message content] |
|
From: <rdi...@us...> - 2008-11-19 02:35:27
|
Revision: 6960
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6960&view=rev
Author: rdiankov
Date: 2008-11-19 02:35:25 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
added parsing and loading tests for openrave robots
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description/manifest.xml
Added Paths:
-----------
pkg/trunk/robot_descriptions/openrave_robot_description/test/
pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-11-19 02:23:15 UTC (rev 6959)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-11-19 02:35:25 UTC (rev 6960)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 473
+SVN_REVISION = -r 474
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2008-11-19 02:23:15 UTC (rev 6959)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2008-11-19 02:35:25 UTC (rev 6960)
@@ -6,8 +6,11 @@
# find needed paths
find_ros_package(wg_robot_description)
find_ros_package(openrave_robot_description)
+find_ros_package(openrave)
get_target_property(urdf2openrave_exe urdf2openrave LOCATION)
+set(robot_files )
+
# process all urdf files
set(PR2_OUTPUT ${openrave_robot_description_PACKAGE_PATH}/robots/pr2.robot.xml)
set(PR2_INPUT ${wg_robot_description_PACKAGE_PATH}/pr2/pr2.xml)
@@ -16,6 +19,11 @@
COMMAND ${urdf2openrave_exe}
ARGS ${PR2_INPUT} ${PR2_OUTPUT} ../models
DEPENDS ${PR2_INPUT} urdf2openrave)
+set(robot_files ${robot_files} ${PR2_OUTPUT})
-add_custom_target(pr2_urdf ALL DEPENDS ${PR2_OUTPUT})
-add_dependencies(pr2_urdf urdf2openrave)
+add_custom_target(urdf_robots ALL DEPENDS ${robot_files})
+add_dependencies(urdf_robots urdf2openrave)
+
+# add the test package
+rospack_add_gtest(test_or_robots test/test_robots.cpp)
+set_target_properties(test_or_robots PROPERTIES COMPILE_FLAGS "-DROBOT_FILES=\\\"${robot_files}\\\" -DOPENRAVE_EXECUTABLE=\\\"${openrave_PACKAGE_PATH}/bin/openrave\\\"")
Added: pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp (rev 0)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp 2008-11-19 02:35:25 UTC (rev 6960)
@@ -0,0 +1,50 @@
+#include <gtest/gtest.h>
+#include <cstdlib>
+#include <string>
+#include <iostream>
+using namespace std;
+
+int runExternalProcess(const std::string &executable, const std::string &args)
+{
+ return system((executable + " " + args).c_str());
+}
+
+vector<string> tokenizer(string str, string delims)
+{
+ vector<string> tokens;
+ int pos, pos2;
+ pos = str.find_first_not_of(delims, 0);
+
+ while (pos >= 0)
+ {
+ pos2 = str.find_first_of(delims, pos);
+ if (pos2 < 0) pos2 = str.length();
+ tokens.push_back(str.substr(pos, pos2-pos));
+ pos = str.find_first_not_of(delims, pos2);
+ }
+ return tokens;
+}
+
+TEST(URDF, LoadRobots)
+{
+ bool bSuccess = true;
+ vector<string> files = tokenizer(ROBOT_FILES,"; \r\n");
+ for(vector<string>::iterator it = files.begin(); it != files.end(); ++it) {
+ cout << "testing: " << *it << "... ";
+ int result = runExternalProcess(OPENRAVE_EXECUTABLE, *it + string(" -testscene"));
+ if( result != 0 ) {
+ bSuccess = false;
+ cout << "fail." << endl;
+ }
+ else
+ cout << "success." << endl;
+ }
+
+ EXPECT_TRUE(bSuccess);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/manifest.xml 2008-11-19 02:23:15 UTC (rev 6959)
+++ pkg/trunk/robot_descriptions/wg_robot_description/manifest.xml 2008-11-19 02:35:25 UTC (rev 6960)
@@ -3,7 +3,7 @@
<description brief="Robot Descriptions">
This package contains XML descriptions of robots in the format
- defined at Willow Garage.
+ defined at Willow Garage along with the triangle meshes of each of the links (in stl format).
</description>
<author>Sachin Chitta, John Hsu</author>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2008-11-19 02:39:08
|
Revision: 6961
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6961&view=rev
Author: vijaypradeep
Date: 2008-11-19 02:39:06 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
Transition point_cloud utils to use to tf instead of rosTF
Now directly calling slerp commands in tf to do laser skew calculations.
Catching tf exceptions in PointCloudAssembler. Removed trailing whitespace.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/util/tf/src/transform_listener.cpp
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-11-19 02:35:25 UTC (rev 6960)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-11-19 02:39:06 UTC (rev 6961)
@@ -64,7 +64,7 @@
#include "ros/node.h"
-#include "rosTF/rosTF.h"
+#include "tf/transform_listener.h"
#include "std_msgs/LaserScan.h"
#include "std_msgs/PointCloud.h"
@@ -90,7 +90,7 @@
{
public:
- rosTFClient tf_;
+ tf::TransformListener tf_;
laser_scan::LaserProjection projector_;
LaserScan scan_;
@@ -103,6 +103,8 @@
PointCloudAssembler() : ros::node("point_cloud_assembler"), tf_(*this)
{
+ tf_.setExtrapolationLimit(ros::Duration(1.0)) ;
+
advertise_service("build_cloud", &PointCloudAssembler::buildCloud, this, 0) ; // Don't spawn threads so that we can avoid dealing with mutexing [for now]
subscribe("scan", scan_, &PointCloudAssembler::scans_callback, 40) ;
@@ -128,7 +130,7 @@
scan_hist_.push_back(scan_) ; // Add the newest scan to the back of the deque
total_pts_ += scan_.get_ranges_size() ; // Add the new scan to the running total of points
- printf("Got Scan: TotalPoints=%u\n", total_pts_) ;
+ //printf("PointCloudAssembler:: Got Scan: TotalPoints=%u", total_pts_) ;
}
bool buildCloud(BuildCloud::request& req, BuildCloud::response& resp)
@@ -152,29 +154,36 @@
{
i++ ;
}
+
printf(" Start i=%u\n", i) ;
-
+
// Populate the cloud
while ( i < scan_hist_.size() && // Don't go past end of deque
scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request
{
const LaserScan& cur_scan = scan_hist_[i] ;
-
- projector_.projectLaser(cur_scan, projector_cloud) ; // CRP: This takes care of converting the scan msg to the right format, so you don't really need to know what the format is.
- // CRP: Will be a transform in TF that takes scan time into account. High-precision, recalc for every point, from scan to point cloud. More expensive. Wait for new library to do this.
- target_frame_cloud=tf_.transformPointCloud(req.target_frame_id, projector_cloud) ;// CRP: We'll make people do this themselves for one scan line, we'll just tansform and publish aggregate clouds.
-
- resp.cloud.header = target_frame_cloud.header ; // Find a better place to do this/way to do this
- // full_cloud_.header.stamp = ros::Time::now(); //HACK
-
- for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
+
+ try
{
- resp.cloud.pts[cloud_count].x = target_frame_cloud.pts[j].x;
- resp.cloud.pts[cloud_count].y = target_frame_cloud.pts[j].y;
- resp.cloud.pts[cloud_count].z = target_frame_cloud.pts[j].z;
- resp.cloud.chan[0].vals[cloud_count] = target_frame_cloud.chan[0].vals[j];
- cloud_count++ ;
+ tf_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan) ;
+
+ for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
+ {
+ resp.cloud.pts[cloud_count].x = target_frame_cloud.pts[j].x ;
+ resp.cloud.pts[cloud_count].y = target_frame_cloud.pts[j].y ;
+ resp.cloud.pts[cloud_count].z = target_frame_cloud.pts[j].z ;
+ resp.cloud.chan[0].vals[cloud_count] = target_frame_cloud.chan[0].vals[j] ;
+ cloud_count++ ;
+ }
}
+ catch(tf::TransformException& ex)
+ {
+ ROS_WARN("Transform Exception %s", ex.what()) ;
+
+ return true ;
+ }
+
+ resp.cloud.header = target_frame_cloud.header ; // Find a better place to do this/way to do this
i++ ; // Check the next scan in the scan history
}
@@ -183,7 +192,6 @@
resp.cloud.set_pts_size( cloud_count ) ; // Resize the output accordingly
resp.cloud.chan[0].set_vals_size( cloud_count ) ;
-
return true ;
}
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-11-19 02:35:25 UTC (rev 6960)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-11-19 02:39:06 UTC (rev 6961)
@@ -58,12 +58,17 @@
pr2_mechanism_controllers::LaserScannerSignal prev_signal_;
pr2_mechanism_controllers::LaserScannerSignal cur_signal_;
+ bool first_time_ ;
+
+
PointCloudSnapshotter() : ros::node("point_cloud_snapshotter")
{
prev_signal_.header.stamp.fromNSec(0) ;
advertise<PointCloud> ("full_cloud", 1) ;
subscribe("laser_scanner_signal", cur_signal_, &PointCloudSnapshotter::scannerSignalCallback, 40) ;
+
+ first_time_ = true ;
}
~PointCloudSnapshotter()
@@ -73,21 +78,29 @@
void scannerSignalCallback()
{
- BuildCloud::request req ;
- BuildCloud::response resp ;
-
- req.begin = prev_signal_.header.stamp ;
- req.end = cur_signal_.header.stamp ;
- req.target_frame_id = "base" ;
-
- printf("Making Service Call...\n") ;
- ros::service::call("build_cloud", req, resp) ;
- printf("Done with service call\n") ;
-
- publish("full_cloud", resp.cloud) ;
- printf("Published Cloud size=%u\n", resp.cloud.get_pts_size()) ;
-
- prev_signal_ = cur_signal_ ;
+ if (first_time_)
+ {
+ prev_signal_ = cur_signal_ ;
+ first_time_ = false ;
+ }
+ else
+ {
+ BuildCloud::request req ;
+ BuildCloud::response resp ;
+
+ req.begin = prev_signal_.header.stamp ;
+ req.end = cur_signal_.header.stamp ;
+ req.target_frame_id = "torso" ;
+
+ printf("PointCloudSnapshotter::Making Service Call...\n") ;
+ ros::service::call("build_cloud", req, resp) ;
+ printf("PointCloudSnapshotter::Done with service call\n") ;
+
+ publish("full_cloud", resp.cloud) ;
+ printf("PointCloudSnapshotter::Published Cloud size=%u\n", resp.cloud.get_pts_size()) ;
+
+ prev_signal_ = cur_signal_ ;
+ }
}
} ;
Modified: pkg/trunk/util/tf/src/transform_listener.cpp
===================================================================
--- pkg/trunk/util/tf/src/transform_listener.cpp 2008-11-19 02:35:25 UTC (rev 6960)
+++ pkg/trunk/util/tf/src/transform_listener.cpp 2008-11-19 02:39:06 UTC (rev 6961)
@@ -200,17 +200,55 @@
std_msgs::PointCloud intermediate; //optimize out
projector_.projectLaser(scanIn, intermediate, -1.0, true);
+ // Extract transforms for the beginning and end of the laser scan
+ ros::Time start_time = scanIn.header.stamp ;
+ ros::Time end_time = scanIn.header.stamp + ros::Duration(scanIn.get_ranges_size()*scanIn.time_increment) ;
+
+ tf::Stamped<tf::Transform> start_transform ;
+ tf::Stamped<tf::Transform> end_transform ;
+ tf::Stamped<tf::Transform> cur_transform ;
+
+ lookupTransform(target_frame, scanIn.header.frame_id, start_time, start_transform) ;
+ lookupTransform(target_frame, scanIn.header.frame_id, end_time, end_transform) ;
+
+
unsigned int count = 0;
for (unsigned int i = 0; i < scanIn.get_ranges_size(); i++)
{
if (scanIn.ranges[i] < scanIn.range_max
&& scanIn.ranges[i] > scanIn.range_min) //only when valid
{
+ // Looking up transforms in tree is too expensive. Need more optimized way
+ /*
pointIn = tf::Stamped<tf::Point>(btVector3(intermediate.pts[i].x, intermediate.pts[i].y, intermediate.pts[i].z),
ros::Time(scanIn.header.stamp.to_ull() + (uint64_t) (scanIn.time_increment * 1000000000)),
pointIn.frame_id_ = scanIn.header.frame_id);///\todo optimize to no copy
transformPoint(target_frame, pointIn, pointOut);
+ */
+
+ // Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
+ btScalar ratio = i / ( (double) scanIn.get_ranges_size() - 1.0) ;
+
+ //! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
+ //Interpolate translation
+ btVector3 v ;
+ v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
+ cur_transform.setOrigin(v) ;
+
+ //Interpolate rotation
+ btQuaternion q1, q2 ;
+ start_transform.getBasis().getRotation(q1) ;
+ end_transform.getBasis().getRotation(q2) ;
+
+ // Compute the slerp-ed rotation
+ cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
+
+ // Apply the transform to the current point
+ btVector3 pointIn(intermediate.pts[i].x, intermediate.pts[i].y, intermediate.pts[i].z) ;
+ btVector3 pointOut = cur_transform * pointIn ;
+
+ // Copy transformed point into cloud
cloudOut.pts[count].x = pointOut.x();
cloudOut.pts[count].y = pointOut.y();
cloudOut.pts[count].z = pointOut.z();
@@ -224,7 +262,6 @@
//downsize if necessary
cloudOut.set_pts_size(count);
cloudOut.chan[0].set_vals_size(count);
-
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2008-11-19 03:23:38
|
Revision: 6967
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6967&view=rev
Author: vijaypradeep
Date: 2008-11-19 03:23:28 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
Added config params to point_cloud assembler. Added launch script to start the assembler
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/build_snapshots.launch
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-11-19 03:22:35 UTC (rev 6966)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-11-19 03:23:28 UTC (rev 6967)
@@ -95,7 +95,8 @@
LaserScan scan_;
- unsigned int max_scans_;
+ unsigned int max_scans_ ;
+ bool ignore_laser_skew_ ;
deque<LaserScan> scan_hist_ ; //!< Stores history of scans. We want them in time-ordered, which is (in most situations) the same as time-of-receipt-ordered
unsigned int total_pts_ ; //!< Stores the total number of range points in the entire stored history of scans. Useful for estimating points/scan
@@ -109,6 +110,7 @@
subscribe("scan", scan_, &PointCloudAssembler::scans_callback, 40) ;
param("point_cloud_assembler/max_scans", max_scans_, (unsigned int) 400) ;
+ param("~ignore_laser_skew", ignore_laser_skew_, true) ;
total_pts_ = 0 ; // We're always going to start with no points in our history
}
@@ -165,8 +167,16 @@
try
{
- tf_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan) ;
-
+ if (ignore_laser_skew_) // Do it the fast (approximate) way
+ {
+ projector_.projectLaser(cur_scan, projector_cloud) ;
+ tf_.transformPointCloud(req.target_frame_id, projector_cloud, target_frame_cloud) ;
+ }
+ else // Do it the slower (more accurate) way
+ {
+ tf_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan) ;
+ }
+
for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
{
resp.cloud.pts[cloud_count].x = target_frame_cloud.pts[j].x ;
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-11-19 03:22:35 UTC (rev 6966)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-11-19 03:23:28 UTC (rev 6967)
@@ -60,6 +60,7 @@
bool first_time_ ;
+ std::string fixed_frame_ ;
PointCloudSnapshotter() : ros::node("point_cloud_snapshotter")
{
@@ -67,6 +68,10 @@
advertise<PointCloud> ("full_cloud", 1) ;
subscribe("laser_scanner_signal", cur_signal_, &PointCloudSnapshotter::scannerSignalCallback, 40) ;
+
+ std::string default_fixed_frame = "torso" ;
+
+ param("~fixed_frame", fixed_frame_, std::string("torso")) ;
first_time_ = true ;
}
@@ -90,7 +95,7 @@
req.begin = prev_signal_.header.stamp ;
req.end = cur_signal_.header.stamp ;
- req.target_frame_id = "torso" ;
+ req.target_frame_id = fixed_frame_ ;
printf("PointCloudSnapshotter::Making Service Call...\n") ;
ros::service::call("build_cloud", req, resp) ;
Added: pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/build_snapshots.launch
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/build_snapshots.launch (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/build_snapshots.launch 2008-11-19 03:23:28 UTC (rev 6967)
@@ -0,0 +1,12 @@
+<launch>
+ <node pkg="point_cloud_assembler" type="point_cloud_assembler" output="screen" name="assembler">
+ <remap from="scan" to="tilt_scan"/>
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ </node>
+
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="laser_controller/laser_scanner_signal"/>
+ <param name="fixed_frame" type="string" value="torso" />
+ </node>
+
+</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2008-11-19 03:37:56
|
Revision: 6969
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6969&view=rev
Author: rdiankov
Date: 2008-11-19 03:37:49 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
updated openrave ik solver and robots tests
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp
pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h
pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m
pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-11-19 03:37:49 UTC (rev 6969)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 474
+SVN_REVISION = -r 475
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp 2008-11-19 03:37:49 UTC (rev 6969)
@@ -68,46 +68,53 @@
joint_type.resize(7);
// Shoulder pan
- aj << 0 << 0 << -1.0;
+ aj << 0 << 0 << 1.0;
axis.push_back(aj);
an << 0 << 0 << 0;
anchor.push_back(an);
+ _vjointmult.push_back(-1);
// Shoulder pitch
- aj << 0 << -1 << 0;
+ aj << 0 << 1 << 0;
axis.push_back(aj);
an << 0.1 << 0 << 0;
anchor.push_back(an);
+ _vjointmult.push_back(-1);
// Shoulder roll
- aj << -1 << 0 << 0;
+ aj << 1 << 0 << 0;
axis.push_back(aj);
an << 0.1 << 0 << 0;
anchor.push_back(an);
+ _vjointmult.push_back(-1);
// Elbow flex
aj << 0 << 1 << 0;
axis.push_back(aj);
an << 0.5 << 0 << 0;
anchor.push_back(an);
+ _vjointmult.push_back(1);
// Forearm roll
aj << 1 << 0 << 0;
axis.push_back(aj);
an << 0.5 << 0 << 0;
anchor.push_back(an);
+ _vjointmult.push_back(1);
// Wrist flex
aj << 0 << 1 << 0;
axis.push_back(aj);
an << 0.82025 << 0 << 0;
anchor.push_back(an);
+ _vjointmult.push_back(1);
// Gripper roll
- aj << -1 << 0 << 0;
+ aj << 1 << 0 << 0;
axis.push_back(aj);
an << 0.82025 << 0 << 0;
anchor.push_back(an);
+ _vjointmult.push_back(-1);
for(int i=0; i < 7; i++)
joint_type[i] = std::string("ROTARY");
@@ -189,11 +196,14 @@
vector<double>& sol = *itsol;
assert( (int)sol.size() == _probot->GetActiveDOF());
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i]*_vjointmult[i];
+
// find the first valid solution that satisfies joint constraints and collisions
wstringstream ss;
int j;
for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
- if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
break;
}
@@ -201,12 +211,11 @@
continue; // out of bounds
// check for self collisions
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i];
_probot->SetActiveDOFValues(NULL, &vravesol[0]);
- if( _probot->CheckSelfCollision() )
+ if( _probot->CheckSelfCollision() ) {
continue;
+ }
COLLISIONREPORT report;
if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot, &report) ) {
@@ -221,7 +230,7 @@
dReal d = 0;
for(int k = 0; k < (int)pmanip->_vecarmjoints.size(); ++k)
- d += SQR(sol[k]-q0[k]);
+ d += SQR(vravesol[k]-q0[k]);
if( bestdist > d ) {
pbest = /
@@ -240,7 +249,7 @@
if( qResult != NULL ) {
for(int i = 0; i < (int)pbest->size(); ++i)
- qResult[i] = (*pbest)[i];
+ qResult[i] = (*pbest)[i] * _vjointmult[i];
}
bsuccess = true;
break;
@@ -307,11 +316,14 @@
FOREACH(itsol, iksolver->solution_ik_) {
vector<double>& sol = *itsol;
assert( (int)sol.size() == _probot->GetActiveDOF());
-
+
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i]*_vjointmult[i];
+
// find the first valid solutino that satisfies joint constraints and collisions
int j;
for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
- if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
break;
}
@@ -319,8 +331,6 @@
continue; // out of bounds
// check for self collisions
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i];
_probot->SetActiveDOFValues(NULL, &vravesol[0]);
if( _probot->CheckSelfCollision() )
@@ -329,10 +339,7 @@
if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot) )
continue;
- qSolutions.push_back(vector<dReal>());
- qSolutions.back().resize(pmanip->_vecarmjoints.size());
- for(int i = 0; i < (int)sol.size(); ++i)
- qSolutions.back()[i] = sol[i];
+ qSolutions.push_back(vravesol);
}
}
@@ -369,10 +376,13 @@
vector<double>& sol = *itsol;
assert( (int)sol.size() == _probot->GetActiveDOF());
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i] * _vjointmult[i];
+
// find the first valid solutino that satisfies joint constraints and collisions
int j;
for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
- if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
break;
}
@@ -380,8 +390,6 @@
continue; // out of bounds
// check for self collisions (does WAM ever self-collide?)
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i];
_probot->SetActiveDOFValues(NULL, &vravesol[0]);
if( _probot->CheckSelfCollision() )
@@ -419,7 +427,7 @@
if( pbest != NULL ) {
if( qResult != NULL ) {
for(int i = 0; i < (int)pbest->size(); ++i)
- qResult[i] = (*pbest)[i];
+ qResult[i] = (*pbest)[i]*_vjointmult[i];
}
return true;
}
@@ -456,10 +464,13 @@
vector<double>& sol = *itsol;
assert( (int)sol.size() == _probot->GetActiveDOF());
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+
// find the first valid solutino that satisfies joint constraints and collisions
int j;
for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
- if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
break;
}
@@ -467,8 +478,6 @@
continue; // out of bounds
// check for self collisions
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i];
_probot->SetActiveDOFValues(NULL, &vravesol[0]);
if( _probot->CheckSelfCollision() )
Modified: pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h 2008-11-19 03:37:49 UTC (rev 6969)
@@ -54,7 +54,7 @@
inline dReal GetPhiInc() { return 0.04f; }
RobotBase* _probot;
- std::vector<dReal> _qlower, _qupper;
+ std::vector<dReal> _qlower, _qupper, _vjointmult;
Vector voffset;
dReal fiFreeParam;
boost::shared_ptr<kinematics::arm7DOF> iksolver;
Modified: pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m 2008-11-19 03:37:49 UTC (rev 6969)
@@ -19,7 +19,9 @@
%% left arm
orProblemSendCommand('SetActiveManip 0')
-orProblemSendCommand('debugik numtests 10',probid);
+tic;
+orProblemSendCommand('debugik numtests 100',probid);
+toc
%% right arm
% orProblemSendCommand('SetActiveManip 1')
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp 2008-11-19 03:37:49 UTC (rev 6969)
@@ -31,7 +31,7 @@
vector<string> files = tokenizer(ROBOT_FILES,"; \r\n");
for(vector<string>::iterator it = files.begin(); it != files.end(); ++it) {
cout << "testing: " << *it << "... ";
- int result = runExternalProcess(OPENRAVE_EXECUTABLE, *it + string(" -testscene"));
+ int result = runExternalProcess(OPENRAVE_EXECUTABLE, *it + string(" -testscene -nogui"));
if( result != 0 ) {
bSuccess = false;
cout << "fail." << endl;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-11-19 09:58:41
|
Revision: 6980
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6980&view=rev
Author: jleibs
Date: 2008-11-19 09:58:35 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
All of these packages still compile with opencv_latest instead of opencv. Switching over so we can conslidate on just latest opencv package.
Modified Paths:
--------------
pkg/trunk/drivers/cam/bumblebee_bridge/manifest.xml
pkg/trunk/vision/borg_laser_extractor/manifest.xml
pkg/trunk/vision/calib_converter/manifest.xml
pkg/trunk/vision/cam_viewer/manifest.xml
pkg/trunk/vision/cv_blob_tracker/manifest.xml
pkg/trunk/vision/cv_cam_calib/manifest.xml
pkg/trunk/vision/cv_movie_streamer/manifest.xml
pkg/trunk/vision/gmmseg/manifest.xml
pkg/trunk/vision/lasiklite/manifest.xml
pkg/trunk/vision/mrf_image_classifier/manifest.xml
pkg/trunk/vision/spacetime_stereo/manifest.xml
pkg/trunk/vision/spectacles/manifest.xml
Modified: pkg/trunk/drivers/cam/bumblebee_bridge/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/bumblebee_bridge/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/drivers/cam/bumblebee_bridge/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -8,5 +8,5 @@
<depend package="std_srvs"/>
<depend package="roscpp"/>
<depend package="image_utils"/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
</package>
Modified: pkg/trunk/vision/borg_laser_extractor/manifest.xml
===================================================================
--- pkg/trunk/vision/borg_laser_extractor/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/borg_laser_extractor/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -5,6 +5,6 @@
<author>Morgan Quigley</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
</package>
Modified: pkg/trunk/vision/calib_converter/manifest.xml
===================================================================
--- pkg/trunk/vision/calib_converter/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/calib_converter/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -7,7 +7,7 @@
<url></url>
<depend package="megamaid"/>
<depend package="logging"/>
-<depend package="opencv"/>
+<depend package="opencv_latest"/>
<depend package="std_msgs"/>
<depend package="image_utils"/>
<depend package="roscpp"/>
Modified: pkg/trunk/vision/cam_viewer/manifest.xml
===================================================================
--- pkg/trunk/vision/cam_viewer/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/cam_viewer/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -6,7 +6,7 @@
<author>John Hsu</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
<depend package="std_msgs"/>
<depend package="image_utils"/>
<depend package="roscpp"/>
Modified: pkg/trunk/vision/cv_blob_tracker/manifest.xml
===================================================================
--- pkg/trunk/vision/cv_blob_tracker/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/cv_blob_tracker/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -6,7 +6,7 @@
<author>Jimmy Sastra (email: sa...@wi...)</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
<depend package="std_msgs"/>
<depend package="image_utils"/>
<depend package="roscpp"/>
Modified: pkg/trunk/vision/cv_cam_calib/manifest.xml
===================================================================
--- pkg/trunk/vision/cv_cam_calib/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/cv_cam_calib/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -7,5 +7,5 @@
<author>Morgan Quigley</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
</package>
Modified: pkg/trunk/vision/cv_movie_streamer/manifest.xml
===================================================================
--- pkg/trunk/vision/cv_movie_streamer/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/cv_movie_streamer/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -6,7 +6,7 @@
<author>Morgan Quigley</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
<depend package="std_msgs"/>
<depend package="image_utils"/>
<depend package="roscpp"/>
Modified: pkg/trunk/vision/gmmseg/manifest.xml
===================================================================
--- pkg/trunk/vision/gmmseg/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/gmmseg/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -7,7 +7,7 @@
<author>Charles C. Kemp, Hai Nguyen</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
<depend package="pyrob"/>
<depend package="numpy"/>
<depend package="rospy"/>
Modified: pkg/trunk/vision/lasiklite/manifest.xml
===================================================================
--- pkg/trunk/vision/lasiklite/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/lasiklite/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -5,7 +5,7 @@
<author>Stephen Gould, with ROS wrapping by Morgan Quigley</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
<depend package="std_msgs"/>
<depend package="image_utils"/>
<depend package="roscpp"/>
Modified: pkg/trunk/vision/mrf_image_classifier/manifest.xml
===================================================================
--- pkg/trunk/vision/mrf_image_classifier/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/mrf_image_classifier/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -9,7 +9,7 @@
<url>http://pr.willowgarage.com</url>
<depend package="libbk_maxflow"/>
<depend package="libfz_ht_superpixels"/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
<depend package="roscpp"/>
<depend package="image_utils"/>
<!-- <depend package="boost"/> -->
Modified: pkg/trunk/vision/spacetime_stereo/manifest.xml
===================================================================
--- pkg/trunk/vision/spacetime_stereo/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/spacetime_stereo/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -2,6 +2,6 @@
<author>Federico</author>
<depend package="roscpp" />
<depend package="std_msgs" />
-<depend package="opencv" />
+<depend package="opencv_latest" />
<depend package="image_utils" />
</package>
\ No newline at end of file
Modified: pkg/trunk/vision/spectacles/manifest.xml
===================================================================
--- pkg/trunk/vision/spectacles/manifest.xml 2008-11-19 09:38:39 UTC (rev 6979)
+++ pkg/trunk/vision/spectacles/manifest.xml 2008-11-19 09:58:35 UTC (rev 6980)
@@ -15,7 +15,7 @@
<author>Morgan Quigley</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lspectacles"/>
</export>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-11-19 10:11:19
|
Revision: 6982
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6982&view=rev
Author: jleibs
Date: 2008-11-19 10:11:09 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
Sysdep changes to get a make buildtest of roscpp to work for gutsy. Changes to MoveBase.cpp and point_cloud_assembler.cpp to make them correctly read non natively typed parameters from the parameter server.
Modified Paths:
--------------
pkg/trunk/3rdparty/bfl_latest/manifest.xml
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/3rdparty/kdl/manifest.xml
pkg/trunk/3rdparty/soqt/manifest.xml
pkg/trunk/3rdparty/svl-1.0/manifest.xml
pkg/trunk/3rdparty/wxmpl/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/util/tf/manifest.xml
pkg/trunk/visualization/ogre_tools/manifest.xml
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/wx_rosout/manifest.xml
Modified: pkg/trunk/3rdparty/bfl_latest/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/bfl_latest/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/3rdparty/bfl_latest/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -19,6 +19,7 @@
<sysdepend os="ubuntu" version="7.04-feisty" package="subversion"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="subversion"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
+<sysdepend os="ubuntu" version="7.10-gutsy" package="libboost-dev=1.34.1-2ubuntu1"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libcppunit-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libcppunit-dev"/>
Modified: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -20,6 +20,8 @@
<sysdepend os="ubuntu" version="8.04-hardy" package="libltdl3-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libfltk1.1"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libfltk1.1-dev"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libboost-signals1.34.1"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libboost-signals-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-signals1.34.1"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-signals-dev"/>
</package>
Modified: pkg/trunk/3rdparty/kdl/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/kdl/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/3rdparty/kdl/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -13,6 +13,8 @@
<sysdepend os="ubuntu" version="7.04-feisty" package="python-sip4"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="sip4"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
+<sysdepend os="ubuntu" version="7.10-gutsy" package="libboost-dev=1.34.1-2ubuntu1"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="python-sip4-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="python-sip4"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="sip4"/>
Modified: pkg/trunk/3rdparty/soqt/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/soqt/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/3rdparty/soqt/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -10,11 +10,17 @@
<cpp lflags="-Wl,-rpath,${prefix}/lib `${prefix}/bin/soqt-config --ldflags` `${prefix}/bin/soqt-config --libs`" cflags="`${prefix}/bin/soqt-config --cppflags`"/>
<doxymaker external="http://developer.nvidia.com/page/documentation.html"/>
</export>
- <sysdepend os="ubuntu" version="7.04-hardy" package="libcoin40-dev"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libqt4-core"/>
+ <sysdepend os="ubuntu" version="7.10-gutsy" package="libqt4-core=4.3.2-0ubuntu3"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libqt4-core"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libcoin40-dev"/>
+ <sysdepend os="ubuntu" version="7.10-gutsy" package="libcoin40-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libcoin40-dev"/>
- <sysdepend os="ubuntu" version="7.04-hardy" package="qt4-dev-tools"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="qt4-dev-tools"/>
+ <sysdepend os="ubuntu" version="7.10-gutsy" package="qt4-dev-tools"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="qt4-dev-tools"/>
- <sysdepend os="ubuntu" version="7.04-hardy" package="libqt4-dev"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libqt4-dev"/>
+ <sysdepend os="ubuntu" version="7.10-gutsy" package="libqt4-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libqt4-dev"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="wget"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="wget"/>
Modified: pkg/trunk/3rdparty/svl-1.0/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/svl-1.0/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/3rdparty/svl-1.0/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -5,7 +5,7 @@
<author>Stephen Gould, with ROS wrapping by Morgan Quigley</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="opencv"/>
+ <depend package="opencv_latest"/>
<depend package="std_msgs"/>
<depend package="image_utils"/>
<depend package="roscpp"/>
Modified: pkg/trunk/3rdparty/wxmpl/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/wxmpl/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/3rdparty/wxmpl/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -15,6 +15,8 @@
<export>
<doxymaker external="http://agni.phys.iit.edu/~kmcivor/wxmpl/documentation/reference/" />
</export>
+<sysdepend os="ubuntu" version="7.04-feisty" package="python-matplotlib" />
+<sysdepend os="ubuntu" version="8.04-hardy" package="python-matplotlib" />
<sysdepend os="ubuntu" version="7.04-feisty" package="wget"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="wget"/>
</package>
Modified: pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -17,5 +17,7 @@
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lethercat_hardware -Wl,-rpath,${prefix}/lib"/>
</export>
<repository>http://pr.willowgarage.com/repos</repository>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
+<sysdepend os="ubuntu" version="7.10-gutsy" package="libboost-dev=1.34.1-2ubuntu1"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
</package>
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-11-19 10:11:09 UTC (rev 6982)
@@ -109,7 +109,13 @@
advertise_service("build_cloud", &PointCloudAssembler::buildCloud, this, 0) ; // Don't spawn threads so that we can avoid dealing with mutexing [for now]
subscribe("scan", scan_, &PointCloudAssembler::scans_callback, 40) ;
- param("point_cloud_assembler/max_scans", max_scans_, (unsigned int) 400) ;
+ int tmp_max_scans;
+ param("point_cloud_assembler/max_scans", tmp_max_scans, 400) ;
+ if (tmp_max_scans < 0)
+ tmp_max_scans = 400;
+
+ max_scans_ = tmp_max_scans;
+
param("~ignore_laser_skew", ignore_laser_skew_, true) ;
total_pts_ = 0 ; // We're always going to start with no points in our history
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-19 10:11:09 UTC (rev 6982)
@@ -233,8 +233,27 @@
double weight(0.1); // Scale costs down by a factor of 10
param("/costmap_2d/base_laser_max_range", baseLaserMaxRange_, baseLaserMaxRange_);
param("/costmap_2d/tilt_laser_max_range", tiltLaserMaxRange_, tiltLaserMaxRange_);
- param("/costmap_2d/lethal_obstacle_threshold", lethalObstacleThreshold, lethalObstacleThreshold);
- param("/costmap_2d/no_information_value", noInformation, noInformation);
+
+ // Unsigned chars cannot be stored in parameter server
+
+ int tmpLethalObstacleThreshold;
+ param("/costmap_2d/lethal_obstacle_threshold", tmpLethalObstacleThreshold, int(lethalObstacleThreshold));
+ if (tmpLethalObstacleThreshold > 255)
+ tmpLethalObstacleThreshold = 255;
+ else if (tmpLethalObstacleThreshold < 0)
+ tmpLethalObstacleThreshold = 0;
+
+ lethalObstacleThreshold = tmpLethalObstacleThreshold;
+
+ int tmpNoInformation;
+ param("/costmap_2d/no_information_value", tmpNoInformation, int(noInformation));
+ if (tmpNoInformation > 255)
+ tmpNoInformation = 255;
+ else if (tmpNoInformation < 0)
+ tmpNoInformation = 0;
+
+ noInformation = tmpNoInformation;
+
param("/costmap_2d/z_threshold", maxZ_, maxZ_);
param("/costmap_2d/freespace_projection_height", freeSpaceProjectionHeight, freeSpaceProjectionHeight);
param("/costmap_2d/inflation_radius", inflationRadius, inflationRadius);
Modified: pkg/trunk/util/tf/manifest.xml
===================================================================
--- pkg/trunk/util/tf/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/util/tf/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -17,8 +17,11 @@
<depend package="bullet"/>
<depend package="laser_scan_utils"/>
<depend package="rosTF"/> <!-- For backwards compatibility only Fixme remove /-->
-
+<sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
+<sysdepend os="ubuntu" version="7.10-gutsy" package="libboost-dev=1.34.1-2ubuntu1"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libboost-thread-dev"/>
+<sysdepend os="ubuntu" version="7.10-gutsy" package="libboost-thread-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-thread-dev"/>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -ltf"/>
Modified: pkg/trunk/visualization/ogre_tools/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_tools/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/visualization/ogre_tools/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -17,6 +17,8 @@
</export>
<sysdepend os="ubuntu" version="7.04-feisty" package="libwxgtk2.8-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libwxgtk2.8-dev"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
+ <sysdepend os="ubuntu" version="7.10-gutsy" package="libboost-dev=1.34.1-2ubuntu1"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libwxgtk2.8-dev"/>
</package>
Modified: pkg/trunk/visualization/ogre_visualizer/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -32,6 +32,8 @@
<sysdepend os="ubuntu" version="7.04-feisty" package="libwxgtk2.8-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libwxgtk2.8-dev"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
+ <sysdepend os="ubuntu" version="7.10-gutsy" package="libboost-dev=1.34.1-2ubuntu1"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-signals-dev"/>
<!-- Note that the presence of python-wxgtk2.6 will cause a
Modified: pkg/trunk/visualization/wx_rosout/manifest.xml
===================================================================
--- pkg/trunk/visualization/wx_rosout/manifest.xml 2008-11-19 10:08:57 UTC (rev 6981)
+++ pkg/trunk/visualization/wx_rosout/manifest.xml 2008-11-19 10:11:09 UTC (rev 6982)
@@ -19,6 +19,8 @@
</export>
<sysdepend os="ubuntu" version="7.04-feisty" package="libwxgtk2.8-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libwxgtk2.8-dev"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
+<sysdepend os="ubuntu" version="7.10-gutsy" package="libboost-dev=1.34.1-2ubuntu1"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libboost-regex-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-regex-dev"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-19 18:02:24
|
Revision: 6990
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6990&view=rev
Author: hsujohnhsu
Date: 2008-11-19 18:02:19 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
* update arm launch file to stop generating .model on launch.
* move Ros_Time plugin block from world files to gazebo_defs.xml.
* update testcameras.xml valid image.
* update Makefile for pr2_prototype1. (remove these later).
* fix indents in pr2d2.xml.
* sped up robot_floorobj.world to 200Hz.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/arm.launch
pkg/trunk/drivers/simulator/gazebo_plugin/test/testcamera.valid.ppm
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_pr2d2.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_simple.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/teststereo.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm/pr2d2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/Makefile
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/Makefile
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gazebo_defs.xml
Modified: pkg/trunk/demos/arm_gazebo/arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-19 18:02:19 UTC (rev 6990)
@@ -1,7 +1,5 @@
<launch>
<group name="wg">
- <!-- create model file for gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_arm_test/pr2_arm.xml.expanded $(find gazebo_robot_description)/world/pr2_arm.model" />
<!-- send pr2_arm.xml to param server -->
<include file="$(find wg_robot_description)/pr2_arm_test/send_description.launch" />
<!-- -g flag runs gazebo in gui-less mode -->
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testcamera.valid.ppm
===================================================================
(Binary files differ)
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -157,13 +157,6 @@
<!-- Robot Arm -->
<model:physical name="robot_model1">
- <!-- Broadcat time over ROS -->
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<!-- location of robot in the world -->
<xyz>0.0 0.0 0.0408</xyz>
<rpy>0.0 0.0 0.0</rpy>
@@ -173,16 +166,6 @@
<xi:include href="pr2_arm.model" />
</include>
- <!-- P3D for position groundtruth -->
- <controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <bodyName>left_gripper_palm</bodyName>
- <topicName>left_gripper_palm_pose_ground_truth</topicName>
- <frameName>map</frameName>
- <interface:position name="p3d_left_wrist_position"/>
- </controller:P3D>
-
</model:physical>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -24,7 +24,7 @@
<!-- erp is typically .1-.8 -->
<!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.001</stepTime>
+ <stepTime>0.005</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.000000000001</cfm>
<erp>0.2</erp>
@@ -305,12 +305,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 0.0 0.0408</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
<rpy>0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -340,12 +340,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 0.0 0.0408</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
<rpy>0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_pr2d2.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_pr2d2.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_pr2d2.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -309,12 +309,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 0.0 0.02</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
<rpy>0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -24,7 +24,7 @@
<!-- erp is typically .1-.8 -->
<!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.001</stepTime>
+ <stepTime>0.005</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
@@ -131,12 +131,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 0.0 0.0408</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
<rpy>0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_simple.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_simple.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_simple.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -273,12 +273,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 0.0 0.0408</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
<rpy>0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -300,12 +300,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 0.0 0.0408</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
<rpy>0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -131,12 +131,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 0.0 0.0408</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
<rpy>0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -191,12 +191,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz> -3.0 0.0 0.0408 </xyz>
<rpy> 0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -189,12 +189,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 0.0 0.0408 </xyz>
<rpy>0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -336,12 +336,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 8.0 110.0 </xyz>
<rpy>0.0 0.0 90.0 </rpy>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/teststereo.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/teststereo.world 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/teststereo.world 2008-11-19 18:02:19 UTC (rev 6990)
@@ -189,12 +189,6 @@
<model:physical name="robot_model1">
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
<xyz>0.0 0.0 0.0408 </xyz>
<rpy>0.0 0.0 0.0 </rpy>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm/pr2d2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm/pr2d2.xml 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm/pr2d2.xml 2008-11-19 18:02:19 UTC (rev 6990)
@@ -506,16 +506,22 @@
<map name="transmissions_gazebo_mechanism_control" flag="gazebo"> <!-- we can set a name too, but the convertor only cares about the flag -->
<verbatim key="transmissions_gazebo_mechanism_control" includes="true"> <!-- The key attribute is needed noly if multiple <xml> tags are in the same <map> tag -->
- <!-- PR2_ACTARRAY -->
- <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
- <robot filename="pr2d2.xml" />
+ <robot filename="pr2d2.xml" />
- <interface:audio name="gazebo_mechanism_control_dummy_iface" />
- </controller:gazebo_mechanism_control>
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
</verbatim>
</map>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/Makefile
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/Makefile 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/Makefile 2008-11-19 18:02:19 UTC (rev 6990)
@@ -4,11 +4,11 @@
ROBOT = pr2_arm
-$(ROBOT).model: $(ROBOT).xml
- $(XACRO) $^ > $^.expanded
- $(URDF2GAZEBO) $^.expanded $(MODEL_FILE)/$@
+$(ROBOT).model: $(ROBOT).xacro.xml
+ $(XACRO) $^ > $(ROBOT).xml
+ $(URDF2GAZEBO) $(ROBOT).xml $(MODEL_FILE)/$@
clean:
- $(RM) $(ROBOT).xml.expanded $(MODEL_FILE)/$(ROBOT).model
+ $(RM) $(ROBOT).xml $(MODEL_FILE)/$(ROBOT).model
$(RM) core.* Ogre.log
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml 2008-11-19 18:02:19 UTC (rev 6990)
@@ -58,4 +58,19 @@
</collision>
</link>
+ <map name="gazebo_material" flag="gazebo">
+ <verbatim>
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>left_gripper_palm</bodyName>
+ <topicName>left_gripper_palm_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_left_wrist_position"/>
+ </controller:P3D>
+ </verbatim>
+ </map>
+
+
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/Makefile
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/Makefile 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/Makefile 2008-11-19 18:02:19 UTC (rev 6990)
@@ -9,5 +9,5 @@
$(URDF2GAZEBO) $^.xml $(MODEL_FILE)/$@
clean:
- $(RM) $(ROBOT).xml.expanded $(MODEL_FILE)/$(ROBOT).model
+ $(RM) $(ROBOT).xml $(MODEL_FILE)/$(ROBOT).model
$(RM) core.* Ogre.log
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gazebo_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gazebo_defs.xml 2008-11-19 17:11:50 UTC (rev 6989)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gazebo_defs.xml 2008-11-19 18:02:19 UTC (rev 6990)
@@ -25,6 +25,12 @@
<interface:audio name="battery_dummy_interface" />
</controller:gazebo_battery>
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
</verbatim>
</map>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-11-19 18:06:18
|
Revision: 6991
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6991&view=rev
Author: stuglaser
Date: 2008-11-19 18:06:10 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
New safety code (with velocity limits) and the updated robot descriptions to go with it.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-11-19 18:06:10 UTC (rev 6991)
@@ -68,12 +68,12 @@
// Calibration parameters
double reference_position_; // The reading of the reference position, in radians
- /** \brief indicates wether the offset calibration value has been determined and wether the position safety parameters should be enforced.
- * \note The velocity and effort safety parameters are always enforced.
- */
- // Safety parameters
- /** Indicates if some safety limit parameters have been provided. Should be deprecated soon.*/
+
bool has_safety_limits_;
+
+ double k_position_limit_;
+ double k_velocity_limit_;
+
double spring_constant_min_;
double damping_constant_min_;
double safety_length_min_;
Modified: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-11-19 18:06:10 UTC (rev 6991)
@@ -54,39 +54,47 @@
void Joint::enforceLimits(JointState *s)
{
- s->commanded_effort_ = min(max(s->commanded_effort_, -effort_limit_), effort_limit_);
+ double vel_high, vel_low;
+ double effort_high, effort_low;
- if( !(has_safety_limits_ && s->calibrated_) )
+ if (!has_safety_limits_)
return;
- //TODO: add velocity control
-
- double upper_limit = joint_limit_max_ - safety_length_max_;
- double lower_limit = joint_limit_min_ + safety_length_min_;
-
- if (s->position_ > upper_limit)
+ if(s->calibrated_ && s->joint_->type_ != JOINT_CONTINUOUS)
{
- // Damping
- if (s->velocity_ > 0)
- s->commanded_effort_ += -damping_constant_max_ * s->velocity_;
+ // Computes the position bounds based on the safety lengths.
+ double pos_high = joint_limit_max_ - safety_length_max_;
+ double pos_low = joint_limit_min_ + safety_length_min_;
- // Spring
- double offset = s->position_ - upper_limit;
- s->commanded_effort_ += -spring_constant_max_ * offset;
+ // Computes the velocity bounds based on the absolute limit and the
+ // proximity to the joint limit.
+ vel_high = min(velocity_limit_,
+ -k_position_limit_ * (s->position_ - pos_high));
+ vel_low = max(-velocity_limit_,
+ -k_position_limit_ * (s->position_ - pos_low));
}
- else if(s->position_ < lower_limit)
+ else
{
- // Damping
- if (s->velocity_ < 0)
- s->commanded_effort_ += -damping_constant_min_ * s->velocity_;
+ vel_high = velocity_limit_;
+ vel_low = -velocity_limit_;
+ }
- // Spring
- double offset = s->position_ - lower_limit;
- s->commanded_effort_ += -spring_constant_min_ * offset;
+ // Computes the effort bounds based on the velocity bounds.
+ if (velocity_limit_ >= 0.0)
+ {
+ effort_high = min(effort_limit_,
+ -k_velocity_limit_ * (s->velocity_ - vel_high));
+ effort_low = max(-effort_limit_,
+ -k_velocity_limit_ * (s->velocity_ - vel_low));
}
+ else
+ {
+ effort_high = effort_limit_;
+ effort_low = -effort_limit_;
+ }
- // One more time, just in case there are bugs in the safety limit code
- s->commanded_effort_ = min(max(s->commanded_effort_, -effort_limit_), effort_limit_);
+ s->commanded_effort_ =
+ min( max(s->commanded_effort_, effort_low), effort_high);
}
bool Joint::initXml(TiXmlElement *elt)
@@ -124,8 +132,17 @@
}
if (limits->QueryDoubleAttribute("velocity", &velocity_limit_) != TIXML_SUCCESS)
- velocity_limit_ = 0.0;
+ velocity_limit_ = -1.0;
+ else
+ {
+ if (limits->QueryDoubleAttribute("k_velocity", &k_velocity_limit_) != TIXML_SUCCESS)
+ {
+ fprintf(stderr, "No k_velocity for joint %s\n", name_.c_str());
+ return false;
+ }
+ }
+
TiXmlElement *calibration = elt->FirstChildElement("calibration");
if(calibration)
{
@@ -146,59 +163,30 @@
fprintf(stderr, "Error: no min and max limits specified for joint \"%s\"\n", name_.c_str());
return false;
}
- }
- // Safety limit code
- if (type_ == JOINT_ROTARY || type_ == JOINT_PRISMATIC)
- {
- // Loads safety limits
- TiXmlElement *safetyMinElt =elt->FirstChildElement("safety_limit_min");
-
- if(!safetyMinElt && SAFETY_LIMS_STRICTLY_ENFORCED)
- return false;
- else if (safetyMinElt)
+ if (type_ == JOINT_ROTARY || type_ == JOINT_PRISMATIC)
{
- if(safetyMinElt->QueryDoubleAttribute("spring_constant", &spring_constant_min_) != TIXML_SUCCESS)
- return false;
- if(safetyMinElt->QueryDoubleAttribute("damping_constant", &damping_constant_min_) != TIXML_SUCCESS)
- return false;
- if(safetyMinElt->QueryDoubleAttribute("safety_length", &safety_length_min_) != TIXML_SUCCESS)
- return false;
+ if (limits->QueryDoubleAttribute("k_position", &k_position_limit_) != TIXML_SUCCESS)
+ fprintf(stderr, "No k_position for joint %s\n", name_.c_str());
+ if (limits->QueryDoubleAttribute("safety_length_min", &safety_length_min_) != TIXML_SUCCESS)
+ fprintf(stderr, "No safety_length_min_ for joint %s\n", name_.c_str());
+ if (limits->QueryDoubleAttribute("safety_length_max", &safety_length_max_) != TIXML_SUCCESS)
+ fprintf(stderr, "No safety_lenght_max_ for joint %s\n", name_.c_str());
- TiXmlElement *safetyMaxElt =elt->FirstChildElement("safety_limit_max");
- if(!safetyMaxElt)
- return SAFETY_LIMS_STRICTLY_ENFORCED == false;
-
- if(safetyMaxElt->QueryDoubleAttribute("spring_constant", &spring_constant_max_) != TIXML_SUCCESS)
- return false;
- if(safetyMaxElt->QueryDoubleAttribute("damping_constant", &damping_constant_max_) != TIXML_SUCCESS)
- return false;
- if(safetyMaxElt->QueryDoubleAttribute("safety_length", &safety_length_max_) != TIXML_SUCCESS)
- return false;
-
- std::cout<<"Loaded safety limit code for joint "<<name_<<std::endl;
- std::cout<<spring_constant_min_<<" "<<damping_constant_min_<<" "<<safety_length_min_<<"\n";
-
- assert(safety_length_max_ > 0);
- assert(safety_length_min_ > 0);
- assert(joint_limit_max_ > joint_limit_min_);
- const double midpoint = 0.5*(joint_limit_max_+joint_limit_min_);
- assert(joint_limit_max_ - safety_length_max_ > midpoint);
- assert(joint_limit_min_ + safety_length_min_ < midpoint);
-
has_safety_limits_ = true;
}
}
- // Parses out the joint properties, this is done for all joints as default
+
+ // Parses out the joint properties, this is done for all joints by default
TiXmlElement *prop_el = elt->FirstChildElement("joint_properties");
if (!prop_el)
{
fprintf(stderr, "Warning: Joint \"%s\" did not specify any joint properties, default to 0.\n", name_.c_str());
joint_damping_coefficient_ = 0.0;
joint_friction_coefficient_ = 0.0;
- }
- else
+ }
+ else
{
if (prop_el->QueryDoubleAttribute("damping", &joint_damping_coefficient_) != TIXML_SUCCESS)
fprintf(stderr,"damping is not specified\n");
@@ -216,7 +204,7 @@
return false;
}
std::vector<double> axis_pieces;
- urdf::queryVectorAttribute(axis_el, "xyz", &axis_pieces);
+ urdf::queryVectorAttribute(axis_el, "xyz", &axis_pieces);
if (axis_pieces.size() != 3)
{
fprintf(stderr, "Error: The axis for joint \"%s\" must have 3 value\n", name_.c_str());
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml 2008-11-19 18:06:10 UTC (rev 6991)
@@ -4,6 +4,7 @@
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
<property name="M_PI" value="3.1415926535897931" />
+ <property name="VELOCITY_LIMIT_SCALE" value="0.6" />
<property name="shoulder_pitch_length" value="0.10" />
<property name="shoulder_pitch_radius" value="0.12" />
@@ -17,10 +18,8 @@
<joint name="${prefix}_l_finger_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="0" max="${gripper_max_angle}" />
+ <limit effort="100" min="0" max="${gripper_max_angle}" />
<calibration reference_position="0" values="0 0" />
- <safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
- <safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<joint_properties damping="0.1" />
</joint>
@@ -62,10 +61,8 @@
<joint name="${prefix}_r_finger_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="${-gripper_max_angle}" max="0" />
+ <limit effort="100" min="${-gripper_max_angle}" max="0" />
<calibration reference_position="0" values="0 0" />
- <safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
- <safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<joint_properties damping="0.1" />
<mimic name="${prefix}_l_finger_joint" mult="${-1}" offset="0"/>
<map name="${prefix}_r_finger_mimic" flag="gazebo">
@@ -113,7 +110,7 @@
<joint name="${prefix}_l_finger_tip_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="${-gripper_max_angle}" max="0" />
+ <limit effort="100" min="${-gripper_max_angle}" max="0" />
<calibration reference_position="0" values="0 0" />
<safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
@@ -164,7 +161,7 @@
<joint name="${prefix}_r_finger_tip_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="0" max="${gripper_max_angle}" />
+ <limit effort="100" min="0" max="${gripper_max_angle}" />
<calibration reference_position="0" values="0 0" />
<safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
@@ -373,12 +370,13 @@
<joint name="${side}_shoulder_pan_joint" type="revolute">
<axis xyz="0 0 1" />
- <!-- TODO: the arm on the test stand has the optical switch at 0. It is actually
- at 45 degrees -->
- <limit min="-1.5" max="1.5" effort="30" velocity="5" />
- <calibration reference_position="0" values="1.5 -1" />
- <safety_limit_min safety_length="0.15" damping_constant="20" spring_constant="100" />
- <safety_limit_max safety_length="0.15" damping_constant="20" spring_constant="100" />
+
+ <limit min="${reflect*M_PI/4-1.5}" max="${reflect*M_PI/4+1.5}"
+ effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.48}"
+ k_position="100" k_velocity="10"
+ safety_length_min="0.15" safety_length_max="0.15" />
+ <calibration reference_position="${reflect*M_PI/4}" values="1.5 -1" />
+
<joint_properties damping="10.0" />
</joint>
@@ -423,10 +421,12 @@
<joint name="${side}_shoulder_pitch_joint" type="revolute">
<axis xyz="0 1 0" />
<anchor xyz="0 0 0" />
- <limit min="-0.4" max="1.5" effort="30" velocity="5" />
+
+ <limit min="-0.4" max="1.5" effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.47}"
+ k_position="100" k_velocity="10"
+ safety_length_min="0.1" safety_length_max="0.1" />
<calibration reference_position="0" values="1.5 -1 " />
- <safety_limit_min safety_length="0.1" spring_constant="50" damping_constant="12"/>
- <safety_limit_max safety_length="0.1" spring_constant="50" damping_constant="12"/>
+
<joint_properties damping="100.0" />
</joint>
@@ -471,10 +471,12 @@
<joint name="${side}_upper_arm_roll_joint" type="revolute">
<axis xyz="1 0 0" />
<anchor xyz="0 0 0" />
- <limit min="-3.9" max="0.8" effort="30" velocity="5" />
+
+ <limit min="-3.9" max="0.8" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.45}"
+ k_position="100" k_velocity="2"
+ safety_length_min="0.15" safety_length_max="0.15" />
<calibration reference_position="${-M_PI/2}" values="1.5 -1 " />
- <safety_limit_min safety_length="0.15" spring_constant="80" damping_constant="5"/>
- <safety_limit_max safety_length="0.15" spring_constant="80" damping_constant="5"/>
+
<joint_properties damping="1.0" />
</joint>
@@ -523,10 +525,12 @@
<joint name="${side}_elbow_flex_joint" type="revolute">
<axis xyz="0 -1 0" />
<anchor xyz="0 0 0" />
- <limit min="-0.1" max="2.3" effort="30" velocity="5" />
+
+ <limit min="-0.1" max="2.3" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.5}"
+ k_position="100" k_velocity="3"
+ safety_length_min="0.1" safety_length_max="0.4" />
<calibration reference_position="1.1" values="1.5 -1" />
- <safety_limit_min safety_length="0.1" spring_constant="60" damping_constant="6"/>
- <safety_limit_max safety_length="0.4" spring_constant="60" damping_constant="6"/>
+
<joint_properties damping="10.0" />
</joint>
@@ -622,8 +626,10 @@
<joint name="${side}_forearm_roll_joint" type="revolute">
<axis xyz="-1 0 0" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="5" />
+
+ <limit effort="30" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="1" />
<calibration reference_position="0" values="1.5 -1" />
+
<joint_properties damping="1.0" />
</joint>
@@ -668,10 +674,12 @@
<joint name="${side}_wrist_flex_joint" type="revolute">
<axis xyz="0 -1 0" />
<anchor xyz="0 0 0" />
- <limit min="-0.1" max="2.2" effort="200" velocity="5" />
+
+ <limit min="-0.1" max="2.2" effort="200" velocity="${VELOCITY_LIMIT_SCALE*5.13}"
+ k_position="20" k_velocity="4"
+ safety_length_min="0.2" safety_length_max="0.2" />
<calibration reference_position="0.4" values="1.5 -1" />
- <safety_limit_min safety_length="0.2" spring_constant="15" damping_constant="4"/>
- <safety_limit_max safety_length="0.2" spring_constant="15" damping_constant="4"/>
+
<joint_properties damping="1.0" />
</joint>
@@ -710,8 +718,7 @@
<joint name="${side}_wrist_roll_joint" type="revolute">
<axis xyz="1 0 0" />
<anchor xyz="0 0 0" />
- <limit effort="10" velocity="5" />
- <!--<calibration reference_position="${-0.3+M_PI/2}" values="1.5 -1" />-->
+ <limit effort="10" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="2" />
<calibration reference_position="1.27" values="1.5 -1" />
<joint_properties damping="1.0" />
</joint>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml 2008-11-19 18:06:10 UTC (rev 6991)
@@ -12,11 +12,11 @@
<macro name="pr2_wheel" params="suffix parent reflect">
<joint name="wheel_${suffix}_joint" type="revolute">
- <axis xyz=" 0 1 0 " />
+ <axis xyz="0 1 0" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="5" />
+ <limit effort="100" velocity="100" k_velocity="10" />
<calibration values="1.5 -1" />
- <joint_properties damping="0.0" friction="0.0" />
+ <joint_properties damping="1.0" friction="0.0" />
</joint>
<link name="wheel_${suffix}">
@@ -63,8 +63,8 @@
<joint name="caster_${suffix}_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
+ <limit effort="100" velocity="100" k_velocity="10" />
<calibration values="1.5 -1" />
- <limit effort="100" velocity="5" />
<joint_properties damping="1.0" friction="0.0" />
</joint>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml 2008-11-19 18:06:10 UTC (rev 6991)
@@ -7,7 +7,10 @@
<macro name="pr2_torso" params="name parent *origin">
<joint name="${name}_joint" type="prismatic">
- <limit min="0.0" max="0.396" effort="10000" velocity="5" />
+ <limit min="0.0" max="0.396" effort="10000"
+ velocity="5" k_velocity="1000"
+ safety_length_min="0.01" safety_length_max="0.01" k_position="100" />
+ <calibration reference_position="0.00536" values="0 0" />
<axis xyz="0 0 1" />
<joint_properties damping="10.0" />
</joint>
@@ -15,9 +18,7 @@
<transmission type="SimpleTransmission" name="${name}_trans">
<actuator name="${name}_motor" />
<joint name="${name}_joint" />
- <!-- <mechanicalReduction>19.26</mechanicalReduction> -->
- <mechanicalReduction>-${52143.33}</mechanicalReduction><!-- measured by Stu and Keenan -->
- <calibration reference_position="0.00536" values="0 0" />
+ <mechanicalReduction>-52143.33</mechanicalReduction>
</transmission>
<link name="${name}">
@@ -61,7 +62,7 @@
actuator="torso_motor"
transmission="torso_trans"
velocity="-2.0" />
- <pid p="6000.0" i="10" d="0" iClamp="1000" />
+ <pid p="20000.0" i="10" d="0" iClamp="1000" />
</controller>
</macro>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml 2008-11-19 18:02:19 UTC (rev 6990)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml 2008-11-19 18:06:10 UTC (rev 6991)
@@ -11,7 +11,9 @@
<joint name="${name}_mount_joint" type="revolute">
<axis xyz="0 1 0" />
<anchor xyz="0 0 0" />
- <limit min="-0.785" max="1.48" effort="0.5292" velocity="1256.0" />
+ <limit min="-0.785" max="1.48" effort="0.5292"
+ velocity="100" k_velocity="0.05"
+ safety_length_min="0.15" safety_length_max="0.15" k_position="100" />
<calibration reference_position="-0.35" values="0 0" />
<joint_properties damping="0.1" />
</joint>
@@ -212,10 +214,10 @@
<joint name="${name}_joint" type="revolute">
<axis xyz="0 0 1" />
<limit min="-2.92" max="2.92"
- effort="2.645" velocity="1256.64" />
+ effort="2.645"
+ velocity="100" k_velocity="1.5"
+ safety_length_min="0.15" safety_length_max="0.15" k_position="100" />
<calibration reference_position="-2.79" values="0 0" />
- <safety_limit_min spring_constant="15.0" safety_length="0.15" damping_constant="0.7"/>
- <safety_limit_max spring_constant="15.0" safety_length="0.15" damping_constant="0.7"/>
<joint_properties damping="1.0" />
</joint>
@@ -262,11 +264,10 @@
<joint name="${name}_joint" type="revolute">
<axis xyz="0 1 0" />
- <limit min="-0.55" max="1.047"
- effort="12.21" velocity="1256.64" />
+ <limit min="-0.55" max="1.047" effort="12.21"
+ velocity="100" k_velocity="1.5"
+ safety_length_min="0.1" safety_length_max="0.1" k_position="100" />
<calibration reference_position="-0.195" values="0 0" />
- <safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
- <safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<joint_properties damping="1.0" />
</joint>
@@ -341,7 +342,7 @@
actuator="head_tilt_motor"
transmission="head_tilt_trans"
velocity="-0.7" />
- <pid p="2.0" i="0.0" d="0" iClamp="5.0" />
+ <pid p="4.0" i="0.0" d="0" iClamp="5.0" />
</controller>
</macro>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-11-19 18:28:52
|
Revision: 6993
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6993&view=rev
Author: stuglaser
Date: 2008-11-19 18:28:50 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
Mechanism now publishes whether a joint is calibrated or not.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/robot_msgs/msg/JointState.msg
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-11-19 18:21:48 UTC (rev 6992)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-11-19 18:28:50 UTC (rev 6993)
@@ -249,6 +249,7 @@
out->velocity = in->velocity_;
out->applied_effort = in->applied_effort_;
out->commanded_effort = in->commanded_effort_;
+ out->is_calibrated = in->calibrated_;
}
for (unsigned int i = 0; i < mc_->hw_->actuators_.size(); ++i)
Modified: pkg/trunk/robot_msgs/msg/JointState.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/JointState.msg 2008-11-19 18:21:48 UTC (rev 6992)
+++ pkg/trunk/robot_msgs/msg/JointState.msg 2008-11-19 18:28:50 UTC (rev 6993)
@@ -3,3 +3,4 @@
float64 velocity
float64 applied_effort
float64 commanded_effort
+byte is_calibrated
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-11-19 22:17:17
|
Revision: 7010
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7010&view=rev
Author: jleibs
Date: 2008-11-19 22:17:12 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
Making dcam_node output pointclouds correctly. Fixing segfault condition due to uninitialized memory in dcam. Adding roslaunch example to stereo_view.
Modified Paths:
--------------
pkg/trunk/drivers/cam/dcam/CMakeLists.txt
pkg/trunk/drivers/cam/dcam/Makefile.standalone
pkg/trunk/drivers/cam/dcam/src/image.cpp
pkg/trunk/drivers/cam/dcam_node/CMakeLists.txt
pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
pkg/trunk/vision/stereo_view/stereo_view.cpp
Added Paths:
-----------
pkg/trunk/vision/stereo_view/stereo.launch
Modified: pkg/trunk/drivers/cam/dcam/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/cam/dcam/CMakeLists.txt 2008-11-19 22:16:33 UTC (rev 7009)
+++ pkg/trunk/drivers/cam/dcam/CMakeLists.txt 2008-11-19 22:17:12 UTC (rev 7010)
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
-set(ROS_BUILD_TYPE Release)
+set(ROS_BUILD_TYPE RelWithDebInfo)
rospack(dcam)
@@ -17,5 +17,5 @@
rospack_add_executable(acquire src/acquire.cpp)
target_link_libraries(acquire dcam fltk)
-rospack_add_executable(stacquire src/stacquire.cpp src/imwin.cpp)
-target_link_libraries(stacquire dcam fltk)
\ No newline at end of file
+rospack_add_executable(stacquire src/stacquire.cpp src/imwin.cpp src/im3Dwin.cpp)
+target_link_libraries(stacquire dcam fltk fltk_gl glut)
\ No newline at end of file
Modified: pkg/trunk/drivers/cam/dcam/Makefile.standalone
===================================================================
--- pkg/trunk/drivers/cam/dcam/Makefile.standalone 2008-11-19 22:16:33 UTC (rev 7009)
+++ pkg/trunk/drivers/cam/dcam/Makefile.standalone 2008-11-19 22:17:12 UTC (rev 7010)
@@ -10,7 +10,7 @@
CFLAGS = -DGCC -msse2 -g -O3 -mpreferred-stack-boundary=4 -Wall -Iinclude -I/usr/local/include
GCC = g++
#LDFLAGS = -Lbin -ldcam -ldc1394 -lcv -lhighgui -lfltk
-LDFLAGS = -Llib -ldcam -ldc1394 -lcv -lhighgui -lfltk -lfltk_gl -Wl,-rpath,$(shell rospack find opencv_latest)/opencv/lib -L$(shell rospack find opencv_latest)/opencv/lib -L$(shell rospack find libdc1394v2)/libdc1394v2/lib -Wl,-rpath,$(shell rospack find libdc1394v2)/libdc1394v2/lib -Wl,-rpath,$(shell rospack find dcam)/lib
+LDFLAGS = -Llib -ldcam -ldc1394 -lcv -lhighgui -lfltk -lfltk_gl -lglut -Wl,-rpath,$(shell rospack find opencv_latest)/opencv/lib -L$(shell rospack find opencv_latest)/opencv/lib -L$(shell rospack find libdc1394v2)/libdc1394v2/lib -Wl,-rpath,$(shell rospack find libdc1394v2)/libdc1394v2/lib -Wl,-rpath,$(shell rospack find dcam)/lib
SHAREDFLAGS = -shared -Wl,-soname,
Modified: pkg/trunk/drivers/cam/dcam/src/image.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam/src/image.cpp 2008-11-19 22:16:33 UTC (rev 7009)
+++ pkg/trunk/drivers/cam/dcam/src/image.cpp 2008-11-19 22:17:12 UTC (rev 7010)
@@ -273,6 +273,7 @@
// point array/vector
numPts = 0;
imPts = NULL;
+ imPtsColor = NULL;
isPtArray = false;
imPtsSize = 0;
}
Modified: pkg/trunk/drivers/cam/dcam_node/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/cam/dcam_node/CMakeLists.txt 2008-11-19 22:16:33 UTC (rev 7009)
+++ pkg/trunk/drivers/cam/dcam_node/CMakeLists.txt 2008-11-19 22:17:12 UTC (rev 7010)
@@ -1,6 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
-set(ROS_BUILD_TYPE Release)
+set(ROS_BUILD_TYPE RelWithDebInfo)
rospack(dcam_node)
rospack_add_executable(dcam_node dcam_node.cpp)
Modified: pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-19 22:16:33 UTC (rev 7009)
+++ pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-19 22:17:12 UTC (rev 7010)
@@ -71,6 +71,7 @@
public:
static dcam::Dcam* cam_;
+ static cam::StereoDcam* stcam_;
DcamNode() : ros::node("dcam"), diagnostic_(this), count_(0)
@@ -204,7 +205,8 @@
// is definitely wrong.
if (stereo_cam_)
{
- cam_ = new cam::StereoDcam(guid);
+ stcam_ = new cam::StereoDcam(guid);
+ cam_ = stcam_;
cam_->setFormat(mode, fps, speed);
cam_->setProcMode(videre_mode);
} else {
@@ -213,6 +215,10 @@
}
cam_->start();
+
+ cam_->setUniqueThresh(12);
+ cam_->setTextureThresh(10);
+
serviceCam();
printf("Advertising\n");
advertiseCam();
@@ -237,19 +243,22 @@
if (do_rectify_)
{
- ( (cam::StereoDcam*)(cam_) )->doRectify();
+ // ( (cam::StereoDcam*)(cam_) )->doRectify();
+ stcam_->doRectify();
}
// cam_->doRectify();
if (do_stereo_)
{
- ( (cam::StereoDcam*)(cam_) )->doDisparity();
+ // ( (cam::StereoDcam*)(cam_) )->doDisparity();
+ stcam_->doDisparity();
}
if (do_calc_points_)
{
- ( (cam::StereoDcam*)(cam_) )->doCalcPts();
+ //( (cam::StereoDcam*)(cam_) )->doCalcPts();
+ stcam_->doCalcPts();
}
count_++;
@@ -511,6 +520,7 @@
};
dcam::Dcam* DcamNode::cam_ = 0;
+cam::StereoDcam* DcamNode::stcam_ = 0;
void sigsegv_handler(int sig)
{
Modified: pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-19 22:16:33 UTC (rev 7009)
+++ pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-19 22:17:12 UTC (rev 7010)
@@ -112,7 +112,15 @@
reallocCvtIfNeeded(cvGetSize(rosimg_), IPL_DEPTH_8U, 3);
cvCvtColor(rosimg_, cvtimg_, CV_RGB2BGR);
img_ = cvtimg_;
- } else {
+ }
+ else if (encoding == "bgr" && rosimg.encoding == "mono" )
+ {
+ reallocCvtIfNeeded(cvGetSize(rosimg_), IPL_DEPTH_8U, 3);
+ cvCvtColor(rosimg_, cvtimg_, CV_GRAY2BGR);
+ img_ = cvtimg_;
+ }
+ else
+ {
return false;
}
}
Added: pkg/trunk/vision/stereo_view/stereo.launch
===================================================================
--- pkg/trunk/vision/stereo_view/stereo.launch (rev 0)
+++ pkg/trunk/vision/stereo_view/stereo.launch 2008-11-19 22:17:12 UTC (rev 7010)
@@ -0,0 +1,19 @@
+<launch>
+ <node name="dcam" pkg="dcam_node" type="dcam_node" respawn="false">
+ <!-- video_mode should be one of the following:
+ 640x480_videre_disparity: Disparity and rectification done on chip.
+ Provides: Disparity and left mono image
+ 640x480_videre_disparity_raw: Disparity done on chip (debayer and rectification in software).
+ Provides: disparity and left color image.
+ 640x480_videre_none: No stereo on chip (all processing done in software).
+ Provides: all 3 images available
+ -->
+ <param name="video_mode" type="str" value="640x480_videre_disparity_raw"/>
+ <param name="do_stereo" type="bool" value="True"/>
+ </node>
+ <node name="stereo_view" pkg="stereo_view" type="stereo_view" respawn="false">
+ <!-- Remove this parameter to subscribe to mono images instead -->
+ <param name="subscribe_color" type="bool" value="True"/>
+ </node>
+</launch>
+
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-19 22:16:33 UTC (rev 7009)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-19 22:17:12 UTC (rev 7010)
@@ -54,6 +54,7 @@
image_msgs::Image limage;
image_msgs::Image rimage;
image_msgs::Image dimage;
+ image_msgs::StereoInfo stinfo;
image_msgs::CvBridge lbridge;
image_msgs::CvBridge rbridge;
@@ -61,15 +62,26 @@
TopicSynchronizer<StereoView> sync;
- StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout)
+ bool subscribe_color_;
+
+ StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout), subscribe_color_(false)
{
cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
- sync.subscribe("dcam/left/image_rect_color", limage, 1);
- sync.subscribe("dcam/right/image_rect_color", rimage, 1);
+ param("~subscribe_color", subscribe_color_, false);
+
+ if (subscribe_color_)
+ {
+ sync.subscribe("dcam/left/image_rect_color", limage, 1);
+ sync.subscribe("dcam/right/image_rect_color", rimage, 1);
+ } else {
+ sync.subscribe("dcam/left/image_rect", limage, 1);
+ sync.subscribe("dcam/right/image_rect", rimage, 1);
+ }
sync.subscribe("dcam/disparity", dimage, 1);
+ sync.subscribe("dcam/stereo_info", stinfo, 1);
}
void image_cb_all(ros::Time t)
@@ -84,7 +96,7 @@
{
// Disparity has to be scaled to be be nicely displayable
IplImage* disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
- cvCvtScale(dbridge.toIpl(), disp, 1/4.0);
+ cvCvtScale(dbridge.toIpl(), disp, 4.0/stinfo.dpp);
cvShowImage("disparity", disp);
cvReleaseImage(&disp);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-11-19 23:26:21
|
Revision: 7014
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7014&view=rev
Author: jleibs
Date: 2008-11-19 23:26:15 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
Stereo point clouds now have color information as well.
Modified Paths:
--------------
pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
pkg/trunk/vision/stereo_view/stereo_view.cpp
Modified: pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-19 22:59:41 UTC (rev 7013)
+++ pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-19 23:26:15 UTC (rev 7014)
@@ -293,6 +293,9 @@
cloud_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
cloud_.header.frame_id = "stereo";
cloud_.pts.resize(stcam->stIm->numPts);
+ cloud_.chan.resize(1);
+ cloud_.chan[0].name = "rgb";
+ cloud_.chan[0].vals.resize(stcam->stIm->numPts);
for (int i = 0; i < stcam->stIm->numPts; i++)
{
@@ -302,6 +305,14 @@
// printf("(%d/%d) %f %f %f\n", i, stcam->stIm->numPts, cloud_.pts[i].x, cloud_.pts[i].y, cloud_.pts[i].z);
}
+ for (int i = 0; i < stcam->stIm->numPts; i++)
+ {
+ int rgb = (stcam->stIm->imPtsColor[3*i] << 16) | (stcam->stIm->imPtsColor[3*i + 1] << 8) | stcam->stIm->imPtsColor[3*i + 2];
+ cloud_.chan[0].vals[i] = *(float*)(&rgb);
+ }
+
+
+
publish("~cloud", cloud_);
}
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-19 22:59:41 UTC (rev 7013)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-19 23:26:15 UTC (rev 7014)
@@ -64,7 +64,7 @@
bool subscribe_color_;
- StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout), subscribe_color_(false)
+ StereoView() : ros::node("stereo_view"), sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout), subscribe_color_(false)
{
cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-11-20 00:43:32
|
Revision: 7020
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7020&view=rev
Author: stuglaser
Date: 2008-11-20 00:43:30 +0000 (Thu, 20 Nov 2008)
Log Message:
-----------
Added a cartesian orientation controller.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_orientation_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_orientation_controller.cpp
pkg/trunk/robot_srvs/srv/GetQuaternion.srv
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2008-11-20 00:35:46 UTC (rev 7019)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2008-11-20 00:43:30 UTC (rev 7020)
@@ -12,10 +12,11 @@
src/cartesian_velocity_controller.cpp
src/cartesian_position_controller.cpp
src/cartesian_torque_controller.cpp
+ src/cartesian_orientation_controller.cpp
src/joint_autotuner.cpp
src/joint_pd_controller.cpp
src/joint_calibration_controller.cpp
src/joint_blind_calibration_controller.cpp
src/lqr_controller.cpp
src/ros_serialchain_model.cpp
- )
\ No newline at end of file
+ )
Added: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_orientation_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_orientation_controller.h (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_orientation_controller.h 2008-11-20 00:43:30 UTC (rev 7020)
@@ -0,0 +1,112 @@
+/*
+ * 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.
+ */
+
+/*
+ * Example config:
+
+ <controller type="CartesianOrientationController" name="controller_name">
+ <chain root="root_link" tip="tip_link" />
+ </controller>
+
+ * The root is fixed, and all commands are specified in its coordinate
+ * frame.
+ *
+ * Author: Stuart Glaser
+ */
+
+#ifndef CARTESIAN_ORIENTATION_CONTROLLER_H
+#define CARTESIAN_ORIENTATION_CONTROLLER_H
+
+
+#include <vector>
+#include "ros/node.h"
+#include "robot_srvs/GetQuaternion.h"
+#include "robot_mechanism_controllers/cartesian_torque_controller.h"
+#include "control_toolbox/pid.h"
+#include "mechanism_model/controller.h"
+#include "tf/tf.h"
+#include "tf/transform_listener.h"
+#include "misc_utils/realtime_publisher.h"
+#include "misc_utils/advertised_service_guard.h"
+#include "misc_utils/subscription_guard.h"
+
+namespace controller {
+
+class CartesianOrientationController : public Controller
+{
+public:
+ CartesianOrientationController();
+ ~CartesianOrientationController();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+
+ tf::Quaternion command_;
+ void getTipOrientation(tf::Quaternion *q);
+ std::string rootFrame();
+
+private:
+ mechanism::RobotState *robot_;
+ mechanism::LinkState *tip_;
+ CartesianTorqueController torque_;
+ control_toolbox::Pid pid_x_, pid_y_, pid_z_;
+ double last_time_;
+
+ bool reset_;
+};
+
+class CartesianOrientationControllerNode : public Controller
+{
+public:
+ CartesianOrientationControllerNode();
+ ~CartesianOrientationControllerNode();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ void update();
+
+ void setCommand();
+ bool getActual(robot_srvs::GetQuaternion::request &req,
+ robot_srvs::GetQuaternion::response &resp);
+
+private:
+ mechanism::RobotState *robot_;
+ CartesianOrientationController c_;
+ AdvertisedServiceGuard guard_get_actual_;
+
+ SubscriptionGuard guard_set_command_;
+ std_msgs::QuaternionStamped command_msg_;
+
+ misc_utils::RealtimePublisher<std_msgs::QuaternionStamped> *pos_publisher_;
+ tf::TransformListener TF;
+ int loop_count_;
+};
+
+}
+
+#endif
Added: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_orientation_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_orientation_controller.cpp (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_orientation_controller.cpp 2008-11-20 00:43:30 UTC (rev 7020)
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Stuart Glaser
+ */
+
+#include "robot_mechanism_controllers/cartesian_orientation_controller.h"
+#include <algorithm>
+#include "tf/transform_datatypes.h"
+
+namespace controller {
+
+//ROS_REGISTER_CONTROLLER(CartesianOrientationController)
+
+CartesianOrientationController::CartesianOrientationController()
+: command_(0,0,0), robot_(NULL), last_time_(0), reset_(true)
+{
+}
+
+CartesianOrientationController::~CartesianOrientationController()
+{
+}
+
+bool CartesianOrientationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ robot_ = robot;
+
+ if (!torque_.initXml(robot, config))
+ return false;
+
+ tip_ = torque_.links_[torque_.links_.size() - 1];
+
+ TiXmlElement *pid_el = config->FirstChildElement("pid");
+ if (!pid_el)
+ {
+ fprintf(stderr, "Error: CartesianOrientationController requires a pid element\n");
+ return false;
+ }
+ if (!pid_x_.initXml(pid_el))
+ return false;
+ pid_y_ = pid_x_;
+ pid_z_ = pid_x_;
+
+ last_time_ = robot_->hw_->current_time_;
+
+ return true;
+}
+
+void CartesianOrientationController::update()
+{
+ for (unsigned int i = 0; i < torque_.joints_.size(); ++i)
+ {
+ if (!torque_.joints_[i]->calibrated_)
+ return;
+ }
+
+ if (reset_) {
+ reset_ = false;
+ getTipOrientation(&command_);
+ }
+
+ assert(tip_);
+ double time = robot_->hw_->current_time_;
+
+ tf::Quaternion tip_orientation;
+ getTipOrientation(&tip_orientation);
+
+ tf::Quaternion q_diff = (command_ - tip_orientation).normalized();
+ tf::Vector3 error = q_diff.getAxis() * q_diff.getAngle();
+ torque_.command_[0] = -pid_x_.updatePid(error.x(), time - last_time_);
+ torque_.command_[1] = -pid_y_.updatePid(error.y(), time - last_time_);
+ torque_.command_[2] = -pid_z_.updatePid(error.z(), time - last_time_);
+
+ torque_.update();
+
+ last_time_ = time;
+}
+
+void CartesianOrientationController::getTipOrientation(tf::Quaternion *q)
+{
+ *q = tip_->abs_orientation_;
+}
+
+std::string CartesianOrientationController::rootFrame()
+{
+ return torque_.links_[0]->link_->name_;
+}
+
+
+ROS_REGISTER_CONTROLLER(CartesianOrientationControllerNode)
+
+CartesianOrientationControllerNode::CartesianOrientationControllerNode()
+: robot_(NULL), pos_publisher_(NULL), TF(*ros::node::instance(), false) , loop_count_(0)
+{
+ assert(ros::node::instance());
+ TF.setExtrapolationLimit(ros::Duration(10.0e-3));
+}
+
+CartesianOrientationControllerNode::~CartesianOrientationControllerNode()
+{
+ if (pos_publisher_)
+ delete pos_publisher_;
+}
+
+bool CartesianOrientationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ robot_ = robot;
+ ros::node *node = ros::node::instance();
+
+ std::string topic = config->Attribute("name") ? config->Attribute("name") : "";
+ if (topic == "")
+ {
+ fprintf(stderr, "No topic given to CartesianOrientationControllerNode\n");
+ return false;
+ }
+
+ if (!c_.initXml(robot, config))
+ return false;
+
+ /*
+ node->advertise_service(topic + "/set_command",
+ &CartesianOrientationControllerNode::setCommand, this);
+ guard_set_command_.set(topic + "/set_command");
+ node->advertise_service(topic + "/get_actual",
+ &CartesianOrientationControllerNode::getActual, this);
+ guard_get_actual_.set(topic + "/get_actual");
+
+ node->subscribe(topic + "/command", command_msg_,
+ &CartesianOrientationControllerNode::command, this, 0);
+ guard_command_.set(topic + "/command");
+ */
+
+
+
+
+
+ node->subscribe(topic + "/set_command", command_msg_,
+ &CartesianOrientationControllerNode::setCommand, this, 1);
+ guard_set_command_.set(topic + "/set_command");
+
+ pos_publisher_ = new misc_utils::RealtimePublisher<std_msgs::QuaternionStamped>(topic + "/position", 1);
+
+ return true;
+}
+
+void CartesianOrientationControllerNode::update()
+{
+ if (++loop_count_ % 10 == 0)
+ {
+ if (pos_publisher_)
+ {
+ if (pos_publisher_->trylock())
+ {
+ pos_publisher_->msg_.header.stamp = robot_->hw_->current_time_;
+ pos_publisher_->msg_.header.frame_id = c_.rootFrame();
+
+ tf::Quaternion q;
+ c_.getTipOrientation(&q);
+ tf::QuaternionTFToMsg(q, pos_publisher_->msg_.quaternion);
+
+ pos_publisher_->unlockAndPublish();
+ }
+ }
+ }
+
+ c_.update();
+}
+
+void CartesianOrientationControllerNode::setCommand()
+{
+ using namespace tf;
+
+ // Transforms the command into the root frame of the chain
+ Stamped<Quaternion> quat, out;
+ QuaternionStampedMsgToTF(command_msg_, quat);
+ try
+ {
+ TF.transformQuaternion(c_.rootFrame(), quat, out);
+ c_.command_ = out;
+ }
+ catch (tf::ExtrapolationException ex)
+ {
+ fprintf(stderr, "CartesianOrientationController extrapolated too far: %s\n", ex.what());
+ }
+ catch (tf::ConnectivityException ex)
+ {
+ fprintf(stderr, "CartesianOrientationController cannot act in frame: %s\n", ex.what());
+ }
+
+}
+
+bool CartesianOrientationControllerNode::getActual(
+ robot_srvs::GetQuaternion::request &req,
+ robot_srvs::GetQuaternion::response &resp)
+{
+ tf::Quaternion q;
+ c_.getTipOrientation(&q);
+ tf::QuaternionTFToMsg(q, resp.q);
+ return true;
+}
+
+}
Added: pkg/trunk/robot_srvs/srv/GetQuaternion.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/GetQuaternion.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/GetQuaternion.srv 2008-11-20 00:43:30 UTC (rev 7020)
@@ -0,0 +1,2 @@
+---
+std_msgs/Quaternion q
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jam...@us...> - 2008-11-20 01:21:27
|
Revision: 7023
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7023&view=rev
Author: jamesbowman
Date: 2008-11-20 01:21:22 +0000 (Thu, 20 Nov 2008)
Log Message:
-----------
Moved Pose to robot_msgs/VOPose
Modified Paths:
--------------
pkg/trunk/vision/visual_odometry/manifest.xml
pkg/trunk/vision/visual_odometry/nodes/vo.py
pkg/trunk/vision/visual_odometry/src/visualodometer.py
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/VOPose.msg
Removed Paths:
-------------
pkg/trunk/vision/visual_odometry/msg/Pose.msg
Added: pkg/trunk/robot_msgs/msg/VOPose.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/VOPose.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/VOPose.msg 2008-11-20 01:21:22 UTC (rev 7023)
@@ -0,0 +1,3 @@
+Header header
+std_msgs/Pose pose
+int32 inliers
Modified: pkg/trunk/vision/visual_odometry/manifest.xml
===================================================================
--- pkg/trunk/vision/visual_odometry/manifest.xml 2008-11-20 01:16:05 UTC (rev 7022)
+++ pkg/trunk/vision/visual_odometry/manifest.xml 2008-11-20 01:21:22 UTC (rev 7023)
@@ -9,6 +9,7 @@
<depend package="rostest"/>
<depend package="rospy"/>
<depend package="std_msgs"/>
+ <depend package="robot_msgs"/>
<depend package="boost"/>
<depend package="fast_detector"/>
<depend package="star_detector"/>
Deleted: pkg/trunk/vision/visual_odometry/msg/Pose.msg
===================================================================
--- pkg/trunk/vision/visual_odometry/msg/Pose.msg 2008-11-20 01:16:05 UTC (rev 7022)
+++ pkg/trunk/vision/visual_odometry/msg/Pose.msg 2008-11-20 01:21:22 UTC (rev 7023)
@@ -1,3 +0,0 @@
-Header header
-std_msgs/Pose pose
-int32 inliers
Modified: pkg/trunk/vision/visual_odometry/nodes/vo.py
===================================================================
--- pkg/trunk/vision/visual_odometry/nodes/vo.py 2008-11-20 01:16:05 UTC (rev 7022)
+++ pkg/trunk/vision/visual_odometry/nodes/vo.py 2008-11-20 01:21:22 UTC (rev 7023)
@@ -41,11 +41,11 @@
from math import *
from std_msgs.msg import Image, ImageArray, String, VisualizationMarker
+from robot_msgs.msg import VOPose
import std_msgs.msg as stdmsg
import rospy
from stereo import DenseStereoFrame, SparseStereoFrame
from visualodometer import VisualOdometer, FeatureDetectorHarris, FeatureDetector4x4, FeatureDetectorFast
-from visual_odometry.msg import Pose
import camera
import PIL.Image
@@ -65,7 +65,7 @@
rospy.TopicSub('/videre/images', ImageArray, self.handle_array)
rospy.TopicSub('/videre/cal_params', String, self.handle_params)
- self.pub_vo = rospy.Publisher("/vo", Pose)
+ self.pub_vo = rospy.Publisher("/vo", VOPose)
self.vo = None
@@ -86,7 +86,7 @@
pose = self.vo.handle_frame(af)
print self.vo.num_frames, pose.xform(0,0,0), pose.quaternion()
- p = Pose()
+ p = VOPose()
p.inliers = self.vo.inl
p.header = iar.header
p.pose = stdmsg.Pose(stdmsg.Point(*pose.xform(0,0,0)), stdmsg.Quaternion(*pose.quaternion()))
Modified: pkg/trunk/vision/visual_odometry/src/visualodometer.py
===================================================================
--- pkg/trunk/vision/visual_odometry/src/visualodometer.py 2008-11-20 01:16:05 UTC (rev 7022)
+++ pkg/trunk/vision/visual_odometry/src/visualodometer.py 2008-11-20 01:21:22 UTC (rev 7023)
@@ -110,6 +110,9 @@
x,y,z = self.xform(0,0,0)
return sqrt(x * x + y * y + z * z)
+ def assert_sane(self):
+ print
+
import fast
class FeatureDetector:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-20 02:15:03
|
Revision: 7030
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7030&view=rev
Author: hsujohnhsu
Date: 2008-11-20 02:14:58 +0000 (Thu, 20 Nov 2008)
Log Message:
-----------
udpate demos to start with empty world.
using urdf2factory to read robotdesc/pr2 and spwan robot in gazebo.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/arm.launch
pkg/trunk/demos/pr2_gazebo/pr2.launch
pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cc
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/wg.world
Added Paths:
-----------
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/willow-walls.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/obstacle.world
Removed Paths:
-------------
pkg/trunk/robot_descriptions/gazebo_robot_description/world/tests/willow-walls.model
Modified: pkg/trunk/demos/arm_gazebo/arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-20 02:01:49 UTC (rev 7029)
+++ pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-20 02:14:58 UTC (rev 7030)
@@ -2,13 +2,18 @@
<group name="wg">
<!-- send pr2_arm.xml to param server -->
<include file="$(find wg_robot_description)/pr2_arm_test/send_description.launch" />
+
<!-- -g flag runs gazebo in gui-less mode -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_arm_test.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_arm_test/controllers_arm.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
<node pkg="robot_mechanism_controllers" type="control.py" args="set gripper_left_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
<!-- for visualization, heavy, off by default -->
Modified: pkg/trunk/demos/pr2_gazebo/pr2.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2.launch 2008-11-20 02:01:49 UTC (rev 7029)
+++ pkg/trunk/demos/pr2_gazebo/pr2.launch 2008-11-20 02:14:58 UTC (rev 7030)
@@ -5,13 +5,16 @@
<include file="$(find wg_robot_description)/pr2/send_description.launch" />
<!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_simple.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
<!-- load controllers -->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch 2008-11-20 02:01:49 UTC (rev 7029)
+++ pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch 2008-11-20 02:14:58 UTC (rev 7030)
@@ -4,12 +4,16 @@
<include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
<!-- -g flag runs gazebo in gui-less mode -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_floorobj.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/floorobj.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-11-20 02:01:49 UTC (rev 7029)
+++ pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-11-20 02:14:58 UTC (rev 7030)
@@ -4,13 +4,16 @@
<include file="$(find wg_robot_description)/pr2/send_description.launch" />
<!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_obstacle.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/obstacle.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
<!-- load controllers -->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2008-11-20 02:01:49 UTC (rev 7029)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2008-11-20 02:14:58 UTC (rev 7030)
@@ -4,13 +4,16 @@
<include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
<!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_wg.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/wg.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
<!-- load controllers -->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-20 02:01:49 UTC (rev 7029)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-20 02:14:58 UTC (rev 7030)
@@ -2,18 +2,22 @@
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
<!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
<group name="wg">
- <!-- create model file for gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded $(find gazebo_robot_description)/world/pr2_xml_prototype1.model" />
+
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
<include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_prototype1.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/wg.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
<include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
</group>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cc 2008-11-20 02:01:49 UTC (rev 7029)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cc 2008-11-20 02:14:58 UTC (rev 7030)
@@ -398,15 +398,13 @@
void usage(const char *progname)
{
- printf("\nUsage: %s URDF.xml Gazebo.model\n", progname);
- printf(" where URDF.xml is the file containing a robot description in the Willow Garage format (URDF)\n");
- printf(" and Gazebo.model is the file where the Gazebo model should be written\n\n");
- printf(" if nothing is not specified, read from param and send to gazebo factory\n\n");
+ printf("\nUsage: %s robotdesc/pr2\n", progname);
+ printf(" read robotdesc/pr2 from param server and send to gazebo factory to spawn robot\n\n");
}
int main(int argc, char **argv)
{
- if (argc < 0)
+ if (argc < 2)
{
usage(argv[0]);
exit(1);
@@ -414,33 +412,27 @@
// connect to gazebo
gazebo::Client *client = new gazebo::Client();
- gazebo::SimulationIface *simIface = new gazebo::SimulationIface();
gazebo::FactoryIface *factoryIface = new gazebo::FactoryIface();
int serverId = 0;
+ bool connected_to_server = false;
/// Connect to the libgazebo server
- try
+ while (!connected_to_server)
{
- client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
+ try
+ {
+ client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
+ connected_to_server = true;
+ }
+ catch (gazebo::GazeboError e)
+ {
+ std::cout << "Gazebo error: Unable to connect\n" << e << "\n";
+ usleep(1000000);
+ connected_to_server = false;
+ }
}
- catch (gazebo::GazeboError e)
- {
- std::cout << "Gazebo error: Unable to connect\n" << e << "\n";
- return -1;
- }
- /// Open the Simulation Interface
- try
- {
- simIface->Open(client, "default");
- }
- catch (gazebo::GazeboError e)
- {
- std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n";
- return -1;
- }
-
/// Open the Factory interface
try
{
@@ -459,7 +451,7 @@
ros::node* rosnode = new ros::node("pr2_factory",ros::node::DONT_HANDLE_SIGINT);
printf("-------------------- starting node for pr2 param server factory \n");
std::string xml_content;
- rosnode->get_param("robotdesc/pr2",xml_content);
+ rosnode->get_param(argv[1],xml_content);
// Parse URDF to get gazebo model.
bool enforce_limits = true;
Copied: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/willow-walls.model (from rev 7001, pkg/trunk/robot_descriptions/gazebo_robot_description/world/tests/willow-walls.model)
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/willow-walls.model (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/willow-walls.model 2008-11-20 02:14:58 UTC (rev 7030)
@@ -0,0 +1,11197 @@
+<?xml version="1.0"?>
+<!-- This file autogenerated by gazebo_map_extruder by parsing /u/gerkey/code/personalrobots/demos/2dnav-gazebo/world/Media/materials/textures/willowMap.png -->
+<model:physical name="willow-walls"
+xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+>
+ <xyz> 0.0 0.0 0.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+
+
+ <body:box name="wall_0_body">
+ <xyz> 4.700 25.550 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_0_geom">
+ <mesh>default</mesh>
+ <size> 34.500 0.200 2.0</size>
+ <visual>
+ <size> 34.500 0.200 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_1_body">
+ <xyz> -8.100 23.450 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_1_geom">
+ <mesh>default</mesh>
+ <size> 8.900 4.000 2.0</size>
+ <visual>
+ <size> 8.900 4.000 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_2_body">
+ <xyz> 0.200 23.450 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_2_geom">
+ <mesh>default</mesh>
+ <size> 0.500 4.000 2.0</size>
+ <visual>
+ <size> 0.500 4.000 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_3_body">
+ <xyz> 4.750 23.500 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_3_geom">
+ <mesh>default</mesh>
+ <size> 0.400 3.900 2.0</size>
+ <visual>
+ <size> 0.400 3.900 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_4_body">
+ <xyz> 8.050 23.500 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_4_geom">
+ <mesh>default</mesh>
+ <size> 0.200 3.900 2.0</size>
+ <visual>
+ <size> 0.200 3.900 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_5_body">
+ <xyz> 11.350 23.500 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_5_geom">
+ <mesh>default</mesh>
+ <size> 0.400 3.900 2.0</size>
+ <visual>
+ <size> 0.400 3.900 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_6_body">
+ <xyz> 18.900 25.400 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_6_geom">
+ <mesh>default</mesh>
+ <size> 6.100 0.100 2.0</size>
+ <visual>
+ <size> 6.100 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_7_body">
+ <xyz> 4.500 25.300 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_7_geom">
+ <mesh>default</mesh>
+ <size> 0.100 0.100 2.0</size>
+ <visual>
+ <size> 0.100 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_8_body">
+ <xyz> 16.050 23.750 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_8_geom">
+ <mesh>default</mesh>
+ <size> 0.400 3.200 2.0</size>
+ <visual>
+ <size> 0.400 3.200 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_9_body">
+ <xyz> 21.700 -0.100 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_9_geom">
+ <mesh>default</mesh>
+ <size> 0.500 50.900 2.0</size>
+ <visual>
+ <size> 0.500 50.900 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_10_body">
+ <xyz> 4.500 25.100 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_10_geom">
+ <mesh>default</mesh>
+ <size> 0.100 0.100 2.0</size>
+ <visual>
+ <size> 0.100 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_11_body">
+ <xyz> 16.100 21.850 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_11_geom">
+ <mesh>default</mesh>
+ <size> 0.300 0.600 2.0</size>
+ <visual>
+ <size> 0.300 0.600 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_12_body">
+ <xyz> -17.100 21.900 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_12_geom">
+ <mesh>default</mesh>
+ <size> 9.100 0.300 2.0</size>
+ <visual>
+ <size> 9.100 0.300 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_13_body">
+ <xyz> 11.650 21.750 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_13_geom">
+ <mesh>default</mesh>
+ <size> 0.200 0.400 2.0</size>
+ <visual>
+ <size> 0.200 0.400 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_14_body">
+ <xyz> 6.000 21.700 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_14_geom">
+ <mesh>default</mesh>
+ <size> 2.100 0.300 2.0</size>
+ <visual>
+ <size> 2.100 0.300 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_15_body">
+ <xyz> 7.850 21.750 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_15_geom">
+ <mesh>default</mesh>
+ <size> 0.200 0.200 2.0</size>
+ <visual>
+ <size> 0.200 0.200 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_16_body">
+ <xyz> 8.250 21.750 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_16_geom">
+ <mesh>default</mesh>
+ <size> 0.200 0.200 2.0</size>
+ <visual>
+ <size> 0.200 0.200 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_17_body">
+ <xyz> 10.150 21.700 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_17_geom">
+ <mesh>default</mesh>
+ <size> 2.000 0.300 2.0</size>
+ <visual>
+ <size> 2.000 0.300 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_18_body">
+ <xyz> 11.800 21.800 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_18_geom">
+ <mesh>default</mesh>
+ <size> 0.100 0.100 2.0</size>
+ <visual>
+ <size> 0.100 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_19_body">
+ <xyz> 14.300 21.700 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_19_geom">
+ <mesh>default</mesh>
+ <size> 3.300 0.300 2.0</size>
+ <visual>
+ <size> 3.300 0.300 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_20_body">
+ <xyz> 19.250 21.750 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_20_geom">
+ <mesh>default</mesh>
+ <size> 4.400 0.200 2.0</size>
+ <visual>
+ <size> 4.400 0.200 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_21_body">
+ <xyz> -21.450 17.200 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_21_geom">
+ <mesh>default</mesh>
+ <size> 0.400 9.100 2.0</size>
+ <visual>
+ <size> 0.400 9.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_22_body">
+ <xyz> -15.950 20.100 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_22_geom">
+ <mesh>default</mesh>
+ <size> 0.200 3.300 2.0</size>
+ <visual>
+ <size> 0.200 3.300 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_23_body">
+ <xyz> -13.200 20.100 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_23_geom">
+ <mesh>default</mesh>
+ <size> 1.300 3.300 2.0</size>
+ <visual>
+ <size> 1.300 3.300 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_24_body">
+ <xyz> -2.350 21.600 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_24_geom">
+ <mesh>default</mesh>
+ <size> 2.600 0.300 2.0</size>
+ <visual>
+ <size> 2.600 0.300 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_25_body">
+ <xyz> -0.150 21.600 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_25_geom">
+ <mesh>default</mesh>
+ <size> 0.200 0.300 2.0</size>
+ <visual>
+ <size> 0.200 0.300 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_26_body">
+ <xyz> 2.200 21.700 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_26_geom">
+ <mesh>default</mesh>
+ <size> 3.300 0.100 2.0</size>
+ <visual>
+ <size> 3.300 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_27_body">
+ <xyz> 0.550 21.600 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_27_geom">
+ <mesh>default</mesh>
+ <size> 0.200 0.100 2.0</size>
+ <visual>
+ <size> 0.200 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_28_body">
+ <xyz> 1.300 21.600 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_28_geom">
+ <mesh>default</mesh>
+ <size> 0.500 0.100 2.0</size>
+ <visual>
+ <size> 0.500 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_29_body">
+ <xyz> 1.700 21.600 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_29_geom">
+ <mesh>default</mesh>
+ <size> 0.100 0.100 2.0</size>
+ <visual>
+ <size> 0.100 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_30_body">
+ <xyz> 2.850 21.600 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_30_geom">
+ <mesh>default</mesh>
+ <size> 2.000 0.100 2.0</size>
+ <visual>
+ <size> 2.000 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_31_body">
+ <xyz> 7.900 21.600 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_31_geom">
+ <mesh>default</mesh>
+ <size> 0.100 0.100 2.0</size>
+ <visual>
+ <size> 0.100 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_32_body">
+ <xyz> 8.200 21.600 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_32_geom">
+ <mesh>default</mesh>
+ <size> 0.100 0.100 2.0</size>
+ <visual>
+ <size> 0.100 0.100 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+
+ <body:box name="wall_33_body">
+ <xyz> 17.250 21.600 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <geom:box name="wall_33_geom">
+ <mesh>default</mesh>
+ <size> 0.400 0.100 2.0</size>
+ <visual>
+ <size> 0.400 0.100 2.0</size>
+ <material>G...
[truncated message content] |
|
From: <stu...@us...> - 2008-11-20 05:51:17
|
Revision: 7047
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7047&view=rev
Author: stuglaser
Date: 2008-11-20 05:51:07 +0000 (Thu, 20 Nov 2008)
Log Message:
-----------
effect.py quickly brings up an effort controller on any joint, allowing you to command the joint easily
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp
pkg/trunk/mechanism/mechanism_control/scripts/spawner.py
Added Paths:
-----------
pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py
Copied: pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py (from rev 7046, pkg/trunk/mechanism/mechanism_control/scripts/spawner.py)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py 2008-11-20 05:51:07 UTC (rev 7047)
@@ -0,0 +1,81 @@
+#! /usr/bin/python
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright
+# 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.
+
+# This script brings up an effort controller on your joint of choice
+# and allows you to type in the desired efforts.
+#
+# Author: Stuart Glaser
+
+CONTROLLER_NAME = 'quick_effort_controller'
+
+import sys
+
+import rostools
+rostools.update_path('robot_mechanism_controllers')
+rostools.update_path('mechanism_control')
+import rospy
+from std_msgs.msg import *
+from mechanism_control import mechanism
+from robot_srvs.srv import SpawnController, KillController
+
+
+def xml_for(joint):
+ return "\
+<controller name=\"%s\" type=\"JointEffortControllerNode\">\
+<joint name=\"%s\" />\
+</controller>" % (CONTROLLER_NAME, joint)
+
+def main():
+ if len(sys.argv) < 2:
+ print "Usage: effect.py <joint>"
+ sys.exit(1)
+ joint = sys.argv[1]
+
+ rospy.init_node('effect', anonymous=True)
+ rospy.wait_for_service('spawn_controller')
+ spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
+ kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+
+ resp = spawn_controller(xml_for(joint))
+ if len(resp.ok) < 1 or not ord(resp.ok[0]):
+ print "Failed to spawn effort controller"
+ sys.exit(1)
+
+ pub = rospy.Publisher("/%s/set_command" % CONTROLLER_NAME, Float64)
+
+ try:
+ print "Enter efforts:"
+ while not rospy.is_shutdown():
+ effort = float(sys.stdin.readline().strip())
+ pub.publish(Float64(effort))
+ finally:
+ kill_controller(CONTROLLER_NAME)
+
+
+if __name__ == '__main__':
+ main()
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp 2008-11-20 05:10:12 UTC (rev 7046)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp 2008-11-20 05:51:07 UTC (rev 7047)
@@ -69,7 +69,7 @@
TiXmlElement *j = config->FirstChildElement("joint");
if (!j)
{
- fprintf(stderr, "JointPositionController was not given a joint\n");
+ fprintf(stderr, "JointEffortController was not given a joint\n");
return false;
}
Modified: pkg/trunk/mechanism/mechanism_control/scripts/spawner.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/spawner.py 2008-11-20 05:10:12 UTC (rev 7046)
+++ pkg/trunk/mechanism/mechanism_control/scripts/spawner.py 2008-11-20 05:51:07 UTC (rev 7047)
@@ -60,7 +60,7 @@
spawned = []
for i in range(len(resp.ok)):
- if resp.ok[i]:
+ if ord(resp.ok[i]):
spawned.append(resp.name[i])
else:
print "Failed to spawn %s" % resp.name[i]
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-20 18:19:53
|
Revision: 7062
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7062&view=rev
Author: hsujohnhsu
Date: 2008-11-20 18:19:49 +0000 (Thu, 20 Nov 2008)
Log Message:
-----------
update examples to use urdf2factory.
Modified Paths:
--------------
pkg/trunk/demos/examples_gazebo/dual_link.xml
pkg/trunk/demos/examples_gazebo/multi_link.xml
pkg/trunk/demos/examples_gazebo/single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/multi_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/single_link.xml
Modified: pkg/trunk/demos/examples_gazebo/dual_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-20 18:16:57 UTC (rev 7061)
+++ pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-20 18:19:49 UTC (rev 7062)
@@ -4,13 +4,16 @@
<include file="$(find wg_robot_description)/dual_link_test/send_description.launch" />
<!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_dual_link.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
</group>
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
<!--node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/dual_link_test/controllers_dual_link.xml" respawn="false" output="screen" /--> <!-- load default arm controller -->
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
</launch>
Modified: pkg/trunk/demos/examples_gazebo/multi_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-20 18:16:57 UTC (rev 7061)
+++ pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-20 18:19:49 UTC (rev 7062)
@@ -4,13 +4,16 @@
<include file="$(find wg_robot_description)/multi_link_test/send_description.launch" />
<!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_multi_link.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
</group>
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
<!--node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/multi_link_test/controllers_multi_link.xml" respawn="false" output="screen" /--> <!-- load default arm controller -->
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
</launch>
Modified: pkg/trunk/demos/examples_gazebo/single_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-20 18:16:57 UTC (rev 7061)
+++ pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-20 18:19:49 UTC (rev 7062)
@@ -4,13 +4,16 @@
<include file="$(find wg_robot_description)/single_link_test/send_description.launch" />
<!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_single_link.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
</group>
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/single_link_test/controllers_single_link.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
<node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
</launch>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/dual_link.xml 2008-11-20 18:16:57 UTC (rev 7061)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/dual_link.xml 2008-11-20 18:19:49 UTC (rev 7062)
@@ -126,4 +126,44 @@
<!-- mechanism controls -->
<include filename="./transmissions_dual_link.xml" />
+ <map name="sensor" flag="gazebo">
+ <verbatim key="mechanism_control_simulation">
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
+ <!-- pr2_actarray -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwayson>true</alwayson>
+ <updaterate>1000.0</updaterate>
+
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_link1_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>link1</bodyName>
+ <topicName>link1_pose</topicName>
+ <frameName>map</frameName>
+ <xyzOffsets>0 0 0.5</xyzOffsets> <!-- at pivot of link2 -->
+ <rpyOffsets>0 0 0.0</rpyOffsets>
+ <interface:position name="p3d_link1_position"/>
+ </controller:P3D>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_link2_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>link2</bodyName>
+ <topicName>link2_pose</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_link2_position"/>
+ </controller:P3D>
+
+ </verbatim>
+ </map>
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/multi_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/multi_link.xml 2008-11-20 18:16:57 UTC (rev 7061)
+++ pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/multi_link.xml 2008-11-20 18:19:49 UTC (rev 7062)
@@ -326,5 +326,33 @@
<include filename="./groups_multi_link.xml" />
<!-- mechanism controls -->
<include filename="./transmissions_multi_link.xml" />
+
+ <map name="sensor" flag="gazebo">
+ <verbatim key="mechanism_control_simulation">
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_link3_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>link3</bodyName>
+ <topicName>link3_pose</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_link3_position"/>
+ </controller:P3D>
+ </verbatim>
+ </map>
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/single_link.xml 2008-11-20 18:16:57 UTC (rev 7061)
+++ pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/single_link.xml 2008-11-20 18:19:49 UTC (rev 7062)
@@ -80,6 +80,33 @@
<elem key="turnGravityOff">true</elem>
</map>
</link>
+ <map name="sensor" flag="gazebo">
+ <verbatim key="mechanism_control_simulation">
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_link1_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bodyName>link1</bodyName>
+ <topicName>link1_pose</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_link1_position"/>
+ </controller:P3D>
+ </verbatim>
+ </map>
<!-- Define groups of links; a link may be part of multiple groups -->
<include filename="./groups_single_link.xml" />
<!-- mechanism controls -->
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mm...@us...> - 2008-11-20 22:22:45
|
Revision: 7082
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7082&view=rev
Author: mmwise
Date: 2008-11-20 22:22:40 +0000 (Thu, 20 Nov 2008)
Log Message:
-----------
Grand renaming of joints, motors, transmissions, etc...
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/actuators.conf
pkg/trunk/robot_descriptions/wg_robot_description/arm/arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/head_pan_tilt/head_pan_tilt.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/cal.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xacro.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/lqr_control_leftarm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xacro.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/transmissions_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/cal.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_base_lab.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/laser_controller_config.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/pr2_prototype1.xacro.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/robo_dev_head/cal.xml
pkg/trunk/robot_descriptions/wg_robot_description/robo_dev_head/robo_dev_head.xml
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/build_snapshots.launch
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/cal.xml
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/calibrate.launch
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/control_tilt_laser_roslaunch.xml
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/tilt_laser.xml
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/tilt_laser_controller.xml
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser/tilt_laser_traj_controller.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/init.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2/transmissions.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm/
Modified: pkg/trunk/drivers/motor/ethercat_hardware/actuators.conf
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/actuators.conf 2008-11-20 22:20:53 UTC (rev 7081)
+++ pkg/trunk/drivers/motor/ethercat_hardware/actuators.conf 2008-11-20 22:22:40 UTC (rev 7082)
@@ -83,49 +83,53 @@
<actuators>
<!-- Casters -->
- <actuator name="caster_front_left_motor" motor="236672"/>
- <actuator name="wheel_front_left_l_motor" motor="236672"/>
- <actuator name="wheel_front_left_r_motor" motor="236672"/>
- <actuator name="caster_front_right_motor" motor="236672"/>
- <actuator name="wheel_front_right_l_motor" motor="236672"/>
- <actuator name="wheel_front_right_r_motor" motor="236672"/>
- <actuator name="caster_rear_left_motor" motor="236672"/>
- <actuator name="wheel_rear_left_l_motor" motor="236672"/>
- <actuator name="wheel_rear_left_r_motor" motor="236672"/>
- <actuator name="caster_rear_right_motor" motor="236672"/>
- <actuator name="wheel_rear_right_l_motor" motor="236672"/>
- <actuator name="wheel_rear_right_r_motor" motor="236672"/>
+ <actuator name="fl_caster_rotation_motor" motor="236672"/>
+ <actuator name="fl_caster_l_wheel_motor" motor="236672"/>
+ <actuator name="fl_caster_r_wheel_motor" motor="236672"/>
+
+ <actuator name="fr_caster_rotation_motor" motor="236672"/>
+ <actuator name="fr_caster_l_wheel_motor" motor="236672"/>
+ <actuator name="fr_caster_r_wheel_motor" motor="236672"/>
+
+ <actuator name="bl_caster_rotation_motor" motor="236672"/>
+ <actuator name="bl_caster_l_wheel_motor" motor="236672"/>
+ <actuator name="bl_caster_r_wheel_motor" motor="236672"/>
+
+ <actuator name="br_caster_rotation_motor" motor="236672"/>
+ <actuator name="br_caster_l_wheel_motor" motor="236672"/>
+ <actuator name="br_caster_r_wheel_motor" motor="236672"/>
+
<!-- Spine -->
- <actuator name="torso_motor" motor="148877"/>
+ <actuator name="torso_lift_motor" motor="148877"/>
<!-- Arm -->
- <actuator name="left_shoulder_pan_motor" motor="148877"/>
- <actuator name="left_shoulder_pitch_motor" motor="148877"/>
- <actuator name="left_upper_arm_roll_motor" motor="148877"/>
- <actuator name="left_elbow_flex_motor" motor="148877"/>
- <actuator name="left_forearm_roll_motor" motor="310007"/>
+ <actuator name="l_shoulder_pan_motor" motor="148877"/>
+ <actuator name="l_shoulder_lift_motor" motor="148877"/>
+ <actuator name="l_upper_arm_roll_motor" motor="148877"/>
+ <actuator name="l_elbow_flex_motor" motor="148877"/>
+ <actuator name="l_forearm_roll_motor" motor="310007"/>
- <actuator name="right_shoulder_pan_motor" motor="148877"/>
- <actuator name="right_shoulder_pitch_motor" motor="148877"/>
- <actuator name="right_upper_arm_roll_motor" motor="148877"/>
- <actuator name="right_elbow_flex_motor" motor="148877"/>
- <actuator name="right_forearm_roll_motor" motor="310007"/>
+ <actuator name="r_shoulder_pan_motor" motor="148877"/>
+ <actuator name="r_shoulder_lift_motor" motor="148877"/>
+ <actuator name="r_upper_arm_roll_motor" motor="148877"/>
+ <actuator name="r_elbow_flex_motor" motor="148877"/>
+ <actuator name="r_forearm_roll_motor" motor="310007"/>
<!-- Wrist -->
- <actuator name="left_wrist_l_motor" motor="310007"/>
- <actuator name="left_wrist_r_motor" motor="310007"/>
+ <actuator name="l_wrist_l_motor" motor="310007"/>
+ <actuator name="l_wrist_r_motor" motor="310007"/>
- <actuator name="right_wrist_l_motor" motor="310007"/>
- <actuator name="right_wrist_r_motor" motor="310007"/>
+ <actuator name="r_wrist_l_motor" motor="310007"/>
+ <actuator name="r_wrist_r_motor" motor="310007"/>
<!-- Gripper -->
- <actuator name="left_gripper_motor" motor="222057"/>
- <actuator name="right_gripper_motor" motor="222057"/>
+ <actuator name="l_gripper_motor" motor="222057"/>
+ <actuator name="r_gripper_motor" motor="222057"/>
<!-- Head -->
<actuator name="head_pan_motor" motor="310009-pan"/>
<actuator name="head_tilt_motor" motor="310009-tilt"/>
- <actuator name="tilt_laser_motor" motor="310009"/>
+ <actuator name="laser_tilt_motor" motor="310009"/>
</actuators>
</configuration>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/arm/arm.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/arm/arm.xml 2008-11-20 22:20:53 UTC (rev 7081)
+++ pkg/trunk/robot_descriptions/wg_robot_description/arm/arm.xml 2008-11-20 22:22:40 UTC (rev 7082)
@@ -17,7 +17,7 @@
<include filename="../pr2_robot_defs/arm_defs.xml" />
<joint name="base_joint" type="planar" />
- <link name="torso">
+ <link name="torso_lift">
<parent name="world" />
<joint name="base_joint" />
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -45,10 +45,10 @@
</collision>
</link>
- <pr2_arm side="right" reflect="-1" parent="torso">
+ <pr2_arm side="right" reflect="-1" parent="torso_lift">
<origin xyz="0 0 0.7" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="right" parent="right_wrist_roll" />
+ <pr2_gripper side="right" parent="r_wrist_roll" />
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/head_pan_tilt/head_pan_tilt.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/head_pan_tilt/head_pan_tilt.xml 2008-11-20 22:20:53 UTC (rev 7081)
+++ pkg/trunk/robot_descriptions/wg_robot_description/head_pan_tilt/head_pan_tilt.xml 2008-11-20 22:22:40 UTC (rev 7082)
@@ -3,11 +3,11 @@
<include filename="../pr2_robot_defs/head_defs.xml" />
<include filename="../pr2_robot_defs/body_defs.xml" />
- <pr2_torso name="torso" parent="world">
+ <pr2_torso name="torso_lift" parent="world">
<origin xyz="0 0 0" rpy="0 0 0" />
</pr2_torso>
- <pr2_head_pan name="head_pan" parent="torso">
+ <pr2_head_pan name="head_pan" parent="torso_lift">
<origin xyz="0 0 0.3815" rpy="0 0 0" />
</pr2_head_pan>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/cal.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/cal.xml 2008-11-20 22:20:53 UTC (rev 7081)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/cal.xml 2008-11-20 22:22:40 UTC (rev 7082)
@@ -4,14 +4,14 @@
<include filename="../pr2_robot_defs/head_defs.xml" />
<include filename="../pr2_robot_defs/body_defs.xml" />
- <upper_arm_calibrator side="right" />
- <forearm_calibrator side="right" />
- <gripper_calibrator side="right" />
+ <upper_arm_calibrator side="r" />
+ <forearm_calibrator side="r" />
+ <gripper_calibrator side="r" />
<!--
- <upper_arm_calibrator side="left" />
- <forearm_calibrator side="left" />
- <gripper_calibrator side="left" />
+ <upper_arm_calibrator side="l" />
+ <forearm_calibrator side="l" />
+ <gripper_calibrator side="l" />
-->
<base_calibrator />
@@ -20,6 +20,6 @@
<torso_calibrator />
- <tilting_laser_calibrator name="tilt_laser" />
+ <tilting_laser_calibrator name="laser_tilt" />
</controllers>
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-11-20 22:20:53 UTC (rev 7081)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-11-20 22:22:40 UTC (rev 7082)
@@ -1,229 +0,0 @@
-<?xml version="1.0"?>
-
-<controllers>
-
- <controller name="base_controller" topic="base_controller" type="BaseControllerNode">
- <map name="velocity_control" flag="base_control">
- <elem key="kp_speed">100.0</elem>
- <elem key="kp_caster_steer">0</elem>
- <elem key="timeout">3</elem>
-
- <elem key="max_x_dot">1</elem>
- <elem key="max_y_dot">1</elem>
- <elem key="max_yaw_dot">1</elem>
-
- <elem key="max_x_accel">2.0</elem>
- <elem key="max_y_accel">2.0</elem>
- <elem key="max_yaw_accel">1.0</elem>
-
- </map>
- <controller name="wheel_front_left_l_controller" topic="wheel_front_left_l_controller" type="JointVelocityControllerNode">
- <joint name="wheel_front_left_l_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
- </joint>
- </controller>
- <controller name="wheel_front_left_r_controller" topic="wheel_front_left_r_controller" type="JointVelocityControllerNode">
- <joint name="wheel_front_left_r_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
- </joint>
- </controller>
- <controller name="wheel_front_right_l_controller" topic="wheel_front_right_l_controller" type="JointVelocityControllerNode">
- <joint name="wheel_front_right_l_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
- </joint>
- </controller>
- <controller name="wheel_front_right_r_controller" topic="wheel_front_right_r_controller" type="JointVelocityControllerNode">
- <joint name="wheel_front_right_r_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
- </joint>
- </controller>
- <controller name="wheel_rear_left_l_controller" topic="wheel_rear_left_l_controller" type="JointVelocityControllerNode">
- <joint name="wheel_rear_left_l_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
- </joint>
- </controller>
- <controller name="wheel_rear_left_r_controller" topic="wheel_rear_left_r_controller" type="JointVelocityControllerNode">
- <joint name="wheel_rear_left_r_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
- </joint>
- </controller>
- <controller name="wheel_rear_right_l_controller" topic="wheel_rear_right_l_controller" type="JointVelocityControllerNode">
- <joint name="wheel_rear_right_l_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
- </joint>
- </controller>
- <controller name="wheel_rear_right_r_controller" topic="wheel_rear_right_r_controller" type="JointVelocityControllerNode">
- <joint name="wheel_rear_right_r_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
- </joint>
- </controller>
-
- <controller name="caster_front_left_controller" topic="caster_front_left_controller" type="JointVelocityControllerNode">
- <joint name="caster_front_left_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
- </joint>
- </controller>
- <controller name="caster_front_right_controller" topic="caster_front_right_controller" type="JointVelocityControllerNode">
- <joint name="caster_front_right_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
- </joint>
- </controller>
- <controller name="caster_rear_left_controller" topic="caster_rear_left_controller" type="JointVelocityControllerNode">
- <joint name="caster_rear_left_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
- </joint>
- </controller>
- <controller name="caster_rear_right_controller" topic="caster_rear_right_controller" type="JointVelocityControllerNode">
- <joint name="caster_rear_right_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
- </joint>
- </controller>
-
-
- </controller>
-
- <!-- ========================================= -->
- <!-- torso array -->
- <controller name="torso_controller" topic="torso_controller" type="JointPositionControllerNode">
- <joint name="torso_joint">
- <pid p="1000" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
- <!-- ========================================= -->
- <!-- left arm array -->
- <controller name="left_arm_controller" type="ArmPositionControllerNode">
- <listen_topic name="left_arm_commands" />
- <kinematics>
- <elem key="kdl_chain_name">left_arm</elem>
- </kinematics>
- <map name="controller_param">
- <elem key="kdl_chain_name">left_arm</elem>
- </map>
- <controller name="left_shoulder_pan_controller" topic="left_shoulder_pan_controller" type="JointPositionController">
- <joint name="left_shoulder_pan_joint" >
- <pid p="100" d="200" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="left_shoulder_pitch_controller" topic="left_shoulder_pitch_controller" type="JointPositionController">
- <joint name="left_shoulder_pitch_joint" >
- <pid p="100" d="100.0" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="left_upper_arm_roll_controller" topic="left_upper_arm_roll_controller" type="JointPositionController">
- <joint name="left_upper_arm_roll_joint" >
- <pid p="400" d="400" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="left_elbow_flex_controller" topic="left_elbow_flex_controller" type="JointPositionController">
- <joint name="left_elbow_flex_joint" >
- <pid p="100" d="100" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="left_forearm_roll_controller" topic="left_forearm_roll_controller" type="JointPositionController">
- <joint name="left_forearm_roll_joint" >
- <pid p="200" d="200" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="left_wrist_flex_controller" topic="left_wrist_flex_controller" type="JointPositionController">
- <joint name="left_wrist_flex_joint" >
- <pid p="100" d="100" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="left_wrist_roll_controller" topic="left_wrist_roll_controller" type="JointPositionController">
- <joint name="left_wrist_roll_joint" >
- <pid p="100" d="100" i="0.1" iClamp="0" />
- </joint>
- </controller>
- </controller>
- <!-- Special gripper joint -->
- <controller name="gripper_left_controller" topic="gripper_left_controller" type="JointPositionControllerNode">
- <joint name="left_gripper_l_finger_joint">
- <pid p="1.0" d="0.00" i="0.00" iClamp="0.00" />
- </joint>
- </controller>
- <!-- ========================================= -->
- <!-- right arm array -->
- <controller name="right_arm_controller" type="ArmPositionControllerNode">
- <listen_topic name="right_arm_commands" />
- <kinematics>
- <elem key="kdl_chain_name">right_arm</elem>
- </kinematics>
- <map name="controller_param">
- <elem key="kdl_chain_name">right_arm</elem>
- </map>
- <controller name="right_shoulder_pan_controller" topic="right_shoulder_pan_controller" type="JointPositionController">
- <joint name="right_shoulder_pan_joint" >
- <pid p="100" d="200" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="right_shoulder_pitch_controller" topic="right_shoulder_pitch_controller" type="JointPositionController">
- <joint name="right_shoulder_pitch_joint" >
- <pid p="100" d="100.0" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="right_upper_arm_roll_controller" topic="right_upper_arm_roll_controller" type="JointPositionController">
- <joint name="right_upper_arm_roll_joint" >
- <pid p="400" d="400" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="right_elbow_flex_controller" topic="right_elbow_flex_controller" type="JointPositionController">
- <joint name="right_elbow_flex_joint" >
- <pid p="100" d="100" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="right_forearm_roll_controller" topic="right_forearm_roll_controller" type="JointPositionController">
- <joint name="right_forearm_roll_joint" >
- <pid p="200" d="200" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="right_wrist_flex_controller" topic="right_wrist_flex_controller" type="JointPositionController">
- <joint name="right_wrist_flex_joint" >
- <pid p="100" d="100" i="0.1" iClamp="1" />
- </joint>
- </controller>
- <controller name="right_wrist_roll_controller" topic="right_wrist_roll_controller" type="JointPositionController">
- <joint name="right_wrist_roll_joint" >
- <pid p="100" d="100" i="0.1" iClamp="0" />
- </joint>
- </controller>
- </controller>
- <!-- Special gripper joint -->
- <controller name="gripper_right_controller" topic="gripper_right_controller" type="JointPositionControllerNode">
- <joint name="right_gripper_l_finger_joint">
- <pid p="1.0" d="0.00" i="0.00" iClamp="0.00" />
- </joint>
- </controller>
- <!-- head and above array -->
- <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
- <joint name="head_pan_joint" >
- <pid p="100" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
- <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
- <joint name="head_tilt_joint" >
- <pid p="100" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
-
- <controller name="tilt_laser_controller" topic="tilt_laser_controller" type="LaserScannerControllerNode">
- <joint name="tilt_laser_mount_joint" >
- <pid p="12" i=".1" d="1" iClamp="0.5" />
- </joint>
- </controller>
-
- <!-- this version of laser scanner controller seems to be broken
- <controller name="tilt_laser_controller" topic="laser_test" type="LaserScannerControllerNode">
- <velocity>
- <velocityFilter smoothing="0.2"/>
- <joint name="tilt_laser_mount_joint" type="velocity">
- <pid p="0.56" i = "14" d = "0.000001" iClamp = "0.02" />
- </joint>
- </velocity>
- <position>
- <joint name="single_joint" type="position">
- <pid p="4.4382" i = "31.2110" d = "0.0453" iClamp = "0.004" />
- </joint>
- </position>
- </controller>
- -->
-</controllers>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-11-20 22:20:53 UTC (rev 7081)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-11-20 22:22:40 UTC (rev 7082)
@@ -32,13 +32,13 @@
</group>
<group name="left_arm" flags="planning kinematic">
- left_shoulder_pan
- left_shoulder_pitch
- left_upper_arm_roll
- left_elbow_flex
- left_forearm_roll
- left_wrist_flex
- left_wrist_roll
+ l_shoulder_pan
+ l_shoulder_lift
+ l_upper_arm_roll
+ l_elbow_flex
+ l_forearm_roll
+ l_wrist_flex
+ l_wrist_roll
<map name="RRT" flag="planning">
<elem key="range" value="0.75" />
@@ -56,13 +56,13 @@
<group name="right_arm" flags="planning kinematic">
- right_shoulder_pan
- right_shoulder_pitch
- right_upper_arm_roll
- right_elbow_flex
- right_forearm_roll
- right_wrist_flex
- right_wrist_roll
+ r_shoulder_pan
+ r_shoulder_lift
+ r_upper_arm_roll
+ r_elbow_flex
+ r_forearm_roll
+ r_wrist_flex
+ r_wrist_roll
<map name="RRT" flag="planning">
<elem key="range" value="0.75" />
@@ -82,25 +82,25 @@
<group name="base_left_arm" flags="planning">
base
torso
- left_shoulder_pan
- left_shoulder_pitch
- left_upper_arm_roll
- left_elbow_flex
- left_forearm_roll
- left_wrist_flex
- left_wrist_roll
+ l_shoulder_pan
+ l_shoulder_lift
+ l_upper_arm_roll
+ l_elbow_flex
+ l_forearm_roll
+ l_wrist_flex
+ l_wrist_roll
</group>
<group name="base_right_arm" flags="planning">
base
torso
- right_shoulder_pan
- right_shoulder_pitch
- right_upper_arm_roll
- right_elbow_flex
- right_forearm_roll
- right_wrist_flex
- right_wrist_roll
+ r_shoulder_pan
+ r_shoulder_lift
+ r_upper_arm_roll
+ r_elbow_flex
+ r_forearm_roll
+ r_wrist_flex
+ r_wrist_roll
<map name="RRT" flag="planning">
<elem key="range" value="0.75" />
@@ -118,39 +118,39 @@
</group>
<group name="arms" flags="planning">
- left_shoulder_pan
- left_shoulder_pitch
- left_upper_arm_roll
- left_elbow_flex
- left_forearm_roll
- left_wrist_flex
- left_wrist_roll
- right_shoulder_pan
- right_shoulder_pitch
- right_upper_arm_roll
- right_elbow_flex
- right_forearm_roll
- right_wrist_flex
- right_wrist_roll
+ l_shoulder_pan
+ l_shoulder_lift
+ l_upper_arm_roll
+ l_elbow_flex
+ l_forearm_roll
+ l_wrist_flex
+ l_wrist_roll
+ r_shoulder_pan
+ r_shoulder_lift
+ r_upper_arm_roll
+ r_elbow_flex
+ r_forearm_roll
+ r_wrist_flex
+ r_wrist_roll
</group>
<group name="base_arms" flags="planning">
base
torso
- left_shoulder_pan
- left_shoulder_pitch
- left_upper_arm_roll
- left_elbow_flex
- left_forearm_roll
- left_wrist_flex
- left_wrist_roll
- right_shoulder_pan
- right_shoulder_pitch
- right_upper_arm_roll
- right_elbow_flex
- right_forearm_roll
- right_wrist_flex
- right_wrist_roll
+ l_shoulder_pan
+ l_shoulder_lift
+ l_upper_arm_roll
+ l_elbow_flex
+ l_forearm_roll
+ l_wrist_flex
+ l_wrist_roll
+ r_shoulder_pan
+ r_shoulder_lift
+ r_upper_arm_roll
+ r_elbow_flex
+ r_forearm_roll
+ r_wrist_flex
+ r_wrist_roll
</group>
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml 2008-11-20 22:20:53 UTC (rev 7081)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml 2008-11-20 22:22:40 UTC (rev 7082)
@@ -5,24 +5,24 @@
<!-- Define the parts of the robot that the robot's scanners can see -->
<group name="self_see" flags="collision">
- left_upper_arm_roll
- right_upper_arm_roll
- left_elbow_flex
- right_elbow_flex
- left_forearm_roll
- right_forearm_roll
- left_wrist_flex
- right_wrist_flex
- left_wrist_roll
- right_wrist_roll
- left_gripper_l_finger
- left_gripper_r_finger
- right_gripper_l_finger
- right_gripper_r_finger
- right_shoulder_pan
- right_shoulder_pitch
- left_shoulder_pan
- left_shoulder_pitch
+ l_upper_arm_roll
+ r_upper_arm_roll
+ l_elbow_flex
+ r_elbow_flex
+ l_forearm_roll
+ r_forearm_roll
+ l_wrist_flex
+ r_wrist_flex
+ l_wrist_roll
+ r_wrist_roll
+ l_gripper_l_finger
+ l_gripper_r_finger
+ r_gripper_l_finger
+ r_gripper_r_finger
+ r_shoulder_pan
+ r_shoulder_lift
+ l_shoulder_pan
+ l_shoulder_lift
torso
base
</group>
@@ -31,24 +31,24 @@
<group name="collision_check" flags="collision">
base
torso
- right_shoulder_pan
- right_shoulder_pitch
- left_shoulder_pan
- left_shoulder_pitch
- left_upper_arm_roll
- right_upper_arm_roll
- left_elbow_flex
- right_elbow_flex
- left_forearm_roll
- right_forearm_roll
- left_wrist_flex
- right_wrist_flex
- left_wrist_roll
- right_wrist_roll
- left_gripper_l_finger
- left_gripper_r_finger
- right_gripper_l_finger
- right_gripper_r_finger
+ r_shoulder_pan
+ r_shoulder_lift
+ l_shoulder_pan
+ l_shoulder_lift
+ l_upper_arm_roll
+ r_upper_arm_roll
+ l_elbow_flex
+ r_elbow_flex
+ l_forearm_roll
+ r_forearm_roll
+ l_wrist_flex
+ r_wrist_flex
+ l_wrist_roll
+ r_wrist_roll
+ l_gripper_l_finger
+ l_gripper_r_finger
+ r_gripper_l_finger
+ r_gripper_r_finger
head_pan
head_tilt
</group>
@@ -56,52 +56,52 @@
<!-- Define groups to check for self collision -->
<group name="collisions1" flags="self_collision">
- right_upper_arm_roll
+ r_upper_arm_roll
torso
</group>
<group name="collisions2" flags="self_collision">
- right_elbow_flex
+ r_elbow_flex
base
</group>
<group name="collisions3" flags="self_collision">
- right_forearm_roll
+ r_forearm_roll
base
</group>
<group name="collisions4" flags="self_collision">
- right_wrist_flex
+ r_wrist_flex
base
</group>
<group name="collisions5" flags="self_collision">
- right_wrist_roll
+ r_wrist_roll
base
</group>
<group name="collisions6" flags="self_collision">
- left_upper_arm_roll
+ l_upper_arm_roll
torso
</group>
<group name="collisions7" flags="self_collision">
- left_elbow_flex
+ l_elbow_flex
base
</group>
<group name="collisions8" flags="self_collision">
- left_forearm_roll
+ l_forearm_roll
base
</group>
<group name="collisions9" flags="self_collision">
- left_wrist_flex
+ l_wrist_flex
base
</group>
<group name="collisions10" flags="self_collision">
- left_wrist_roll
+ l_wrist_roll
base
</group>
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/pr2/init.launch
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/init.launch 2008-11-20 22:20:53 UTC (rev 7081)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/init.launch 2008-11-20 22:22:40 UTC (rev 7082)
@@ -1,9 +0,0 @@
-<launch>
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
- <include file="$(find wg_robot_description)/pr2/calibrate.launch" />
- <group name="wg">
- <node pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i rteth0 -x /robotdesc/pr2"
- output="screen"/>
- </group>
-</launch>
-
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xacro.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xacro.xml 2008-11-20 22:20:53 UTC (rev 7081)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xacro.xml 2008-11-20 22:22:40 UTC (rev 7082)
@@ -30,33 +30,33 @@
</pr2_base>
- <pr2_torso name="torso" parent="base">
+ <pr2_torso name="torso_lift" parent="base">
<origin xyz="-0.05 0 0.739675" rpy="0 0 0" />
</pr2_torso>
- <pr2_head name="head" parent="torso">
+ <pr2_head name="head" parent="torso_lift">
<origin xyz="0 0 0.3815" rpy="0 0 0" />
</pr2_head>
- <pr2_tilting_laser name="tilt_laser" parent="torso">
+ <pr2_tilting_laser name="laser_tilt" parent="torso_lift">
<origin xyz="0.1 0 0.19525" rpy="0 0 0" />
</pr2_tilting_laser>
- <pr2_arm side="right" reflect="-1" parent="torso">
+ <pr2_arm side="r" reflect="-1" parent="torso_lift">
<origin xyz="0 -0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="right" parent="right_wrist_roll" />
+ <pr2_gripper side="r" parent="r_wrist_roll" />
- <pr2_arm side="left" reflect="1" parent="torso">
+ <pr2_arm side="l" reflect="1" parent="torso_lift">
<origin xyz="0.0 0.188 0.0" rpy="0 0 0" />
</pr2_arm>
- <pr2_gripper side="left" parent="left_wrist_roll" />
+ <pr2_gripper side="l" parent="l_wrist_roll" />
- <pr2_ptz side="right" reflect="-1" parent="t...
[truncated message content] |
|
From: <mc...@us...> - 2008-11-20 23:33:43
|
Revision: 7089
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7089&view=rev
Author: mcgann
Date: 2008-11-20 23:33:37 +0000 (Thu, 20 Nov 2008)
Log Message:
-----------
Moved Observation Buffer stuff to the costmap package
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
pkg/trunk/world_models/costmap_2d/CMakeLists.txt
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt 2008-11-20 23:33:37 UTC (rev 7089)
@@ -10,13 +10,6 @@
genmsg()
-
-#uncomment for profiling
-#set(ROS_COMPILE_FLAGS "-g -pg" ${ROS_COMPILE_FLAGS})
-#set(ROS_LINK_FLAGS "-g -pg" ${ROS_LINK_FLAGS})
-#set(ROS_COMPILE_FLAGS "-g" ${ROS_COMPILE_FLAGS})
-#set(ROS_LINK_FLAGS "-g" ${ROS_LINK_FLAGS})
-
# Library
rospack_add_library(highlevel_controllers src/MoveBase.cpp src/VelocityControllers.cc)
target_link_libraries(highlevel_controllers trajectory_rollout costmap_2d sbpl_util)
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-20 23:33:37 UTC (rev 7089)
@@ -40,6 +40,7 @@
// Costmap used for the map representation
#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/basic_observation_buffer.h>
// Message structures used
#include <std_msgs/Planner2DState.h>
@@ -63,38 +64,6 @@
namespace ros {
namespace highlevel_controllers {
- /**
- * @brief Extend base class to handle buffering until a transform is available, and to support locking for mult-threaded
- * access
- */
- class ObservationBuffer: public costmap_2d::ObservationBuffer {
- public:
- ObservationBuffer(const std::string& frame_id, rosTFClient& tf, ros::Duration keepAlive, double robotRadius, double minZ, double maxZ);
-
- void buffer_cloud(const std_msgs::PointCloud& local_cloud);
-
- virtual void get_observations(std::vector<Observation>& observations);
-
- private:
-
- /**
- * @brief Test if point in the square footprint of the robot
- */
- bool inFootprint(double x, double y) const;
-
- /**
- * @brief Provide a filtered set of points based on the extraction of the robot footprint and the
- * filter based on the min and max z values
- */
- std_msgs::PointCloud * extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const;
-
- const std::string frame_id_;
- rosTFClient& tf_;
- std::deque<std_msgs::PointCloud> point_clouds_; /**< Buffer point clouds until a transform is available */
- ros::thread::mutex buffer_mutex_;
- const double robotRadius_, minZ_, maxZ_; /**< Constraints for filtering points */
- };
-
class MoveBase : public HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal> {
public:
@@ -228,10 +197,14 @@
rosTFClient tf_; /**< Used to do transforms */
- // Observation Buffers
- ObservationBuffer* baseScanBuffer_;
- ObservationBuffer* tiltScanBuffer_;
- ObservationBuffer* stereoCloudBuffer_;
+ // Observation Buffers are dynamically allocated since their constructors take
+ // arguments bound by lookup to the param server. This could be chnaged with some reworking of how paramaters
+ // are looked up. If we wanted to generalize this node further, we could use a factory pattern to dynamically
+ // load specific derived classes. For that we would need a hand-shaking pattern to register subscribers for them
+ // with this node
+ costmap_2d::BasicObservationBuffer* baseScanBuffer_;
+ costmap_2d::BasicObservationBuffer* tiltScanBuffer_;
+ costmap_2d::BasicObservationBuffer* stereoCloudBuffer_;
/** Should encapsulate as a controller wrapper that is not resident in the trajectory rollout package */
VelocityController* controller_;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-20 23:33:37 UTC (rev 7089)
@@ -34,159 +34,17 @@
#include <MoveBase.hh>
+#include <costmap_2d/basic_observation_buffer.h>
#include <std_msgs/BaseVel.h>
#include <std_msgs/PointCloud.h>
#include <std_msgs/Pose2DFloat32.h>
#include <std_msgs/Polyline2D.h>
#include <std_srvs/StaticMap.h>
#include <std_msgs/PointStamped.h>
-#include <deque>
-#include <set>
namespace ros {
namespace highlevel_controllers {
- ObservationBuffer::ObservationBuffer(const std::string& frame_id, rosTFClient& tf, ros::Duration keepAlive,
- double robotRadius, double minZ, double maxZ)
- : costmap_2d::ObservationBuffer(keepAlive), frame_id_(frame_id), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ){}
-
- void ObservationBuffer::buffer_cloud(const std_msgs::PointCloud& local_cloud)
- {
- static const ros::Duration max_transform_delay(10, 0); // max time we will wait for a transform before chucking out the data
- point_clouds_.push_back(local_cloud);
-
- std_msgs::PointCloud * newData = NULL;
- std_msgs::PointCloud * map_cloud = NULL;
-
- while(!point_clouds_.empty()){
- const std_msgs::PointCloud& point_cloud = point_clouds_.front();
- std_msgs::Point origin;
-
- // Throw out point clouds that are old.
- if((local_cloud.header.stamp - point_cloud.header.stamp) > max_transform_delay){
- ROS_DEBUG("Discarding stale point cloud\n");
- point_clouds_.pop_front();
- continue;
- }
-
- libTF::TFPoint map_origin;
- std_msgs::PointCloud base_cloud;
-
- /* Transform to the base frame */
- try
- {
- // First we want the origin for the sensor
- libTF::TFPoint local_origin;
- local_origin.x = 0;
- local_origin.y = 0;
- local_origin.z = 0;
- local_origin.time = point_cloud.header.stamp.toNSec();
- local_origin.frame = frame_id_;
- map_origin = tf_.transformPoint("map", local_origin);
-
- tf_.transformPointCloud("base", base_cloud, point_cloud);
- newData = extractFootprintAndGround(base_cloud);
- map_cloud = new std_msgs::PointCloud();
- tf_.transformPointCloud("map", *map_cloud, *newData);
-
- ROS_DEBUG("Buffering cloud for %s at origin <%f, %f, %f>\n", frame_id_.c_str(), map_origin.x, map_origin.y, map_origin.z);
- }
- catch(libTF::TransformReference::LookupException& ex)
- {
- ROS_ERROR("Lookup exception for %s : %s\n", frame_id_.c_str(), ex.what());
- break;
- }
- catch(libTF::TransformReference::ExtrapolateException& ex)
- {
- ROS_DEBUG("No transform available yet for %s - have to try later: %s . Buffer size is %d\n",
- frame_id_.c_str(), ex.what(), point_clouds_.size());
- break;
- }
- catch(libTF::TransformReference::ConnectivityException& ex)
- {
- ROS_ERROR("Connectivity exception for %s: %s\n", frame_id_.c_str(), ex.what());
- break;
- }
- catch(...)
- {
- ROS_ERROR("Exception in point cloud computation\n");
- break;
- }
-
- point_clouds_.pop_front();
-
- if (newData != NULL){
- delete newData;
- newData = NULL;
- }
-
- // Get the point from whihc we ray trace
- std_msgs::Point o;
- o.x = map_origin.x;
- o.y = map_origin.y;
- o.z = map_origin.z;
-
- // Allocate and buffer the observation
- Observation obs(o, map_cloud);
- buffer_observation(obs);
- map_cloud = NULL;
- }
-
- // In case we get thrown out on the second transform - clean up
- if (newData != NULL){
- delete newData;
- newData = NULL;
- }
-
- if(map_cloud != NULL){
- delete map_cloud;
- map_cloud = NULL;
- }
- }
-
- void ObservationBuffer::get_observations(std::vector<costmap_2d::Observation>& observations){
- costmap_2d::ObservationBuffer::get_observations(observations);
- }
-
- /**
- * The point is in the footprint if its x and y values are in the range [0 robotWidth] in
- * the base frame.
- */
- bool ObservationBuffer::inFootprint(double x, double y) const {
- bool result = fabs(x) <= robotRadius_ && fabs(y) <= robotRadius_;
-
- if(result){
- ROS_DEBUG("Discarding point <%f, %f> in footprint\n", x, y);
- }
-
- return result;
- }
-
- std_msgs::PointCloud * ObservationBuffer::extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const {
- std_msgs::PointCloud *copy = new std_msgs::PointCloud();
- copy->header = baseFrameCloud.header;
-
- unsigned int n = baseFrameCloud.get_pts_size();
- unsigned int j = 0;
- copy->set_pts_size(n);
-
- for (unsigned int k = 0 ; k < n ; ++k){
- bool ok = baseFrameCloud.pts[k].z > minZ_ && baseFrameCloud.pts[k].z < maxZ_;
- if (ok && !inFootprint(baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y)){
- copy->pts[j++] = baseFrameCloud.pts[k];
- }
- else {
- ROS_DEBUG("Discarding <%f, %f, %f>\n", baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y, baseFrameCloud.pts[k].z);
- }
- }
-
- copy->set_pts_size(j);
-
- ROS_DEBUG("Filter discarded %d points (%d left) \n", n - j, j);
-
- return copy;
- }
-
MoveBase::MoveBase()
: HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal>("move_base", "state", "goal"),
tf_(*this, true, 10000000000ULL, 0ULL), // cache for 10 sec, no extrapolation
@@ -265,9 +123,9 @@
robotWidth_ = inscribedRadius * 2;
// Allocate observation buffers
- baseScanBuffer_ = new ObservationBuffer(std::string("base_laser"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
- tiltScanBuffer_ = new ObservationBuffer(std::string("tilt_laser"), tf_, ros::Duration(1, 0), inscribedRadius, minZ_, maxZ_);
- stereoCloudBuffer_ = new ObservationBuffer(std::string("stereo"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+ baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+ tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("tilt_laser"), tf_, ros::Duration(1, 0), inscribedRadius, minZ_, maxZ_);
+ stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
// get map via RPC
std_srvs::StaticMap::request req;
@@ -474,13 +332,11 @@
void MoveBase::baseScanCallback()
{
- ROS_DEBUG("Base Scan Callback");
// Project laser into point cloud
std_msgs::PointCloud local_cloud;
local_cloud.header = baseScanMsg_.header;
projector_.projectLaser(baseScanMsg_, local_cloud, baseLaserMaxRange_);
petTheWatchDog(local_cloud.header.stamp);
- ROS_DEBUG("Projected");
lock();
baseScanBuffer_->buffer_cloud(local_cloud);
unlock();
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-20 23:33:37 UTC (rev 7089)
@@ -12,7 +12,7 @@
<remap from="base_pose_gazebo_ground_truth" to="base_pose_ground_truth"/>
<param name="max_publish_frequency" value="20.0"/>
<!--node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" /-->
- <node pkg="fake_localization" type="fake_localization" args="" respawn="false" />
+ <!--node pkg="fake_localization" type="fake_localization" args="" respawn="false" /-->
<!-- Load parameters for moving the base. -->
<param name="costmap_2d/inflation_radius" type="double" value="0.35" />
Modified: pkg/trunk/world_models/costmap_2d/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/costmap_2d/CMakeLists.txt 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/world_models/costmap_2d/CMakeLists.txt 2008-11-20 23:33:37 UTC (rev 7089)
@@ -4,17 +4,15 @@
rospack(costmap_2d)
genmsg()
-#uncomment for profiling
-#set(ROS_COMPILE_FLAGS "-g -pg" ${ROS_COMPILE_FLAGS})
-#set(ROS_LINK_FLAGS "-g -pg" ${ROS_LINK_FLAGS})
-#set(ROS_COMPILE_FLAGS "-g" ${ROS_COMPILE_FLAGS})
-#set(ROS_LINK_FLAGS "-g" ${ROS_LINK_FLAGS})
+rospack_add_library(costmap_2d src/costmap_2d.cpp
+ src/obstacle_map_accessor.cc
+ src/observation_buffer.cc
+ src/basic_observation_buffer.cc)
-rospack_add_library(costmap_2d src/costmap_2d.cpp src/obstacle_map_accessor.cc)
-
# Test target for module tests to be included in gtest regression test harness
rospack_add_gtest(utest src/test/module-tests.cc)
target_link_libraries(utest costmap_2d)
# Target for benchmarking the costmap
-rospack_add_executable(benchmark src/test/benchmark.cc src/costmap_2d.cpp src/obstacle_map_accessor.cc)
+rospack_add_executable(benchmark src/test/benchmark.cc )
+target_link_libraries(benchmark costmap_2d)
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-20 23:33:37 UTC (rev 7089)
@@ -56,14 +56,12 @@
// Base class
#include "obstacle_map_accessor.h"
-#include <std_msgs/Point.h>
-#include <std_msgs/PointCloud.h>
+// Need observations
+#include "observation.h"
+
//c++
-#include <list>
#include <vector>
-#include <map>
-#include <set>
#include <string>
#include <queue>
@@ -96,50 +94,7 @@
typedef std::priority_queue< QueueElement*, std::vector<QueueElement*>, QueueElementComparator > QUEUE;
- /**
- * @brief Stores an observation in terms of a point cloud and the origin of the source
- * @note Tried to make members and constructor arguments const but the compiler would not accept the default
- * assignment operator for vector insertion!
- */
- class Observation {
- public:
- // Structors
- Observation(std_msgs::Point& p, std_msgs::PointCloud* cloud): origin_(p), cloud_(cloud) {}
- Observation(const Observation& org): origin_(org.origin_), cloud_(org.cloud_){}
- std_msgs::Point origin_;
- std_msgs::PointCloud* cloud_;
- };
-
- /**
- * @brief Base class for buffering observations with a time to live property
- * The inputs are in the map frame
- */
- class ObservationBuffer {
- public:
- ObservationBuffer(ros::Duration keep_alive):keep_alive_(keep_alive){}
-
- virtual ~ObservationBuffer();
-
- /**
- * @brief Buffer a current observation
- * @return true if succeded, false if not (which might occur if there was no transform available for example)
- */
- bool buffer_observation(const Observation& observation);
-
- /**
- * @brief Queries for current observations. Any observations that are no longer
- * current will be deleted. Will append observations to the input vector
- */
- virtual void get_observations(std::vector<Observation>& observations);
-
- private:
- std::list<Observation> buffer_;
- const ros::Duration keep_alive_;
- ros::Time last_updated_;
- };
-
-
class CostMap2D: public ObstacleMapAccessor
{
public:
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-20 23:33:37 UTC (rev 7089)
@@ -68,49 +68,6 @@
return (unsigned int) a;
}
- // Just clean up outstanding observations
- ObservationBuffer::~ObservationBuffer(){
- while(!buffer_.empty()){
- std::list<Observation>::iterator it = buffer_.begin();
- const std_msgs::PointCloud* cloud = it->cloud_;
- delete cloud;
- buffer_.erase(it);
- }
- }
-
- // Only works if the observation is in the map frame - test for it. It should be transformed before
- // we enque it
- bool ObservationBuffer::buffer_observation(const Observation& observation){
- last_updated_ = observation.cloud_->header.stamp;
-
- if(observation.cloud_->header.frame_id != "map")
- return false;
-
- // If the duration is 0, then we just keep the latest one, so we clear out all existing observations
- while(!buffer_.empty()){
- std::list<Observation>::iterator it = buffer_.begin();
- // Get the current one, and check if still alive. if so
- Observation& obs = *it;
- if((last_updated_ - obs.cloud_->header.stamp) > keep_alive_){
- delete obs.cloud_;
- buffer_.erase(it);
- }
- else
- break;
- }
-
- // Otherwise just store it and indicate success
- buffer_.push_back(observation);
- return true;
- }
-
- void ObservationBuffer::get_observations(std::vector<Observation>& observations){
- // Add all remaining observations to the output
- for(std::list<Observation>::const_iterator it = buffer_.begin(); it != buffer_.end(); ++it){
- observations.push_back(*it);
- }
- }
-
CostMap2D::CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, unsigned char threshold, double maxZ, double zLB, double zUB,
double inflationRadius, double circumscribedRadius, double inscribedRadius, double weight,
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-11-20 23:02:24 UTC (rev 7088)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-11-20 23:33:37 UTC (rev 7089)
@@ -33,6 +33,7 @@
*/
#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/observation_buffer.h>
#include <set>
#include <gtest/gtest.h>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-21 00:43:48
|
Revision: 7099
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7099&view=rev
Author: hsujohnhsu
Date: 2008-11-21 00:43:42 +0000 (Fri, 21 Nov 2008)
Log Message:
-----------
demo updates corresponding to name changes.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/arm.launch
pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
Modified: pkg/trunk/demos/arm_gazebo/arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-21 00:41:30 UTC (rev 7098)
+++ pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-21 00:43:42 UTC (rev 7099)
@@ -15,7 +15,7 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_arm_test/controllers_arm.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
- <node pkg="robot_mechanism_controllers" type="control.py" args="set gripper_left_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" /> <!-- open gripper .5 radians -->
<!-- for visualization, heavy, off by default -->
<!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
</group>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2008-11-21 00:41:30 UTC (rev 7098)
+++ pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2008-11-21 00:43:42 UTC (rev 7099)
@@ -2,7 +2,7 @@
<param name="base_controller/odom_publish_rate" value="10" />
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
- <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 1 .45 .40" />
+ <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 1 .45 .40" />
</launch>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-11-21 00:41:30 UTC (rev 7098)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-11-21 00:43:42 UTC (rev 7099)
@@ -2,10 +2,10 @@
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
<param name="base_controller/odom_publish_rate" value="10" />
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_base_lab.xml" output="screen"/>
- <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml" output="screen"/>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml" output="screen"/>
<!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
for details of the profile, rates, see controller::LaserScannerControllerNode. -->
- <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 1 .45 .40" />
+ <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 1 .45 .40" />
</launch>
Modified: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-11-21 00:41:30 UTC (rev 7098)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-11-21 00:43:42 UTC (rev 7099)
@@ -75,27 +75,27 @@
#define COMMAND_TIMEOUT_SEC 0.2
-#define LINKL0 "left_shoulder_pan_joint"
-#define LINKL1 "left_shoulder_lift_joint"
-#define LINKL2 "left_upper_arm_roll_joint"
-#define LINKL3 "left_elbow_flex_joint"
-#define LINKL4 "left_forearm_roll_joint"
-#define LINKL5 "left_wrist_flex_joint"
-#define LINKL6 "left_wrist_roll_joint"
-#define LINKL7 "left_gripper_joint"
-#define LINKR0 "right_shoulder_pan_joint"
-#define LINKR1 "right_shoulder_lift_joint"
-#define LINKR2 "right_upper_arm_roll_joint"
-#define LINKR3 "right_elbow_flex_joint"
-#define LINKR4 "right_forearm_roll_joint"
-#define LINKR5 "right_wrist_flex_joint"
-#define LINKR6 "right_wrist_roll_joint"
-#define LINKR7 "right_gripper_joint"
+#define LINKL0 "l_shoulder_pan_joint"
+#define LINKL1 "l_shoulder_lift_joint"
+#define LINKL2 "l_upper_arm_roll_joint"
+#define LINKL3 "l_elbow_flex_joint"
+#define LINKL4 "l_forearm_roll_joint"
+#define LINKL5 "l_wrist_flex_joint"
+#define LINKL6 "l_wrist_roll_joint"
+#define LINKL7 "l_gripper_joint"
+#define LINKR0 "r_shoulder_pan_joint"
+#define LINKR1 "r_shoulder_lift_joint"
+#define LINKR2 "r_upper_arm_roll_joint"
+#define LINKR3 "r_elbow_flex_joint"
+#define LINKR4 "r_forearm_roll_joint"
+#define LINKR5 "r_wrist_flex_joint"
+#define LINKR6 "r_wrist_roll_joint"
+#define LINKR7 "r_gripper_joint"
#define LEFT_ARM_COMMAND_TOPIC "left_arm_commands"
#define RIGHT_ARM_COMMAND_TOPIC "right_arm_commands"
-#define LEFT_GRIPPER_COMMAND_TOPIC "gripper_left_controller/set_command"
-#define RIGHT_GRIPPER_COMMAND_TOPIC "gripper_right_controller/set_command"
+#define LEFT_GRIPPER_COMMAND_TOPIC "l_gripper_controller/set_command"
+#define RIGHT_GRIPPER_COMMAND_TOPIC "r_gripper_controller/set_command"
#define JOINT_STEP_SIZE 5*M_PI/180
#define GRIPPER_STEP_SIZE 1*M_PI/180
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-21 02:55:41
|
Revision: 7116
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7116&view=rev
Author: hsujohnhsu
Date: 2008-11-21 02:55:30 +0000 (Fri, 21 Nov 2008)
Log Message:
-----------
* moved controllers for 2dnav test to pr2_gazebo.
* using ptz stl models.
* rename controller for pr2_prototype head controllers.
Modified Paths:
--------------
pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/ptz_defs.xml
Added Paths:
-----------
pkg/trunk/demos/pr2_gazebo/controllers_2dnav_test.xml
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_l_hi.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_l_low.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_r_hi.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_r_low.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_l_hi.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_l_low.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_r_hi.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_r_low.stl
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_hi.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_low.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_hi.stl
pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_low.stl
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml
Added: pkg/trunk/demos/pr2_gazebo/controllers_2dnav_test.xml
===================================================================
--- pkg/trunk/demos/pr2_gazebo/controllers_2dnav_test.xml (rev 0)
+++ pkg/trunk/demos/pr2_gazebo/controllers_2dnav_test.xml 2008-11-21 02:55:30 UTC (rev 7116)
@@ -0,0 +1,229 @@
+<?xml version="1.0"?>
+
+<controllers>
+
+ <controller name="base_controller" topic="base_controller" type="BaseControllerNode">
+ <map name="velocity_control" flag="base_control">
+ <elem key="kp_speed">100.0</elem>
+ <elem key="kp_caster_steer">0</elem>
+ <elem key="timeout">3</elem>
+
+ <elem key="max_x_dot">1</elem>
+ <elem key="max_y_dot">1</elem>
+ <elem key="max_yaw_dot">1</elem>
+
+ <elem key="max_x_accel">2.0</elem>
+ <elem key="max_y_accel">2.0</elem>
+ <elem key="max_yaw_accel">1.0</elem>
+ </map>
+
+ <controller name="fl_caster_l_wheel_controller" topic="fl_caster_l_wheel_controller" type="JointVelocityControllerNode">
+ <joint name="fl_caster_l_wheel_joint" >
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ </joint>
+ </controller>
+ <controller name="fl_caster_r_wheel_controller" topic="fl_caster_r_wheel_controller" type="JointVelocityControllerNode">
+ <joint name="fl_caster_r_wheel_joint" >
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ </joint>
+ </controller>
+ <controller name="fr_caster_l_wheel_controller" topic="fr_caster_l_wheel_controller" type="JointVelocityControllerNode">
+ <joint name="fr_caster_l_wheel_joint" >
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ </joint>
+ </controller>
+ <controller name="fr_caster_r_wheel_controller" topic="fr_caster_r_wheel_controller" type="JointVelocityControllerNode">
+ <joint name="fr_caster_r_wheel_joint" >
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ </joint>
+ </controller>
+ <controller name="bl_caster_l_wheel_controller" topic="bl_caster_l_wheel_controller" type="JointVelocityControllerNode">
+ <joint name="bl_caster_l_wheel_joint" >
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ </joint>
+ </controller>
+ <controller name="bl_caster_r_wheel_controller" topic="bl_caster_r_wheel_controller" type="JointVelocityControllerNode">
+ <joint name="bl_caster_r_wheel_joint" >
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ </joint>
+ </controller>
+ <controller name="br_caster_l_wheel_controller" topic="br_caster_l_wheel_controller" type="JointVelocityControllerNode">
+ <joint name="br_caster_l_wheel_joint" >
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ </joint>
+ </controller>
+ <controller name="br_caster_r_wheel_controller" topic="br_caster_r_wheel_controller" type="JointVelocityControllerNode">
+ <joint name="br_caster_r_wheel_joint" >
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ </joint>
+ </controller>
+
+ <controller name="fl_caster_rotation_controller" topic="fl_caster_rotation_controller" type="JointVelocityControllerNode">
+ <joint name="fl_caster_rotation_joint" >
+ <pid p="3" d="0" i="0.1" iClamp="4" />
+ </joint>
+ </controller>
+ <controller name="fr_caster_rotation_controller" topic="fr_caster_rotation_controller" type="JointVelocityControllerNode">
+ <joint name="fr_caster_rotation_joint" >
+ <pid p="3" d="0" i="0.1" iClamp="4" />
+ </joint>
+ </controller>
+ <controller name="bl_caster_rotation_controller" topic="bl_caster_rotation_controller" type="JointVelocityControllerNode">
+ <joint name="bl_caster_rotation_joint" >
+ <pid p="3" d="0" i="0.1" iClamp="4" />
+ </joint>
+ </controller>
+ <controller name="br_caster_rotation_controller" topic="br_caster_rotation_controller" type="JointVelocityControllerNode">
+ <joint name="br_caster_rotation_joint" >
+ <pid p="3" d="0" i="0.1" iClamp="4" />
+ </joint>
+ </controller>
+
+
+ </controller>
+
+ <!-- ========================================= -->
+ <!-- torso array -->
+ <controller name="torso_lift_controller" topic="torso_lift_controller" type="JointPositionControllerNode">
+ <joint name="torso_lift_joint">
+ <pid p="1000" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <!-- ========================================= -->
+ <!-- left arm array -->
+ <controller name="left_arm_controller" type="ArmPositionControllerNode">
+ <listen_topic name="left_arm_commands" />
+ <kinematics>
+ <elem key="kdl_chain_name">left_arm</elem>
+ </kinematics>
+ <map name="controller_param">
+ <elem key="kdl_chain_name">left_arm</elem>
+ </map>
+ <controller name="l_shoulder_pan_controller" topic="l_shoulder_pan_controller" type="JointPositionController">
+ <joint name="l_shoulder_pan_joint" >
+ <pid p="100" d="200" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_shoulder_lift_controller" topic="l_shoulder_lift_controller" type="JointPositionController">
+ <joint name="l_shoulder_lift_joint" >
+ <pid p="100" d="100.0" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_upper_arm_roll_controller" topic="l_upper_arm_roll_controller" type="JointPositionController">
+ <joint name="l_upper_arm_roll_joint" >
+ <pid p="400" d="400" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_elbow_flex_controller" topic="l_elbow_flex_controller" type="JointPositionController">
+ <joint name="l_elbow_flex_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_forearm_roll_controller" topic="l_forearm_roll_controller" type="JointPositionController">
+ <joint name="l_forearm_roll_joint" >
+ <pid p="200" d="200" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_wrist_flex_controller" topic="l_wrist_flex_controller" type="JointPositionController">
+ <joint name="l_wrist_flex_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_wrist_roll_controller" topic="l_wrist_roll_controller" type="JointPositionController">
+ <joint name="l_wrist_roll_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="0" />
+ </joint>
+ </controller>
+ </controller>
+ <!-- Special gripper joint -->
+ <controller name="l_gripper_controller" topic="l_gripper_controller" type="JointPositionControllerNode">
+ <joint name="l_gripper_l_finger_joint">
+ <pid p="1.0" d="0.00" i="0.00" iClamp="0.00" />
+ </joint>
+ </controller>
+ <!-- ========================================= -->
+ <!-- right arm array -->
+ <controller name="right_arm_controller" type="ArmPositionControllerNode">
+ <listen_topic name="right_arm_commands" />
+ <kinematics>
+ <elem key="kdl_chain_name">right_arm</elem>
+ </kinematics>
+ <map name="controller_param">
+ <elem key="kdl_chain_name">right_arm</elem>
+ </map>
+ <controller name="r_shoulder_pan_controller" topic="r_shoulder_pan_controller" type="JointPositionController">
+ <joint name="r_shoulder_pan_joint" >
+ <pid p="100" d="200" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_shoulder_lift_controller" topic="r_shoulder_lift_controller" type="JointPositionController">
+ <joint name="r_shoulder_lift_joint" >
+ <pid p="100" d="100.0" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_upper_arm_roll_controller" topic="r_upper_arm_roll_controller" type="JointPositionController">
+ <joint name="r_upper_arm_roll_joint" >
+ <pid p="400" d="400" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_elbow_flex_controller" topic="r_elbow_flex_controller" type="JointPositionController">
+ <joint name="r_elbow_flex_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_forearm_roll_controller" topic="r_forearm_roll_controller" type="JointPositionController">
+ <joint name="r_forearm_roll_joint" >
+ <pid p="200" d="200" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_wrist_flex_controller" topic="r_wrist_flex_controller" type="JointPositionController">
+ <joint name="r_wrist_flex_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_wrist_roll_controller" topic="r_wrist_roll_controller" type="JointPositionController">
+ <joint name="r_wrist_roll_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="0" />
+ </joint>
+ </controller>
+ </controller>
+ <!-- Special gripper joint -->
+ <controller name="r_gripper_controller" topic="r_gripper_controller" type="JointPositionControllerNode">
+ <joint name="r_gripper_l_finger_joint">
+ <pid p="1.0" d="0.00" i="0.00" iClamp="0.00" />
+ </joint>
+ </controller>
+ <!-- head and above array -->
+ <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
+ <joint name="head_pan_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
+ <joint name="head_tilt_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+
+ <controller name="laser_tilt_controller" topic="laser_tilt_controller" type="LaserScannerControllerNode">
+ <joint name="laser_tilt_mount_joint" >
+ <pid p="12" i=".1" d="1" iClamp="0.5" />
+ </joint>
+ </controller>
+
+ <!-- this version of laser scanner controller seems to be broken
+ <controller name="tilt_laser_controller" topic="laser_test" type="LaserScannerControllerNode">
+ <velocity>
+ <velocityFilter smoothing="0.2"/>
+ <joint name="tilt_laser_mount_joint" type="velocity">
+ <pid p="0.56" i = "14" d = "0.000001" iClamp = "0.02" />
+ </joint>
+ </velocity>
+ <position>
+ <joint name="single_joint" type="position">
+ <pid p="4.4382" i = "31.2110" d = "0.0453" iClamp = "0.004" />
+ </joint>
+ </position>
+ </controller>
+ -->
+</controllers>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2008-11-21 02:32:40 UTC (rev 7115)
+++ pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2008-11-21 02:55:30 UTC (rev 7116)
@@ -1,6 +1,6 @@
<launch>
<param name="base_controller/odom_publish_rate" value="10" />
- <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find pr2_gazebo)/controllers_2dnav_test.xml" respawn="false" output="screen" />
<!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
<node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 1 .45 .40" />
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_hi.stl
===================================================================
(Binary files differ)
Copied: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_l_hi.stl (from rev 7112, pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_hi.stl)
===================================================================
(Binary files differ)
Copied: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_l_low.stl (from rev 7112, pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_low.stl)
===================================================================
(Binary files differ)
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_low.stl
===================================================================
(Binary files differ)
Added: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_r_hi.stl
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_r_hi.stl
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_r_low.stl
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_base_r_low.stl
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_hi.stl
===================================================================
(Binary files differ)
Copied: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_l_hi.stl (from rev 7112, pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_hi.stl)
===================================================================
(Binary files differ)
Copied: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_l_low.stl (from rev 7112, pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_low.stl)
===================================================================
(Binary files differ)
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_low.stl
===================================================================
(Binary files differ)
Added: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_r_hi.stl
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_r_hi.stl
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_r_low.stl
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/models/pr2/ptz_r_low.stl
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Copied: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml (from rev 7112, pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml)
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml 2008-11-21 02:55:30 UTC (rev 7116)
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+
+<controllers>
+
+
+ <!-- ========================================= -->
+ <!-- torso array -->
+ <controller name="torso_controller" topic="torso_controller" type="JointPositionControllerNode">
+ <joint name="torso_joint">
+ <pid p="1000" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <!-- ========================================= -->
+ <!-- head and above array -->
+ <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
+ <listen_topic name="head_pan_commands" />
+ <joint name="head_pan_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
+ <listen_topic name="head_tilt_commands" />
+ <joint name="head_tilt_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+
+ <controller name="laser_tilt_controller" topic="laser_tilt_controller" type="LaserScannerControllerNode">
+ <joint name="laser_tilt_mount_joint" >
+ <pid p="12" i=".1" d="1" iClamp="0.5" />
+ </joint>
+ </controller>
+
+</controllers>
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml 2008-11-21 02:32:40 UTC (rev 7115)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml 2008-11-21 02:55:30 UTC (rev 7116)
@@ -1,34 +0,0 @@
-<?xml version="1.0"?>
-
-<controllers>
-
-
- <!-- ========================================= -->
- <!-- torso array -->
- <controller name="torso_controller" topic="torso_controller" type="JointPositionControllerNode">
- <joint name="torso_joint">
- <pid p="1000" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
- <!-- ========================================= -->
- <!-- head and above array -->
- <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
- <listen_topic name="head_pan_commands" />
- <joint name="head_pan_joint" >
- <pid p="100" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
- <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
- <listen_topic name="head_tilt_commands" />
- <joint name="head_tilt_joint" >
- <pid p="100" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
-
- <controller name="laser_tilt_controller" topic="laser_tilt_controller" type="LaserScannerControllerNode">
- <joint name="laser_tilt_mount_joint" >
- <pid p="12" i=".1" d="1" iClamp="0.5" />
- </joint>
- </controller>
-
-</controllers>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/ptz_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/ptz_defs.xml 2008-11-21 02:32:40 UTC (rev 7115)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/ptz_defs.xml 2008-11-21 02:55:30 UTC (rev 7116)
@@ -28,17 +28,17 @@
</inertial>
<visual>
- <origin xyz="0 ${reflect*0.025} 0" rpy="0 M_PI/2 0" />
+ <origin xyz="0 ${reflect*0.0} 0" rpy="0 0 0" />
<map name="foo" flag="gazebo">
<elem key="material">Gazebo/Blue</elem>
</map>
<geometry name="${side}_ptz_pan_visual">
- <mesh scale="0.05 0.05 0.05" />
+ <mesh filename="ptz_base_${side}" />
</geometry>
</visual>
<collision>
- <origin xyz="0 -0.025 0" rpy="0 M_PI/2 0" />
+ <origin xyz="0 ${reflect*0.0} 0" rpy="0 0 0" />
<geometry name="${side}_ptz_pan_collision">
<cylinder radius="0.05" length="0.05" />
</geometry>
@@ -54,7 +54,7 @@
<sensor name="${side}_ptz_tilt_link" type="camera">
<parent name="${side}_ptz_pan_link" />
- <origin xyz="0.0 ${reflect*0.03} 0.0" rpy="0 0 0"/>
+ <origin xyz="0.0 ${reflect*0.0} 0.0" rpy="0 0 0"/>
<joint name="${side}_ptz_tilt_joint" />
<inertial>
@@ -66,17 +66,17 @@
</inertial>
<visual>
- <origin xyz="0 0 0" rpy="0 M_PI/2 0" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
<map name="foo" flag="gazebo">
<elem key="material">Gazebo/Red</elem>
</map>
<geometry name="${side}_ptz_tilt_visual">
- <mesh scale="0.03 0.03 0.03" />
+ <mesh filename="ptz_${side}" />
</geometry>
</visual>
<collision>
- <origin xyz="0 0 0" rpy="0 M_PI/2 0" />
+ <origin xyz="0 ${reflect*0.0} 0" rpy="0 0 0" />
<geometry name="${side}_ptz_tilt_collision">
<cylinder radius="0.03" length="0.03" />
</geometry>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-11-21 05:38:09
|
Revision: 7120
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7120&view=rev
Author: gerkey
Date: 2008-11-21 05:38:04 +0000 (Fri, 21 Nov 2008)
Log Message:
-----------
Changes for building on OS X
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl/code/ompl/datastructures/Grid.h
pkg/trunk/util/profiling_utils/manifest.xml
Modified: pkg/trunk/motion_planning/ompl/code/ompl/datastructures/Grid.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/datastructures/Grid.h 2008-11-21 03:39:25 UTC (rev 7119)
+++ pkg/trunk/motion_planning/ompl/code/ompl/datastructures/Grid.h 2008-11-21 05:38:04 UTC (rev 7120)
@@ -40,9 +40,19 @@
#include <vector>
#ifdef __GNUC__
-# include <features.h>
+// OS X doesn't have features.h. Instead we'll just paste in the
+// definition of __GNUC_PREREQ() taken from a Linux version of features.h.
+//# include <features.h>
+#ifndef __GNUC_PREREQ
+#if defined __GNUC__ && defined __GNUC_MINOR__
+# define __GNUC_PREREQ(maj, min) \
+ ((__GNUC__ << 16) + __GNUC_MINOR__ >= ((maj) << 16) + (min))
+#else
+# define __GNUC_PREREQ(maj, min) 0
+#endif
+#endif
-# if __GNUC_PREREQ(4,0)
+# if __GNUC_PREREQ(4,1)
# include <tr1/unordered_map>
# define OMPL_NS_HASH std::tr1
# define OMPL_NAME_HASH unordered_map
Modified: pkg/trunk/util/profiling_utils/manifest.xml
===================================================================
--- pkg/trunk/util/profiling_utils/manifest.xml 2008-11-21 03:39:25 UTC (rev 7119)
+++ pkg/trunk/util/profiling_utils/manifest.xml 2008-11-21 05:38:04 UTC (rev 7120)
@@ -14,5 +14,6 @@
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lprofiling_utils -lpthread -lrt"/>
+ <cpp os="osx" cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lprofiling_utils -lpthread"/>
</export>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mc...@us...> - 2008-11-21 16:12:15
|
Revision: 7133
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7133&view=rev
Author: mcgann
Date: 2008-11-21 16:12:09 +0000 (Fri, 21 Nov 2008)
Log Message:
-----------
Changed persistence policy to only revert to the static map when we receive a new goal or when no plan is found. This is trying to enable continuous plannnig but also provide some measure of hysterisis in the face of dynamic obstacles that move out of sensor range
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/highlevel/highlevel_controllers/include/HighlevelController.hh 2008-11-21 16:12:09 UTC (rev 7133)
@@ -158,11 +158,17 @@
if(isInitialized() && isActive()){
// If the plannerCycleTime is 0 then we only call the planner when we need to
- if(plannerCycleTime_ != 0 || !isValid())
+ if(plannerCycleTime_ != 0 || !isValid()){
setValid(makePlan());
+ if(!isValid()){
+ // Could use a refined locking scheme but for now do not want to delegate that to a derived class
+ lock();
+ handlePlanningFailure();
+ unlock();
+ }
+ }
}
-
if(plannerCycleTime_ >= 0)
sleep(currentTime, std::max(plannerCycleTime_, controllerCycleTime_));
else
@@ -238,6 +244,10 @@
*/
virtual void handleActivation(){}
+ /**
+ * @brief A hook to handle the case when global planning fails
+ */
+ virtual void handlePlanningFailure(){}
/**
* @brief Aquire node level lock
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-21 16:12:09 UTC (rev 7133)
@@ -96,6 +96,12 @@
virtual void handleMapUpdates(const std::vector<unsigned int>& updates){}
/**
+ * @brief When planning has failed should reset the cost map to clear persistent
+ * dynamic obstacles. This is important to provide some hysterisis when interleaving planning
+ */
+ virtual void handlePlanningFailure();
+
+ /**
* @brief Overwrites the current plan with a new one. Will handle suitable publication
* @see publishPlan
*/
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-21 16:12:09 UTC (rev 7133)
@@ -317,6 +317,10 @@
stateMsg.goal.y = goalPose.y;
stateMsg.goal.th = goalPose.yaw;
+
+ // We want to revert to the static map, and resume computations for cost map based on laser data - whenever we get a new goal
+ costMap_->revertToStaticMap();
+
ROS_DEBUG("Received new goal (x=%f, y=%f, th=%f)\n", goalMsg.goal.x, goalMsg.goal.y, goalMsg.goal.th);
}
@@ -400,6 +404,13 @@
base_odom_.unlock();
}
+ /**
+ * A lock will already be aquired here, so just revert the cost map
+ */
+ void MoveBase::handlePlanningFailure(){
+ costMap_->revertToStaticMap();
+ }
+
void MoveBase::updatePlan(const std::list<std_msgs::Pose2DFloat32>& newPlan){
lock();
@@ -788,6 +799,7 @@
ROS_DEBUG("Starting cost map update/n");
lock();
+
// Aggregate buffered observations across 3 sources
std::vector<costmap_2d::Observation> observations;
baseScanBuffer_->get_observations(observations);
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml 2008-11-21 16:12:09 UTC (rev 7133)
@@ -7,8 +7,8 @@
<param name="move_base/plannerType" value="ARAPlanner"/>
<param name="move_base/environmentType" value="2D"/>
<param name="move_base/controller_frequency" value="20.0"/>
- <param name="move_base/planner_frequency" value="0.0"/>
- <param name="move_base/plannerTimeLimit" value="10.0"/>
+ <param name="move_base/planner_frequency" value="1.0"/>
+ <param name="move_base/plannerTimeLimit" value="5.0"/>
<param name="/costmap_2d/base_laser_max_range" value="10.0"/>
<param name="/costmap_2d/tilt_laser_max_range" value="10.0"/>
<param name="/costmap_2d/lethal_obstacle_threshold" value="100.0"/>
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-21 16:12:09 UTC (rev 7133)
@@ -174,6 +174,10 @@
*/
double getWeight() const {return weight_;}
+ /**
+ * @brief Will reset the cost data to the initiali propagated costs of the static map
+ */
+ void revertToStaticMap();
private:
/**
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-21 16:12:09 UTC (rev 7133)
@@ -139,6 +139,10 @@
}
+ void CostMap2D::revertToStaticMap(){
+ memcpy(costData_, staticData_, width_ * height_);
+ }
+
/**
* @brief Updated dyanmic obstacles and compute a diff. Mainly for backward compatibility. This will go away soon.
*/
@@ -187,9 +191,7 @@
{
// Revert to initial state
memset(xy_markers_, 0, width_ * height_ * sizeof(bool));
- memcpy(costData_, staticData_, width_ * height_);
-
// Now propagate free space. We iterate again over observations, process only those from an origin
// within a specific range, and a point within a certain z-range. We only want to propagate free space
// in 2D so keep point and its origin within expected range
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-11-21 13:54:59 UTC (rev 7132)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-11-21 16:12:09 UTC (rev 7133)
@@ -127,6 +127,7 @@
}
// Update with no hits. Should clear (revert to the static map
+ map.revertToStaticMap();
cloud.set_pts_size(0);
map.updateDynamicObstacles(0, 0, CostMap2D::toVector(cloud));
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-21 17:39:47
|
Revision: 7138
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7138&view=rev
Author: hsujohnhsu
Date: 2008-11-21 17:39:41 +0000 (Fri, 21 Nov 2008)
Log Message:
-----------
* update texture for wheels due to naming convention for visualizers.
* update arm controller files, moved them to demos for gazebo directory.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/arm.launch
pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/world/Media/materials/scripts/pr2.material
Added Paths:
-----------
pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml
pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml
Modified: pkg/trunk/demos/arm_gazebo/arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-21 17:33:13 UTC (rev 7137)
+++ pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-21 17:39:41 UTC (rev 7138)
@@ -14,10 +14,14 @@
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_arm_test/controllers_arm.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <!-- start arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
+
+ <!-- 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 -->
- <!-- for visualization, heavy, off by default -->
- <!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
+
+ <!-- for visualization -->
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
</group>
</launch>
Added: pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml 2008-11-21 17:39:41 UTC (rev 7138)
@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<controllers>
+ <!-- left arm array -->
+ <!--
+ <controller name="left_arm_controller" type="LQRControllerNode">
+ <model name="serial_chain">
+ <robot_description>pr2</robot_description>
+ <kinematics>left_arm</kinematics>
+ <joints>
+ <joint name="l_shoulder_pan_joint"/>
+ <joint name="l_shoulder_lift_joint"/>
+ <joint name="l_upper_arm_roll_joint"/>
+ <joint name="l_elbow_flex_joint"/>
+ <joint name="l_forearm_roll_joint"/>
+ <joint name="l_wrist_flex_joint"/>
+ <joint name="l_wrist_roll_joint"/>
+ </joints>
+ </model>
+ </controller> -->
+
+ <controller name="left_arm_controller" type="ArmPositionControllerNode">
+ <listen_topic name="left_arm_commands" />
+ <kinematics>
+ <elem key="kdl_chain_name">left_arm</elem>
+ </kinematics>
+ <map name="controller_param">
+ <elem key="kdl_chain_name">left_arm</elem>
+ </map>
+ <controller name="l_shoulder_pan_controller" topic="l_shoulder_pan_controller" type="JointPositionController">
+ <joint name="l_shoulder_pan_joint" >
+ <pid p="100" d="200" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_shoulder_lift_controller" topic="l_shoulder_lift_controller" type="JointPositionController">
+ <joint name="l_shoulder_lift_joint" >
+ <pid p="100" d="100.0" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_upper_arm_roll_controller" topic="l_upper_arm_roll_controller" type="JointPositionController">
+ <joint name="l_upper_arm_roll_joint" >
+ <pid p="400" d="400" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_elbow_flex_controller" topic="l_elbow_flex_controller" type="JointPositionController">
+ <joint name="l_elbow_flex_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_forearm_roll_controller" topic="l_forearm_roll_controller" type="JointPositionController">
+ <joint name="l_forearm_roll_joint" >
+ <pid p="200" d="200" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_wrist_flex_controller" topic="l_wrist_flex_controller" type="JointPositionController">
+ <joint name="l_wrist_flex_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="l_wrist_roll_controller" topic="l_wrist_roll_controller" type="JointPositionController">
+ <joint name="l_wrist_roll_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="0" />
+ </joint>
+ </controller>
+ </controller>
+ <controller name="l_gripper_controller" topic="l_gripper_controller" type="JointPositionControllerNode">
+ <joint name="l_gripper_l_finger_joint">
+ <pid p="1.0" d="0.00" i="0.00" iClamp="0.00" />
+ </joint>
+ </controller>
+</controllers>
Added: pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml 2008-11-21 17:39:41 UTC (rev 7138)
@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<controllers>
+ <!-- right arm array -->
+ <!--
+ <controller name="right_arm_controller" type="LQRControllerNode">
+ <model name="serial_chain">
+ <robot_description>pr2</robot_description>
+ <kinematics>right_arm</kinematics>
+ <joints>
+ <joint name="r_shoulder_pan_joint"/>
+ <joint name="r_shoulder_lift_joint"/>
+ <joint name="r_upper_arm_roll_joint"/>
+ <joint name="r_elbow_flex_joint"/>
+ <joint name="r_forearm_roll_joint"/>
+ <joint name="r_wrist_flex_joint"/>
+ <joint name="r_wrist_roll_joint"/>
+ </joints>
+ </model>
+ </controller> -->
+
+ <controller name="right_arm_controller" type="ArmPositionControllerNode">
+ <listen_topic name="right_arm_commands" />
+ <kinematics>
+ <elem key="kdl_chain_name">right_arm</elem>
+ </kinematics>
+ <map name="controller_param">
+ <elem key="kdl_chain_name">right_arm</elem>
+ </map>
+ <controller name="r_shoulder_pan_controller" topic="r_shoulder_pan_controller" type="JointPositionController">
+ <joint name="r_shoulder_pan_joint" >
+ <pid p="100" d="200" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_shoulder_lift_controller" topic="r_shoulder_lift_controller" type="JointPositionController">
+ <joint name="r_shoulder_lift_joint" >
+ <pid p="100" d="100.0" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_upper_arm_roll_controller" topic="r_upper_arm_roll_controller" type="JointPositionController">
+ <joint name="r_upper_arm_roll_joint" >
+ <pid p="400" d="400" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_elbow_flex_controller" topic="r_elbow_flex_controller" type="JointPositionController">
+ <joint name="r_elbow_flex_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_forearm_roll_controller" topic="r_forearm_roll_controller" type="JointPositionController">
+ <joint name="r_forearm_roll_joint" >
+ <pid p="200" d="200" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_wrist_flex_controller" topic="r_wrist_flex_controller" type="JointPositionController">
+ <joint name="r_wrist_flex_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ <controller name="r_wrist_roll_controller" topic="r_wrist_roll_controller" type="JointPositionController">
+ <joint name="r_wrist_roll_joint" >
+ <pid p="100" d="100" i="0.1" iClamp="0" />
+ </joint>
+ </controller>
+ </controller>
+ <controller name="r_gripper_controller" topic="r_gripper_controller" type="JointPositionControllerNode">
+ <joint name="r_gripper_l_finger_joint">
+ <pid p="1.0" d="0.00" i="0.00" iClamp="0.00" />
+ </joint>
+ </controller>
+</controllers>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2008-11-21 17:33:13 UTC (rev 7137)
+++ pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2008-11-21 17:39:41 UTC (rev 7138)
@@ -1,7 +1,14 @@
<launch>
<param name="base_controller/odom_publish_rate" value="10" />
- <node pkg="mechanism_control" type="mech.py" args="sp $(find pr2_gazebo)/controllers_2dnav_test.xml" respawn="false" output="screen" />
+ <!--node pkg="mechanism_control" type="mech.py" args="sp $(find pr2_gazebo)/controllers_2dnav_test.xml" respawn="false" output="screen" /-->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_default_controller.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <!--node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_base_lab.xml" output="screen"/-->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_alpha)/controllers_base_lab.xml" output="screen"/>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml" output="screen"/>
+
+ <!-- send laser tilt motor a command -->
<!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
<node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 1 .45 .40" />
</launch>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/Media/materials/scripts/pr2.material
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/Media/materials/scripts/pr2.material 2008-11-21 17:33:13 UTC (rev 7137)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/Media/materials/scripts/pr2.material 2008-11-21 17:39:41 UTC (rev 7138)
@@ -58,7 +58,7 @@
-material PR2/wheel_front_right_l
+material PR2/fr_caster_l_wheel_link
{
technique
{
@@ -72,7 +72,7 @@
}
}
-material PR2/wheel_front_left_l
+material PR2/fl_caster_l_wheel_link
{
technique
{
@@ -85,7 +85,7 @@
}
}
}
-material PR2/wheel_rear_right_l
+material PR2/br_caster_l_wheel_link
{
technique
{
@@ -99,7 +99,7 @@
}
}
-material PR2/wheel_rear_left_l
+material PR2/bl_caster_l_wheel_link
{
technique
{
@@ -114,7 +114,7 @@
}
-material PR2/wheel_front_right_r
+material PR2/fr_caster_r_wheel_link
{
technique
{
@@ -128,7 +128,7 @@
}
}
-material PR2/wheel_front_left_r
+material PR2/fl_caster_r_wheel_link
{
technique
{
@@ -141,7 +141,7 @@
}
}
}
-material PR2/wheel_rear_right_r
+material PR2/br_caster_r_wheel_link
{
technique
{
@@ -155,7 +155,7 @@
}
}
-material PR2/wheel_rear_left_r
+material PR2/bl_caster_r_wheel_link
{
technique
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|