Menu
▾
▴
personalrobots-commit
|
From: <sac...@us...> - 2008-06-11 23:28:40
|
Revision: 763
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=763&view=rev
Author: sachinchitta
Date: 2008-06-11 16:28:49 -0700 (Wed, 11 Jun 2008)
Log Message:
-----------
Removed libKDL from build for now. Will be tested and added later since it does not seem to exist for Feisty.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
Added Paths:
-----------
pkg/trunk/util/kinematics/libKDL/include/libKDL/
pkg/trunk/util/kinematics/libKDL/include/libKDL/kdl_kinematics.h
Removed Paths:
-------------
pkg/trunk/util/kinematics/libKDL/include/kdl_kinematics.h
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-11 23:15:45 UTC (rev 762)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-11 23:28:49 UTC (rev 763)
@@ -18,7 +18,7 @@
LIBP = lib/libpr2API.a
LIBS = lib/libpr2API.so
-all: $(LIBP) test_libpr2API bin/pr2_kin_kdl
+all: $(LIBP) test_libpr2API
pr2API.o: src/pr2API.cpp include/libpr2API/pr2API.h
$(CXX) $(CFLAGS) -c -o $@ $<
@@ -33,9 +33,6 @@
test_libpr2API: src/test/test_pr2API.cpp $(LIBP)
$(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
-bin/pr2_kin_kdl: src/test/pr2_kin_kdl.cpp $(LIBP)
- $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
-
clean:
-rm -f test_libpr2API $(LIBP) $(LIBS) bin/*
-rm -rf *.o $(PKG) $(PKG).dSYM
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-11 23:15:45 UTC (rev 762)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-11 23:28:49 UTC (rev 763)
@@ -39,7 +39,9 @@
#include <stdint.h>
#include <string>
+#ifdef KDL_KINEMATICS
#include <kdl_kinematics.h> // for kinematics using KDL -- util/kinematics/libKDL
+#endif
namespace PR2
{
@@ -449,8 +451,11 @@
//public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, double effectorPosition[], double effectorSpeed[]);
public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, NEWMAT::Matrix g);
- // KDL version of SetArmCartesianPosition
+
+#ifdef KDL_KINEMATICS
+// KDL version of SetArmCartesianPosition
public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, KDL::Frame f);
+#endif
/*! \fn
\brief Get the commanded position and speed for the end-effector
@@ -629,8 +634,10 @@
protected: PR2_CONTROL_MODE armControlMode[2];
protected: PR2_CONTROL_MODE baseControlMode;
protected: PR2Arm myArm;
- public: PR2_kinematics pr2_kin; // for kinematics using KDL.
+#ifdef KDL_KINEMATICS
+ public: PR2_kinematics pr2_kin; // for kinematics using KDL.
+#endif
/*! \fn
\brief - Oscillate the Hokuyo, return point cloud
*/
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-11 23:15:45 UTC (rev 762)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-11 23:28:49 UTC (rev 763)
@@ -11,7 +11,6 @@
<depend package="gazebo"/>
<depend package="pr2Core"/>
<depend package="libKinematics"/>
-<depend package="libKDL"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lpr2API"/>
</export>
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-11 23:15:45 UTC (rev 762)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-11 23:28:49 UTC (rev 763)
@@ -591,6 +591,8 @@
return PR2_ALL_OK;
};
+#ifdef KDL_KINEMATICS
+
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, KDL::Frame f)
{
KDL::JntArray q_init = KDL::JntArray(this->pr2_kin.nJnts);
@@ -612,8 +614,8 @@
for(int ii = 0; ii < 7; ii++)
this->SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),q_out(ii),0);
}
+#endif
-
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, NEWMAT::Matrix g)
{
NEWMAT::Matrix theta(8,8);
Deleted: pkg/trunk/util/kinematics/libKDL/include/kdl_kinematics.h
===================================================================
--- pkg/trunk/util/kinematics/libKDL/include/kdl_kinematics.h 2008-06-11 23:15:45 UTC (rev 762)
+++ pkg/trunk/util/kinematics/libKDL/include/kdl_kinematics.h 2008-06-11 23:28:49 UTC (rev 763)
@@ -1,47 +0,0 @@
-
-#ifndef KDL_KINEMATICS_H
-#define KDL_KINEMATICS_H
-
-#include <kdl/chain.hpp>
-#include <kdl/chainfksolver.hpp>
-#include <kdl/chainfksolverpos_recursive.hpp>
-#include <kdl/chainfksolvervel_recursive.hpp>
-#include <kdl/chainiksolvervel_pinv.hpp>
-#include <kdl/chainiksolverpos_nr.hpp>
-#include <kdl/kinfam_io.hpp>
-#include <kdl/frames_io.hpp>
-#include <kdl/framevel_io.hpp>
-#include <stdio.h>
-#include <iostream>
-#include <math.h>
-
-#include <pr2Core/pr2Core.h>
-
-#define RTOD(r) ((r)*180/M_PI)
-#define DTOR(d) ((d)*M_PI/180)
-
-double modulus_double(double a, double b);
-double angle_within_mod180(double ang);
-void angle_within_mod180(KDL::JntArray &q, int nJnts);
-
-class PR2_kinematics
-{
- private:
- KDL::Chain *chain;
- KDL::ChainFkSolverPos_recursive *fk_pos_solver;
- KDL::ChainIkSolverVel_pinv *ik_vel_solver;
- KDL::ChainIkSolverPos_NR *ik_pos_solver;
-
- public:
- int nJnts;
-
- public:
- PR2_kinematics();
- bool FK(const KDL::JntArray &q, KDL::Frame &f);
- bool IK(const KDL::JntArray &q_init, const KDL::Frame &f, KDL::JntArray &q_out);
-
-};
-
-#endif
-
-
Copied: pkg/trunk/util/kinematics/libKDL/include/libKDL/kdl_kinematics.h (from rev 761, pkg/trunk/util/kinematics/libKDL/include/kdl_kinematics.h)
===================================================================
--- pkg/trunk/util/kinematics/libKDL/include/libKDL/kdl_kinematics.h (rev 0)
+++ pkg/trunk/util/kinematics/libKDL/include/libKDL/kdl_kinematics.h 2008-06-11 23:28:49 UTC (rev 763)
@@ -0,0 +1,47 @@
+
+#ifndef KDL_KINEMATICS_H
+#define KDL_KINEMATICS_H
+
+#include <kdl/chain.hpp>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainfksolvervel_recursive.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/chainiksolverpos_nr.hpp>
+#include <kdl/kinfam_io.hpp>
+#include <kdl/frames_io.hpp>
+#include <kdl/framevel_io.hpp>
+#include <stdio.h>
+#include <iostream>
+#include <math.h>
+
+#include <pr2Core/pr2Core.h>
+
+#define RTOD(r) ((r)*180/M_PI)
+#define DTOR(d) ((d)*M_PI/180)
+
+double modulus_double(double a, double b);
+double angle_within_mod180(double ang);
+void angle_within_mod180(KDL::JntArray &q, int nJnts);
+
+class PR2_kinematics
+{
+ private:
+ KDL::Chain *chain;
+ KDL::ChainFkSolverPos_recursive *fk_pos_solver;
+ KDL::ChainIkSolverVel_pinv *ik_vel_solver;
+ KDL::ChainIkSolverPos_NR *ik_pos_solver;
+
+ public:
+ int nJnts;
+
+ public:
+ PR2_kinematics();
+ bool FK(const KDL::JntArray &q, KDL::Frame &f);
+ bool IK(const KDL::JntArray &q_init, const KDL::Frame &f, KDL::JntArray &q_out);
+
+};
+
+#endif
+
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-06-12 01:13:29
|
Revision: 769
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=769&view=rev
Author: tfoote
Date: 2008-06-11 18:13:36 -0700 (Wed, 11 Jun 2008)
Log Message:
-----------
adding zero timestamp = latest and updating constructor
Modified Paths:
--------------
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/transforms/libTF/doc/libTF_Manual.tex
pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h
pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp
pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp
pkg/trunk/util/transforms/libTF/testtf.cc
pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-12 01:13:36 UTC (rev 769)
@@ -298,7 +298,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(*this, true)
+ tf(*this, true, 200000000ULL, 200000000ULL) //nanoseconds
{
// TODO: get map via RPC
char* mapdata;
@@ -570,9 +570,9 @@
robotPose.y = 0;
robotPose.yaw = 0;
robotPose.frame = FRAMEID_ROBOT;
- //robotPose.time = 0; // request most recent pose
- robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
- laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
+ robotPose.time = 0; // request most recent pose
+ //robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
+ // laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
try
{
global_pose = this->tf.transformPose2D(FRAMEID_MAP, robotPose);
Modified: pkg/trunk/util/transforms/libTF/doc/libTF_Manual.tex
===================================================================
--- pkg/trunk/util/transforms/libTF/doc/libTF_Manual.tex 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/doc/libTF_Manual.tex 2008-06-12 01:13:36 UTC (rev 769)
@@ -242,7 +242,10 @@
\subsubsection{Constructor}
\index{libTF API!Constructor}
\begin{verbatim}
-TransformReference(ULLtime cache_time = DEFAULT_CACHE_TIME);
+TransformReference(bool interpolating = true,
+ ULLtime cache_time = DEFAULT_CACHE_TIME,
+ unsigned long long max_extrapolation_distance = DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
+
\end{verbatim}
This is the constructor for the class. It's optional argument is
how long to keep a history of the transforms. It is expressed in
@@ -292,6 +295,8 @@
unsigned int source_frame,
unsigned long long time);
\end{verbatim}
+getMatrix will return the homogeneous transformation matrix between the souce frame and the target frame.
+The standard approach will return the linearly interpolated
\paragraph{transform[DATA\_TYPE]}
\index{libTF API!transform[DATA\_TYPE]}
Modified: pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-06-12 01:13:36 UTC (rev 769)
@@ -161,7 +161,7 @@
/** Constructors **/
// Standard constructor max_cache_time is how long to cache transform data
- Quaternion3D(bool caching = true,
+ Quaternion3D(bool interpolating = true,
unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
unsigned long long max_extrapolation_time = DEFAULT_MAX_EXTRAPOLATION_TIME);
~Quaternion3D();
@@ -243,6 +243,9 @@
unsigned int list_length;
+ //Whether or not to interpolate
+ bool interpolating;
+
};
Modified: pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-06-12 01:13:36 UTC (rev 769)
@@ -203,7 +203,7 @@
/** Constructor
* \param How long to keep a history of transforms in nanoseconds
*/
- TransformReference(bool caching = true,
+ TransformReference(bool interpolating = true,
ULLtime cache_time = DEFAULT_CACHE_TIME,
unsigned long long max_extrapolation_distance = DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
~TransformReference();
@@ -319,7 +319,7 @@
public:
/** Constructor */
- RefFrame(bool caching = true,
+ RefFrame(bool interpolating = true,
unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
unsigned long long max_extrapolation_time = DEFAULT_MAX_EXTRAPOLATION_TIME);
@@ -349,8 +349,8 @@
/// How long to cache transform history
ULLtime cache_time;
- /// whether or not to cache
- bool caching;
+ /// whether or not to interpolate or extrapolate
+ bool interpolating;
/// whether or not to allow extrapolation
unsigned long long max_extrapolation_distance;
Modified: pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp
===================================================================
--- pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp 2008-06-12 01:13:36 UTC (rev 769)
@@ -45,7 +45,7 @@
};
-Quaternion3D::Quaternion3D(bool caching,
+Quaternion3D::Quaternion3D(bool interpolating,
unsigned long long max_cache_time,
unsigned long long _max_extrapolation_time):
max_storage_time(max_cache_time),
@@ -53,11 +53,12 @@
max_extrapolation_time(_max_extrapolation_time),
first(NULL),
last(NULL),
- list_length(0)
+ list_length(0),
+ interpolating(interpolating)
{
//Turn of caching, this should only keep a liked list of lenth 1
// Thus returning only the latest
- if (!caching) max_length_linked_list = 1;
+ // if (!caching) max_length_linked_list = 1; //Removed, simply use 0 time to get first value
pthread_mutex_init( &linked_list_mutex, NULL);
//fixme Normalize();
@@ -504,7 +505,10 @@
}
else
{
- interpolate(p_temp_1, p_temp_2, time, buff);
+ if(interpolating)
+ interpolate(p_temp_1, p_temp_2, time, buff);
+ else
+ buff = p_temp_1;
retval = true;
}
@@ -669,8 +673,8 @@
return 0;
}
- //Case one element list
- else if (first->next == NULL)
+ //Case one element list or latest value is wanted.
+ else if (first->next == NULL || target_time == 0)
{
one = first->data;
time_diff = target_time - first->data.time;
Modified: pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp
===================================================================
--- pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-06-12 01:13:36 UTC (rev 769)
@@ -35,21 +35,21 @@
using namespace libTF;
-TransformReference::RefFrame::RefFrame(bool caching,
+TransformReference::RefFrame::RefFrame(bool interpolating,
unsigned long long max_cache_time,
unsigned long long max_extrapolation_distance) :
- Quaternion3D(caching, max_cache_time, max_extrapolation_distance),
+ Quaternion3D(interpolating, max_cache_time, max_extrapolation_distance),
parent(TransformReference::NO_PARENT)
{
return;
}
-TransformReference::TransformReference(bool caching,
+TransformReference::TransformReference(bool interpolating,
ULLtime cache_time,
unsigned long long max_extrapolation_distance):
cache_time(cache_time),
- caching (caching),
+ interpolating (interpolating),
max_extrapolation_distance(max_extrapolation_distance)
{
@@ -83,7 +83,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
+ frames[frameID] = new RefFrame(interpolating, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromEuler(a,b,c,d,e,f,time);
@@ -95,7 +95,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
+ frames[frameID] = new RefFrame(interpolating, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromDH(a,b,c,d,time);
@@ -109,7 +109,7 @@
//TODO check and throw exception if matrix wrong size
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
+ frames[frameID] = new RefFrame(interpolating, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromMatrix(matrix_in,time);
@@ -122,7 +122,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
+ frames[frameID] = new RefFrame(interpolating, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromQuaternion(xt, yt, zt, xr, yr, zr, w,time);
Modified: pkg/trunk/util/transforms/libTF/testtf.cc
===================================================================
--- pkg/trunk/util/transforms/libTF/testtf.cc 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/libTF/testtf.cc 2008-06-12 01:13:36 UTC (rev 769)
@@ -11,7 +11,7 @@
int
main(void)
{
- libTF::TransformReference mTR(false);
+ libTF::TransformReference mTR(true, 0);
timeval temp_time_struct;
gettimeofday(&temp_time_struct,NULL);
unsigned long long atime = temp_time_struct.tv_sec * 1000000000ULL + (unsigned long long)temp_time_struct.tv_usec * 1000ULL;
Modified: pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
===================================================================
--- pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-06-12 01:13:36 UTC (rev 769)
@@ -62,7 +62,7 @@
{
public:
//Constructor
- rosTFClient(ros::node & rosnode, bool caching = true, unsigned long long max_extrapolation_distance = libTF::TransformReference::DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
+ rosTFClient(ros::node & rosnode, bool interpolating = true, unsigned long long max_cache_time = libTF::TransformReference::DEFAULT_CACHE_TIME, unsigned long long max_extrapolation_distance = libTF::TransformReference::DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
// PointCloudFloat32 transformPointCloud(unsigned int target_frame, const PointCloudFloat32 & cloudIn); // todo switch after ticket:232
std_msgs::PointCloudFloat32 transformPointCloud(unsigned int target_frame, std_msgs::PointCloudFloat32 & cloudIn);
Modified: pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
===================================================================
--- pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-06-12 00:57:25 UTC (rev 768)
+++ pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-06-12 01:13:36 UTC (rev 769)
@@ -33,10 +33,11 @@
#include "rosTF/rosTF.h"
rosTFClient::rosTFClient(ros::node & rosnode,
- bool caching,
+ bool interpolating,
+ unsigned long long max_cache_time,
unsigned long long max_extrapolation_distance):
- TransformReference(caching,
- TransformReference::DEFAULT_CACHE_TIME,
+ TransformReference(interpolating,
+ max_cache_time,
max_extrapolation_distance),
myNode(rosnode)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-06-12 19:51:36
|
Revision: 776
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=776&view=rev
Author: gerkey
Date: 2008-06-12 12:51:44 -0700 (Thu, 12 Jun 2008)
Log Message:
-----------
updated to new message types and Time usage; moved to map_server for occ grid retrieval
Modified Paths:
--------------
pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
pkg/trunk/nav/amcl_player/Makefile
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/manifest.xml
pkg/trunk/nav/nav_view/Makefile
pkg/trunk/nav/nav_view/nav_view.cpp
pkg/trunk/nav/wavefront_player/Makefile
pkg/trunk/nav/wavefront_player/manifest.xml
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
===================================================================
--- pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-06-12 19:51:44 UTC (rev 776)
@@ -1,10 +1,11 @@
<launch>
<ns name="wg">
- <node pkg="rosstage" type="rosstage" args="willow-erratic.world" respawn="true" />
- <node pkg="amcl_player" type="amcl_player" args="willow-full.png 0.1" respawn="true" />
- <node pkg="wavefront_player" type="wavefront_player" args="willow-full.png 0.1" respawn="true" />
- <node pkg="nav_view" type="nav_view" args="willow-full.png 0.1" respawn="true" />
+ <node pkg="rosstage" type="rosstage" args="willow-erratic.world" respawn="false" />
+ <node pkg="map_server" type="map_server" args="willow-full.png 0.1" respawn="false" />
+ <node pkg="amcl_player" type="amcl_player" respawn="false" />
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" />
+ <node pkg="nav_view" type="nav_view" respawn="false" />
</ns>
</launch>
Modified: pkg/trunk/nav/amcl_player/Makefile
===================================================================
--- pkg/trunk/nav/amcl_player/Makefile 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/amcl_player/Makefile 2008-06-12 19:51:44 UTC (rev 776)
@@ -1,6 +1,5 @@
SRC = amcl_player.cc
OUT = amcl_player
PKG = amcl_player
-CFLAGS = -Wall -Werror `pkg-config --cflags gdk-pixbuf-2.0`
-LFLAGS = `pkg-config --libs gdk-pixbuf-2.0` -lplayerdrivers
+LFLAGS = -lplayerdrivers
include $(shell rospack find mk)/node.mk
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-06-12 19:51:44 UTC (rev 776)
@@ -46,21 +46,9 @@
@section usage Usage
@verbatim
-$ amcl_player <map> <res> [standard ROS args]
+$ amcl_player
@endverbatim
-@param map An image file to load as an occupancy grid map. The robot will be localized against this map using laser scans. The lower-left pixel of the map is assigned the pose (0,0,0).
-@param res The resolution of the map, in meters, pixel.
-
-@todo Remove the map and res arguments in favor map retrieval via ROSRPC.
-@todo Remove the x,y,th arguments and expose the particle filter initialization via a ROS topic.
-
-@par Example
-
-@verbatim
-$ amcl_player mymap.png 0.1 10.4 31.2 90.0
-@endverbatim
-
<hr>
@section topic ROS topics
@@ -99,22 +87,19 @@
#include <ros/node.h>
// Messages that I need
-#include <std_msgs/MsgLaserScan.h>
-#include <std_msgs/MsgRobotBase2DOdom.h>
-#include <std_msgs/MsgParticleCloud2D.h>
-#include <std_msgs/MsgPose2DFloat32.h>
+#include <std_msgs/LaserScan.h>
+#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/ParticleCloud2D.h>
+#include <std_msgs/Pose2DFloat32.h>
+#include <std_srvs/StaticMap.h>
// For transform support
#include <rosTF/rosTF.h>
-#include <gdk-pixbuf/gdk-pixbuf.h>
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
// check that given coords are valid (i.e., on the map)
#define MAP_VALID(mf, i, j) ((i >= 0) && (i < mf->sx) && (j >= 0) && (j < mf->sy))
-double get_time(void);
-int read_map_from_image(int* size_x, int* size_y, char** mapdata,
- const char* fname, int negate);
#define PLAYER_QUEUE_LEN 32
@@ -125,7 +110,7 @@
class AmclNode: public ros::node, public Driver
{
public:
- AmclNode(char* fname, double res);
+ AmclNode();
~AmclNode();
int Setup() {return(0);}
@@ -146,11 +131,11 @@
ConfigFile* cf;
// incoming messages
- MsgRobotBase2DOdom localizedOdomMsg;
- MsgParticleCloud2D particleCloudMsg;
- MsgRobotBase2DOdom odomMsg;
- MsgLaserScan laserMsg;
- MsgPose2DFloat32 initialPoseMsg;
+ std_msgs::RobotBase2DOdom localizedOdomMsg;
+ std_msgs::ParticleCloud2D particleCloudMsg;
+ std_msgs::RobotBase2DOdom odomMsg;
+ std_msgs::LaserScan laserMsg;
+ std_msgs::Pose2DFloat32 initialPoseMsg;
// Message callbacks
void odomReceived();
@@ -178,20 +163,14 @@
double resolution;
};
-#define USAGE "USAGE: amcl_player <map.png> <res>"
+#define USAGE "USAGE: amcl_player"
int
main(int argc, char** argv)
{
- if(argc < 3)
- {
- puts(USAGE);
- exit(-1);
- }
-
ros::init(argc, argv);
- AmclNode an(argv[1],atof(argv[2]));
+ AmclNode an;
// Start up the robot
if(an.start() != 0)
@@ -212,7 +191,7 @@
return(0);
}
-AmclNode::AmclNode(char* fname, double res) :
+AmclNode::AmclNode() :
ros::node("amcl_player"),
Driver(NULL,-1,false,PLAYER_QUEUE_LEN)
{
@@ -224,19 +203,42 @@
playerxdr_ftable_init();
puts("advertising");
- advertise<MsgRobotBase2DOdom>("localizedpose");
- advertise<MsgParticleCloud2D>("particlecloud");
+ advertise<std_msgs::RobotBase2DOdom>("localizedpose");
+ advertise<std_msgs::ParticleCloud2D>("particlecloud");
puts("subscribing");
subscribe("odom", odomMsg, &AmclNode::odomReceived);
subscribe("scan", laserMsg, &AmclNode::laserReceived);
subscribe("initialpose", initialPoseMsg, &AmclNode::initialPoseReceived);
puts("done");
- // TODO: get map via RPC
- assert(read_map_from_image(&this->sx, &this->sy, &this->mapdata, fname, 0)
- == 0);
- this->resolution = res;
+ // get map via RPC
+ std_srvs::StaticMap::request req;
+ std_srvs::StaticMap::response resp;
+ puts("Requesting the map...");
+ assert(ros::service::call("static_map", req, resp));
+ printf("Received a %d X %d map @ %.3f m/pix\n",
+ resp.map.width,
+ resp.map.height,
+ resp.map.resolution);
+ this->sx = resp.map.width;
+ this->sy = resp.map.height;
+ this->resolution = resp.map.resolution;
+ // Convert to player format
+ this->mapdata = new char[this->sx*this->sy];
+ for(int i=0;i<this->sx*this->sy;i++)
+ {
+ if(resp.map.data[i] == 0)
+ this->mapdata[i] = -1;
+ else if(resp.map.data[i] == 100)
+ this->mapdata[i] = +1;
+ else
+ this->mapdata[i] = 0;
+ }
+
+ //assert(read_map_from_image(&this->sx, &this->sy, &this->mapdata, fname, 0)
+ //== 0);
+
// TODO: automatically convert between string and player_devaddr_t
// representations
@@ -389,11 +391,12 @@
localizedOdomMsg.pos.x = pdata->pos.px;
localizedOdomMsg.pos.y = pdata->pos.py;
localizedOdomMsg.pos.th = pdata->pos.pa;
- localizedOdomMsg.header.stamp.sec = (unsigned long)floor(hdr->timestamp);
- localizedOdomMsg.header.stamp.nsec =
- (unsigned long)rint(1e9 * (hdr->timestamp -
- localizedOdomMsg.header.stamp.sec));
- localizedOdomMsg.__timestamp_override = true;
+ localizedOdomMsg.header.stamp.from_double(hdr->timestamp);
+ //localizedOdomMsg.header.stamp.sec = (unsigned long)floor(hdr->timestamp);
+ //localizedOdomMsg.header.stamp.nsec =
+ //(unsigned long)rint(1e9 * (hdr->timestamp -
+ //localizedOdomMsg.header.stamp.sec));
+ //localizedOdomMsg.__timestamp_override = true;
publish("localizedpose", localizedOdomMsg);
// Also request and publish the particle cloud
@@ -619,8 +622,7 @@
}
pdata.id = this->laserMsg.header.seq;
- double timestamp = this->laserMsg.header.stamp.sec +
- this->laserMsg.header.stamp.nsec / 1e9;
+ double timestamp = this->laserMsg.header.stamp.to_double();
this->Driver::Publish(this->laser_addr,
PLAYER_MSGTYPE_DATA,
@@ -653,8 +655,7 @@
pdata.vel.pa = this->odomMsg.vel.th;
pdata.stall = this->odomMsg.stall;
- double timestamp = this->odomMsg.header.stamp.sec +
- this->odomMsg.header.stamp.nsec / 1e9;
+ double timestamp = this->odomMsg.header.stamp.to_double();
this->Driver::Publish(this->position2d_addr,
PLAYER_MSGTYPE_DATA,
@@ -663,72 +664,3 @@
×tamp);
}
-int
-read_map_from_image(int* size_x, int* size_y, char** mapdata,
- const char* fname, int negate)
-{
- GdkPixbuf* pixbuf;
- guchar* pixels;
- guchar* p;
- int rowstride, n_channels, bps;
- GError* error = NULL;
- int i,j,k;
- double occ;
- int color_sum;
- double color_avg;
-
- // Initialize glib
- g_type_init();
-
- printf("MapFile loading image file: %s...", fname);
- fflush(stdout);
-
- // Read the image
- if(!(pixbuf = gdk_pixbuf_new_from_file(fname, &error)))
- {
- printf("failed to open image file %s", fname);
- return(-1);
- }
-
- *size_x = gdk_pixbuf_get_width(pixbuf);
- *size_y = gdk_pixbuf_get_height(pixbuf);
-
- assert(*mapdata = (char*)malloc(sizeof(char) * (*size_x) * (*size_y)));
-
- rowstride = gdk_pixbuf_get_rowstride(pixbuf);
- bps = gdk_pixbuf_get_bits_per_sample(pixbuf)/8;
- n_channels = gdk_pixbuf_get_n_channels(pixbuf);
- //if(gdk_pixbuf_get_has_alpha(pixbuf))
- //n_channels++;
-
- // Read data
- pixels = gdk_pixbuf_get_pixels(pixbuf);
- for(j = 0; j < *size_y; j++)
- {
- for (i = 0; i < *size_x; i++)
- {
- p = pixels + j*rowstride + i*n_channels*bps;
- color_sum = 0;
- for(k=0;k<n_channels;k++)
- color_sum += *(p + (k * bps));
- color_avg = color_sum / (double)n_channels;
-
- if(negate)
- occ = color_avg / 255.0;
- else
- occ = (255 - color_avg) / 255.0;
- if(occ > 0.95)
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = +1;
- else if(occ < 0.1)
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = -1;
- else
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = 0;
- }
- }
-
- gdk_pixbuf_unref(pixbuf);
-
- puts("Done.");
- printf("MapFile read a %d X %d map\n", *size_x, *size_y);
- return(0);
-}
Modified: pkg/trunk/nav/amcl_player/manifest.xml
===================================================================
--- pkg/trunk/nav/amcl_player/manifest.xml 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/amcl_player/manifest.xml 2008-06-12 19:51:44 UTC (rev 776)
@@ -5,5 +5,5 @@
<depend package="roscpp" />
<depend package="player" />
<depend package="rosTF" />
- <depend package="std_msgs" />
+ <depend package="std_srvs" />
</package>
Modified: pkg/trunk/nav/nav_view/Makefile
===================================================================
--- pkg/trunk/nav/nav_view/Makefile 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/nav_view/Makefile 2008-06-12 19:51:44 UTC (rev 776)
@@ -1,4 +1,5 @@
SRC = nav_view.cpp
OUT = nav_view
PKG = nav_view
+CFLAGS = -g
include $(shell rospack find mk)/node.mk
Modified: pkg/trunk/nav/nav_view/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view/nav_view.cpp 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/nav_view/nav_view.cpp 2008-06-12 19:51:44 UTC (rev 776)
@@ -125,8 +125,6 @@
bool setting_theta;
double gx,gy;
- std_srvs::StaticMap::response map_resp;
-
rosTFClient tf;
ros::time::clock myClock;
@@ -209,8 +207,8 @@
ros::init(argc, argv);
NavView view;
- //if(!view.load_map())
- if(!view.load_map_from_image(argv[1],atof(argv[2])))
+ if(!view.load_map())
+ //if(!view.load_map_from_image(argv[1],atof(argv[2])))
puts("WARNING: failed to load map");
view.main_loop();
ros::fini();
@@ -380,62 +378,66 @@
NavView::load_map()
{
std_srvs::StaticMap::request req;
- std_srvs::StaticMap::response res;
+ std_srvs::StaticMap::response resp;
puts("Requesting the map...");
- if(ros::service::call("static_map", req, res))
+ if(ros::service::call("static_map", req, resp))
{
puts("success");
- map_resp = res;
- map_res = map_resp.map.resolution;
- map_width = map_resp.map.width;
- map_height = map_resp.map.height;
-
printf("Received a %d X %d map @ %.3f m/pix\n",
- map_resp.map.width,
- map_resp.map.height,
- map_resp.map.resolution);
+ resp.map.width,
+ resp.map.height,
+ resp.map.resolution);
+ map_res = resp.map.resolution;
+
+ // Pad dimensions to power of 2
+ map_width = (int)pow(2,ceil(log2(resp.map.width)));
+ map_height = (int)pow(2,ceil(log2(resp.map.height)));
+
+ printf("Padded dimensions to %d X %d\n", map_width, map_height);
+
// Expand it to be RGB data
- unsigned char* pixels =
- new unsigned char[map_resp.map.width * map_resp.map.height * 4];
- for(unsigned int i=0;i<map_resp.map.width*map_resp.map.height;i++)
+ unsigned char* pixels = new unsigned char[map_width * map_height * 3];
+ assert(pixels);
+ memset(pixels,255,map_width*map_height*3);
+ for(unsigned int j=0;j<resp.map.height;j++)
{
- unsigned char val;
- if(map_resp.map.data[i] == 100)
- val = 0;
- else if(map_resp.map.data[i] == 0)
- val = 255;
- else
- val = 127;
+ for(unsigned int i=0;i<resp.map.width;i++)
+ {
+ unsigned char val;
+ if(resp.map.data[j*resp.map.width+i] == 100)
+ val = 0;
+ else if(resp.map.data[j*resp.map.width+i] == 0)
+ val = 255;
+ else
+ val = 127;
- pixels[4*i] = val;
- pixels[4*i+1] = val;
- pixels[4*i+2] = val;
- pixels[4*i+3] = 255;
+ int pidx = 3*(j*map_width + i);
+ pixels[pidx+0] = val;
+ pixels[pidx+1] = val;
+ pixels[pidx+2] = val;
+ }
}
- GLint nOfColors = 4;
- GLenum texture_format = GL_RGBA;
+ glEnable( GL_TEXTURE_2D );
// Have OpenGL generate a texture object handle for us
- glGenTextures( 1, &map_texture );
+ glGenTextures(1, &map_texture);
// Bind the texture object
- glBindTexture( GL_TEXTURE_2D, map_texture );
+ glBindTexture(GL_TEXTURE_2D, map_texture);
+ glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+
// Set the texture's stretching properties
- glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
- GL_LINEAR );
- glTexParameteri( GL_TEXTURE_2D,
- GL_TEXTURE_MAG_FILTER, GL_LINEAR );
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
- // Edit the texture object's image data using
- //the information SDL_Surface gives us
- glTexImage2D( GL_TEXTURE_2D, 0, nOfColors,
- map_width, map_height, 0,
- texture_format,
- GL_UNSIGNED_BYTE,
- pixels );
+ // Edit the texture object's image data
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB,
+ map_width, map_height, 0,
+ GL_RGB, GL_UNSIGNED_BYTE,
+ pixels);
return(true);
}
else
@@ -464,14 +466,13 @@
// Check that the image's width is a power of 2
if((temp->w & (temp->w - 1)) != 0) {
- printf("warning: image.bmp's width is not a power of 2\n");
+ printf("warning: image width is not a power of 2\n");
}
// Also check if the height is a power of 2
if((temp->h & (temp->h - 1)) != 0) {
- printf("warning: image.bmp's height is not a power of 2\n");
+ printf("warning: image height is not a power of 2\n");
}
-
// Flip the image vertically
map_surface = flip_vert(temp);
SDL_FreeSurface(temp);
Modified: pkg/trunk/nav/wavefront_player/Makefile
===================================================================
--- pkg/trunk/nav/wavefront_player/Makefile 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/wavefront_player/Makefile 2008-06-12 19:51:44 UTC (rev 776)
@@ -1,6 +1,5 @@
SRC = wavefront_player.cc
OUT = wavefront_player
PKG = wavefront_player
-CFLAGS = -Wall -Werror `pkg-config --cflags gdk-pixbuf-2.0`
-LFLAGS = `pkg-config --libs gdk-pixbuf-2.0` -lwavefront_standalone
+LFLAGS = -lwavefront_standalone
include $(shell rospack find mk)/node.mk
Modified: pkg/trunk/nav/wavefront_player/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront_player/manifest.xml 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/wavefront_player/manifest.xml 2008-06-12 19:51:44 UTC (rev 776)
@@ -4,6 +4,6 @@
<license>BSD</license>
<depend package="roscpp" />
<depend package="player" />
- <depend package="std_msgs" />
+ <depend package="std_srvs" />
<depend package="rosTF" />
</package>
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-12 19:51:44 UTC (rev 776)
@@ -64,20 +64,9 @@
@section usage Usage
@verbatim
-$ wavefront_player <map> <res> [standard ROS args]
+$ wavefront_player
@endverbatim
-@param map An image file to load as an occupancy grid map. The robot will be localized against this map using laser scans. The lower-left pixel of the map is assigned the pose (0,0,0).
-@param res The resolution of the map, in meters, pixel.
-
-@todo Remove the map and res arguments in favor map retrieval via ROSRPC.
-
-@par Example
-
-@verbatim
-$ wavefront_player mymap.png 0.1
-@endverbatim
-
<hr>
@section topic ROS topics
@@ -124,15 +113,16 @@
#include <ros/node.h>
// The messages that we'll use
-#include <std_msgs/MsgPlanner2DState.h>
-#include <std_msgs/MsgPlanner2DGoal.h>
-#include <std_msgs/MsgBaseVel.h>
+#include <std_msgs/Planner2DState.h>
+#include <std_msgs/Planner2DGoal.h>
+#include <std_msgs/BaseVel.h>
//#include <std_msgs/MsgRobotBase2DOdom.h>
-#include <std_msgs/MsgLaserScan.h>
-#include <std_msgs/MsgPose2DFloat32.h>
+#include <std_msgs/LaserScan.h>
+#include <std_msgs/Pose2DFloat32.h>
+#include <std_srvs/StaticMap.h>
// For GUI debug
-#include <std_msgs/MsgPolyline2D.h>
+#include <std_msgs/Polyline2D.h>
// For transform support
#include <rosTF/rosTF.h>
@@ -145,15 +135,6 @@
#define RTOD(a) ((a)*180.0/M_PI)
#define SIGN(x) (((x) < 0.0) ? -1 : 1)
-//void draw_cspace(plan_t* plan, const char* fname);
-//void draw_path(plan_t* plan, double lx, double ly, const char* fname);
-#include <gdk-pixbuf/gdk-pixbuf.h>
-// compute linear index for given map coords
-#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
-// computes the signed minimum difference between the two angles.
-int read_map_from_image(int* size_x, int* size_y, char** mapdata,
- const char* fname, int negate);
-
// A bunch of x,y points, with a timestamp
typedef struct
{
@@ -201,7 +182,7 @@
// Map update paramters (for adding obstacles)
double laser_maxrange;
ros::Duration laser_buffer_time;
- std::list<MsgLaserScan> buffered_laser_scans;
+ std::list<std_msgs::LaserScan> buffered_laser_scans;
std::list<laser_pts_t> laser_scans;
double* laser_hitpts;
size_t laser_hitpts_len, laser_hitpts_size;
@@ -212,13 +193,13 @@
double tvmin, tvmax, avmin, avmax, amin, amax;
// incoming/outgoing messages
- MsgPlanner2DGoal goalMsg;
+ std_msgs::Planner2DGoal goalMsg;
//MsgRobotBase2DOdom odomMsg;
- MsgLaserScan laserMsg;
- MsgPolyline2D polylineMsg;
- MsgPolyline2D pointcloudMsg;
- MsgPlanner2DState pstate;
- std::vector<MsgLaserScan> laserScans;
+ std_msgs::LaserScan laserMsg;
+ std_msgs::Polyline2D polylineMsg;
+ std_msgs::Polyline2D pointcloudMsg;
+ std_msgs::Planner2DState pstate;
+ std::vector<std_msgs::LaserScan> laserScans;
//MsgRobotBase2DOdom prevOdom;
bool firstodom;
@@ -236,7 +217,7 @@
// Transform client
rosTFClient tf;
- WavefrontNode(char* fname, double res);
+ WavefrontNode();
~WavefrontNode();
// Stop the robot
@@ -247,20 +228,14 @@
void sleep(double loopstart);
};
-#define USAGE "USAGE: wavefront_player <map.png> <res>"
+#define USAGE "USAGE: wavefront_player"
int
main(int argc, char** argv)
{
- if(argc < 3)
- {
- puts(USAGE);
- exit(-1);
- }
-
ros::init(argc,argv);
- WavefrontNode wn(argv[1],atof(argv[2]));
+ WavefrontNode wn;
struct timeval curr;
while(wn.ok())
@@ -273,7 +248,7 @@
return(0);
}
-WavefrontNode::WavefrontNode(char* fname, double res) :
+WavefrontNode::WavefrontNode() :
ros::node("wavfront_player"),
planner_state(NO_GOAL),
enable(true),
@@ -300,10 +275,30 @@
amax(DTOR(40.0)),
tf(*this, true, 200000000ULL, 200000000ULL) //nanoseconds
{
- // TODO: get map via RPC
+ // get map via RPC
+ std_srvs::StaticMap::request req;
+ std_srvs::StaticMap::response resp;
+ puts("Requesting the map...");
+ assert(ros::service::call("static_map", req, resp));
+ printf("Received a %d X %d map @ %.3f m/pix\n",
+ resp.map.width,
+ resp.map.height,
+ resp.map.resolution);
char* mapdata;
int sx, sy;
- assert(read_map_from_image(&sx, &sy, &mapdata, fname, 0) == 0);
+ sx = resp.map.width;
+ sy = resp.map.height;
+ // Convert to player format
+ mapdata = new char[sx*sy];
+ for(int i=0;i<sx*sy;i++)
+ {
+ if(resp.map.data[i] == 0)
+ mapdata[i] = -1;
+ else if(resp.map.data[i] == 100)
+ mapdata[i] = +1;
+ else
+ mapdata[i] = 0;
+ }
assert((this->plan = plan_alloc(this->robot_radius+this->safety_dist,
this->robot_radius+this->safety_dist,
@@ -322,9 +317,9 @@
for(int i=0;i<sx;i++)
this->plan->cells[i+j*sx].occ_state = mapdata[i+j*sx];
}
- free(mapdata);
+ delete[] mapdata;
- this->plan->scale = res;
+ this->plan->scale = resp.map.resolution;
this->plan->size_x = sx;
this->plan->size_y = sy;
this->plan->origin_x = 0.0;
@@ -347,10 +342,10 @@
FRAMEID_ROBOT,
0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
- advertise<MsgPlanner2DState>("state");
- advertise<MsgPolyline2D>("gui_path");
- advertise<MsgPolyline2D>("gui_laser");
- advertise<MsgBaseVel>("cmd_vel");
+ advertise<std_msgs::Planner2DState>("state");
+ advertise<std_msgs::Polyline2D>("gui_path");
+ advertise<std_msgs::Polyline2D>("gui_laser");
+ advertise<std_msgs::BaseVel>("cmd_vel");
subscribe("goal", goalMsg, &WavefrontNode::goalReceived);
subscribe("scan", laserMsg, &WavefrontNode::laserReceived);
}
@@ -385,8 +380,9 @@
WavefrontNode::laserReceived()
{
// Copy and push this scan into the list of buffered scans
- MsgLaserScan newscan;
+ std_msgs::LaserScan newscan(laserMsg);
// Do a deep copy
+ /*
newscan.header.stamp.sec = laserMsg.header.stamp.sec;
newscan.header.stamp.nsec = laserMsg.header.stamp.nsec;
newscan.header.frame_id = laserMsg.header.frame_id;
@@ -396,10 +392,11 @@
newscan.angle_increment = laserMsg.angle_increment;
newscan.set_ranges_size(laserMsg.get_ranges_size());
memcpy(newscan.ranges,laserMsg.ranges,laserMsg.get_ranges_size()*sizeof(float));
+ */
this->buffered_laser_scans.push_back(newscan);
// Iterate through the buffered scans, trying to interpolate each one
- for(std::list<MsgLaserScan>::iterator it = this->buffered_laser_scans.begin();
+ for(std::list<std_msgs::LaserScan>::iterator it = this->buffered_laser_scans.begin();
it != this->buffered_laser_scans.end();
it++)
{
@@ -414,8 +411,9 @@
libTF::TFPose2D local,global;
local.frame = it->header.frame_id;
- local.time = it->header.stamp.sec * 1000000000ULL +
- it->header.stamp.nsec;
+ local.time = it->header.stamp.to_ull();
+ //local.time = it->header.stamp.sec * 1000000000ULL +
+ //it->header.stamp.nsec;
float b=it->angle_min;
float* r=it->ranges;
unsigned int i;
@@ -462,7 +460,10 @@
{
pts.pts_num = cnt;
this->laser_scans.push_back(pts);
- delete[] it->ranges;
+ // Don't delete ranges here, because the LaserScan destructor does it
+ // when the object falls out of scope after being erased from the
+ // list.
+ //delete[] it->ranges;
it = this->buffered_laser_scans.erase(it);
it--;
}
@@ -540,13 +541,13 @@
// Declare this globally, so that it never gets desctructed (message
// desctruction causes master disconnect)
-MsgBaseVel* cmdvel;
+std_msgs::BaseVel* cmdvel;
void
WavefrontNode::sendVelCmd(double vx, double vy, double vth)
{
if(!cmdvel)
- cmdvel = new MsgBaseVel();
+ cmdvel = new std_msgs::BaseVel();
cmdvel->vx = vx;
cmdvel->vw = vth;
this->ros::node::publish("cmd_vel", *cmdvel);
@@ -739,73 +740,3 @@
usleep((unsigned int)rint(tdiff*1e6));
}
-int
-read_map_from_image(int* size_x, int* size_y, char** mapdata,
- const char* fname, int negate)
-{
- GdkPixbuf* pixbuf;
- guchar* pixels;
- guchar* p;
- int rowstride, n_channels, bps;
- GError* error = NULL;
- int i,j,k;
- double occ;
- int color_sum;
- double color_avg;
-
- // Initialize glib
- g_type_init();
-
- printf("MapFile loading image file: %s...", fname);
- fflush(stdout);
-
- // Read the image
- if(!(pixbuf = gdk_pixbuf_new_from_file(fname, &error)))
- {
- printf("failed to open image file %s", fname);
- return(-1);
- }
-
- *size_x = gdk_pixbuf_get_width(pixbuf);
- *size_y = gdk_pixbuf_get_height(pixbuf);
-
- assert(*mapdata = (char*)malloc(sizeof(char) * (*size_x) * (*size_y)));
-
- rowstride = gdk_pixbuf_get_rowstride(pixbuf);
- bps = gdk_pixbuf_get_bits_per_sample(pixbuf)/8;
- n_channels = gdk_pixbuf_get_n_channels(pixbuf);
- //if(gdk_pixbuf_get_has_alpha(pixbuf))
- //n_channels++;
-
- // Read data
- pixels = gdk_pixbuf_get_pixels(pixbuf);
- for(j = 0; j < *size_y; j++)
- {
- for (i = 0; i < *size_x; i++)
- {
- p = pixels + j*rowstride + i*n_channels*bps;
- color_sum = 0;
- for(k=0;k<n_channels;k++)
- color_sum += *(p + (k * bps));
- color_avg = color_sum / (double)n_channels;
-
- if(negate)
- occ = color_avg / 255.0;
- else
- occ = (255 - color_avg) / 255.0;
- if(occ > 0.5)
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = +1;
- else if(occ < 0.1)
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = -1;
- else
- (*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = 0;
- }
- }
-
- gdk_pixbuf_unref(pixbuf);
-
- puts("Done.");
- printf("MapFile read a %d X %d map\n", *size_x, *size_y);
- return(0);
-}
-
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-06-12 17:09:40 UTC (rev 775)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-06-12 19:51:44 UTC (rev 776)
@@ -242,11 +242,12 @@
// TODO: get the frame ID from somewhere
this->laserMsg.header.frame_id = FRAMEID_LASER;
- this->laserMsg.header.stamp.sec =
- (unsigned long)floor(world->SimTimeNow() / 1e6);
- this->laserMsg.header.stamp.nsec =
- (unsigned long)rint(1e3 * (world->SimTimeNow() -
- this->laserMsg.header.stamp.sec * 1e6));
+ this->laserMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
+ //this->laserMsg.header.stamp.sec =
+ //(unsigned long)floor(world->SimTimeNow() / 1e6);
+ //this->laserMsg.header.stamp.nsec =
+ //(unsigned long)rint(1e3 * (world->SimTimeNow() -
+ //this->laserMsg.header.stamp.sec * 1e6));
this->laserMsg.__timestamp_override = true;
publish("scan",this->laserMsg);
}
@@ -264,11 +265,12 @@
// TODO: get the frame ID from somewhere
this->odomMsg.header.frame_id = FRAMEID_ODOM;
- this->odomMsg.header.stamp.sec =
- (unsigned long)floor(world->SimTimeNow() / 1e6);
- this->odomMsg.header.stamp.nsec =
- (unsigned long)rint(1e3 * (world->SimTimeNow() -
- this->odomMsg.header.stamp.sec * 1e6));
+ this->odomMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
+ //this->odomMsg.header.stamp.sec =
+ //(unsigned long)floor(world->SimTimeNow() / 1e6);
+ //this->odomMsg.header.stamp.nsec =
+ //(unsigned long)rint(1e3 * (world->SimTimeNow() -
+ //this->odomMsg.header.stamp.sec * 1e6));
this->odomMsg.__timestamp_override = true;
publish("odom",this->odomMsg);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-06-12 20:11:37
|
Revision: 781
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=781&view=rev
Author: gerkey
Date: 2008-06-12 13:11:40 -0700 (Thu, 12 Jun 2008)
Log Message:
-----------
moved flatland into simulators
Modified Paths:
--------------
pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
pkg/trunk/visualization/pr2_gui/Makefile
Added Paths:
-----------
pkg/trunk/simulators/flatland/
Removed Paths:
-------------
pkg/trunk/nav/sim/flatland/
Modified: pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
===================================================================
--- pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-06-12 20:09:55 UTC (rev 780)
+++ pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-06-12 20:11:40 UTC (rev 781)
@@ -5,7 +5,7 @@
<node pkg="map_server" type="map_server" args="willow-full.png 0.1" respawn="false" />
<node pkg="amcl_player" type="amcl_player" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" />
- <node pkg="nav_view" type="nav_view" respawn="false" />
+ <!-- node pkg="nav_view" type="nav_view" respawn="true" /-->
</ns>
</launch>
Copied: pkg/trunk/simulators/flatland (from rev 778, pkg/trunk/nav/sim/flatland)
Modified: pkg/trunk/visualization/pr2_gui/Makefile
===================================================================
--- pkg/trunk/visualization/pr2_gui/Makefile 2008-06-12 20:09:55 UTC (rev 780)
+++ pkg/trunk/visualization/pr2_gui/Makefile 2008-06-12 20:11:40 UTC (rev 781)
@@ -1,29 +1,32 @@
#############################################################################
# Makefile for building: bin/pr2_gui
-# Generated by qmake (2.01a) (Qt 4.3.4) on: Tue Jun 10 15:04:20 2008
+# Generated by qmake (2.01a) (Qt 4.2.3) on: Wed Jun 11 16:22:05 2008
# Project: pr2_gui.pro
# Template: app
-# Command: /usr/bin/qmake -unix -o Makefile pr2_gui.pro
+# Command: /usr/bin/qmake-qt4 -unix -o Makefile pr2_gui.pro
#############################################################################
####### Compiler, tools and options
CC = gcc
CXX = g++
-DEFINES = -DQT_SHARED -DQT_NO_DEBUG -DQT_GUI_LIB -DQT_CORE_LIB
-CFLAGS = -pipe $(shell rospack export/cpp/cflags pr2_gui) -Wall -W -D_REENTRANT $(DEFINES)
-CXXFLAGS = -pipe -fpermissive $(shell rospack export/cpp/cflags pr2_gui) -Wall -W -D_REENTRANT $(DEFINES)
+LEX = flex
+YACC = yacc
+DEFINES = -DQT_NO_DEBUG -DQT_GUI_LIB -DQT_CORE_LIB
+CFLAGS = -pipe $(shell rospack export/cpp/cflags pr2_gui) -Wall -W -D_REENTRANT $(DEFINES)
+CXXFLAGS = -pipe $(shell rospack export/cpp/cflags pr2_gui) -Wall -W -D_REENTRANT $(DEFINES)
+LEXFLAGS =
+YACCFLAGS = -d
INCPATH = -I/usr/share/qt4/mkspecs/linux-g++ -I. -I/usr/include/qt4/QtCore -I/usr/include/qt4/QtCore -I/usr/include/qt4/QtGui -I/usr/include/qt4/QtGui -I/usr/include/qt4 -Ibuild -Ibuild
LINK = g++
-LFLAGS = -Wl,--no-undefined
+LFLAGS =
LIBS = $(SUBLIBS) -L/usr/lib $(shell rospack export/cpp/lflags pr2_gui) -lQtGui -lQtCore -lpthread
AR = ar cqs
RANLIB =
-QMAKE = /usr/bin/qmake
+QMAKE = /usr/bin/qmake-qt4
TAR = tar -cf
COMPRESS = gzip -9f
COPY = cp -f
-SED = sed
COPY_FILE = $(COPY)
COPY_DIR = $(COPY) -r
INSTALL_FILE = install -m 644 -p
@@ -47,8 +50,8 @@
OBJECTS = build/launcherimpl.o \
build/main.o \
build/moc_launcherimpl.o
-DIST = /usr/share/qt4/mkspecs/common/g++.conf \
- /usr/share/qt4/mkspecs/common/unix.conf \
+DIST = /usr/share/qt4/mkspecs/common/unix.conf \
+ /usr/share/qt4/mkspecs/common/g++.conf \
/usr/share/qt4/mkspecs/common/linux.conf \
/usr/share/qt4/mkspecs/qconfig.pri \
/usr/share/qt4/mkspecs/features/qt_functions.prf \
@@ -63,8 +66,6 @@
/usr/share/qt4/mkspecs/features/moc.prf \
/usr/share/qt4/mkspecs/features/resources.prf \
/usr/share/qt4/mkspecs/features/uic.prf \
- /usr/share/qt4/mkspecs/features/yacc.prf \
- /usr/share/qt4/mkspecs/features/lex.prf \
pr2_gui.pro
QMAKE_TARGET = pr2_gui
DESTDIR = bin/
@@ -98,8 +99,8 @@
@$(CHK_DIR_EXISTS) bin/ || $(MKDIR) bin/
$(LINK) $(LFLAGS) -o $(TARGET) $(OBJECTS) $(OBJCOMP) $(LIBS)
-Makefile: pr2_gui.pro /usr/share/qt4/mkspecs/linux-g++/qmake.conf /usr/share/qt4/mkspecs/common/g++.conf \
- /usr/share/qt4/mkspecs/common/unix.conf \
+Makefile: pr2_gui.pro /usr/share/qt4/mkspecs/linux-g++/qmake.conf /usr/share/qt4/mkspecs/common/unix.conf \
+ /usr/share/qt4/mkspecs/common/g++.conf \
/usr/share/qt4/mkspecs/common/linux.conf \
/usr/share/qt4/mkspecs/qconfig.pri \
/usr/share/qt4/mkspecs/features/qt_functions.prf \
@@ -113,12 +114,10 @@
/usr/share/qt4/mkspecs/features/unix/thread.prf \
/usr/share/qt4/mkspecs/features/moc.prf \
/usr/share/qt4/mkspecs/features/resources.prf \
- /usr/share/qt4/mkspecs/features/uic.prf \
- /usr/share/qt4/mkspecs/features/yacc.prf \
- /usr/share/qt4/mkspecs/features/lex.prf
+ /usr/share/qt4/mkspecs/features/uic.prf
$(QMAKE) -unix -o Makefile pr2_gui.pro
-/usr/share/qt4/mkspecs/common/g++.conf:
/usr/share/qt4/mkspecs/common/unix.conf:
+/usr/share/qt4/mkspecs/common/g++.conf:
/usr/share/qt4/mkspecs/common/linux.conf:
/usr/share/qt4/mkspecs/qconfig.pri:
/usr/share/qt4/mkspecs/features/qt_functions.prf:
@@ -133,8 +132,6 @@
/usr/share/qt4/mkspecs/features/moc.prf:
/usr/share/qt4/mkspecs/features/resources.prf:
/usr/share/qt4/mkspecs/features/uic.prf:
-/usr/share/qt4/mkspecs/features/yacc.prf:
-/usr/share/qt4/mkspecs/features/lex.prf:
qmake: FORCE
@$(QMAKE) -unix -o Makefile pr2_gui.pro
@@ -143,6 +140,8 @@
$(COPY_FILE) --parents $(SOURCES) $(DIST) build/pr2_gui1.0.0/ && $(COPY_FILE) --parents src/launcherimpl.h build/pr2_gui1.0.0/ && $(COPY_FILE) --parents src/launcherimpl.cpp src/main.cpp build/pr2_gui1.0.0/ && $(COPY_FILE) --parents ui/launcher.ui build/pr2_gui1.0.0/ && (cd `dirname build/pr2_gui1.0.0` && $(TAR) pr2_gui1.0.0.tar pr2_gui1.0.0 && $(COMPRESS) pr2_gui1.0.0.tar) && $(MOVE) `dirname build/pr2_gui1.0.0`/pr2_gui1.0.0.tar.gz . && $(DEL_FILE) -r build/pr2_gui1.0.0
+yaccclean:
+lexclean:
clean:compiler_clean
-$(DEL_FILE) $(OBJECTS)
-$(DEL_FILE) *~ core *.core
@@ -155,6 +154,9 @@
-$(DEL_FILE) Makefile
+/usr/bin/moc-qt4:
+ (cd $(QTDIR)/src/tools/moc && $(MAKE))
+
mocclean: compiler_moc_header_clean compiler_moc_source_clean
mocables: compiler_moc_header_make_all compiler_moc_source_make_all
@@ -162,9 +164,10 @@
compiler_moc_header_make_all: build/moc_launcherimpl.cpp
compiler_moc_header_clean:
-$(DEL_FILE) build/moc_launcherimpl.cpp
-build/moc_launcherimpl.cpp: build/ui_launcher.h \
+build/moc_launcherimpl.cpp: ui_launcher.h \
src/Vis3d.hh \
- src/launcherimpl.h
+ src/launcherimpl.h \
+ /usr/bin/moc-qt4
/usr/bin/moc-qt4 $(DEFINES) $(INCPATH) src/launcherimpl.h -o build/moc_launcherimpl.cpp
compiler_rcc_make_all:
@@ -180,23 +183,17 @@
build/ui_launcher.h: ui/launcher.ui
/usr/bin/uic-qt4 ui/launcher.ui -o build/ui_launcher.h
-compiler_yacc_decl_make_all:
-compiler_yacc_decl_clean:
-compiler_yacc_impl_make_all:
-compiler_yacc_impl_clean:
-compiler_lex_make_all:
-compiler_lex_clean:
-compiler_clean: compiler_moc_header_clean compiler_uic_clean
+compiler_clean: compiler_moc_header_clean compiler_rcc_clean compiler_image_collection_clean compiler_moc_source_clean compiler_uic_clean
####### Compile
build/launcherimpl.o: src/launcherimpl.cpp src/launcherimpl.h \
- build/ui_launcher.h \
+ ui_launcher.h \
src/Vis3d.hh
$(CXX) -c $(CXXFLAGS) $(INCPATH) -o build/launcherimpl.o src/launcherimpl.cpp
build/main.o: src/main.cpp src/launcherimpl.h \
- build/ui_launcher.h \
+ ui_launcher.h \
src/Vis3d.hh
$(CXX) -c $(CXXFLAGS) $(INCPATH) -o build/main.o src/main.cpp
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <adv...@us...> - 2008-06-12 23:05:31
|
Revision: 788
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=788&view=rev
Author: advaitjain
Date: 2008-06-12 16:05:38 -0700 (Thu, 12 Jun 2008)
Log Message:
-----------
moved teleop_arm_keyboard from ros-pkg/nav to ros-pkg/manip.
Added Paths:
-----------
pkg/trunk/manip/
pkg/trunk/manip/teleop_arm_keyboard/
pkg/trunk/manip/teleop_arm_keyboard/Makefile
pkg/trunk/manip/teleop_arm_keyboard/manifest.xml
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
Removed Paths:
-------------
pkg/trunk/manip/teleop_arm_keyboard/Makefile
pkg/trunk/manip/teleop_arm_keyboard/manifest.xml
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
pkg/trunk/nav/teleop_arm_keyboard/
Copied: pkg/trunk/manip/teleop_arm_keyboard (from rev 739, pkg/trunk/nav/teleop_arm_keyboard)
Deleted: pkg/trunk/manip/teleop_arm_keyboard/Makefile
===================================================================
--- pkg/trunk/nav/teleop_arm_keyboard/Makefile 2008-06-11 16:38:23 UTC (rev 739)
+++ pkg/trunk/manip/teleop_arm_keyboard/Makefile 2008-06-12 23:05:38 UTC (rev 788)
@@ -1,16 +0,0 @@
-SRC = teleop_arm_keyboard.cc
-OUT = teleop_arm_keyboard
-PKG = teleop_arm_keyboard
-CXX = g++
-all: $(PKG)
-
-
-CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
-
-LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
-
-teleop_arm_keyboard: teleop_arm_keyboard.cc
- $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
-
-clean:
- rm -rf *.o $(PKG) $(PKG).dSYM
Copied: pkg/trunk/manip/teleop_arm_keyboard/Makefile (from rev 787, pkg/trunk/nav/teleop_arm_keyboard/Makefile)
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/Makefile (rev 0)
+++ pkg/trunk/manip/teleop_arm_keyboard/Makefile 2008-06-12 23:05:38 UTC (rev 788)
@@ -0,0 +1,16 @@
+SRC = teleop_arm_keyboard.cc
+OUT = teleop_arm_keyboard
+PKG = teleop_arm_keyboard
+CXX = g++
+all: $(PKG)
+
+
+CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
+
+LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
+
+teleop_arm_keyboard: teleop_arm_keyboard.cc
+ $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
+
+clean:
+ rm -rf *.o $(PKG) $(PKG).dSYM
Deleted: pkg/trunk/manip/teleop_arm_keyboard/manifest.xml
===================================================================
--- pkg/trunk/nav/teleop_arm_keyboard/manifest.xml 2008-06-11 16:38:23 UTC (rev 739)
+++ pkg/trunk/manip/teleop_arm_keyboard/manifest.xml 2008-06-12 23:05:38 UTC (rev 788)
@@ -1,10 +0,0 @@
-<package>
-<description>A ROS node that supports position-mode teleoperation of
-individual joints of the PR2Arm from keyboard input</description>
- <author>Advait Jain</author>
- <license>BSD</license>
- <depend package="roscpp"/>
- <depend package="gazebo"/>
- <depend package="libpr2API"/>
- <depend package="std_msgs"/>
-</package>
Copied: pkg/trunk/manip/teleop_arm_keyboard/manifest.xml (from rev 787, pkg/trunk/nav/teleop_arm_keyboard/manifest.xml)
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/manifest.xml (rev 0)
+++ pkg/trunk/manip/teleop_arm_keyboard/manifest.xml 2008-06-12 23:05:38 UTC (rev 788)
@@ -0,0 +1,10 @@
+<package>
+<description>A ROS node that supports position-mode teleoperation of
+individual joints of the PR2Arm from keyboard input</description>
+ <author>Advait Jain</author>
+ <license>BSD</license>
+ <depend package="roscpp"/>
+ <depend package="gazebo"/>
+ <depend package="libpr2API"/>
+ <depend package="std_msgs"/>
+</package>
Deleted: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
===================================================================
--- pkg/trunk/nav/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-06-11 16:38:23 UTC (rev 739)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-06-12 23:05:38 UTC (rev 788)
@@ -1,363 +0,0 @@
-/*
- * teleop_base_keyboard
- * 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 <ORGANIZATION> nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/**
-
-@mainpage
-
-@htmlinclude manifest.html
-
-@b teleop_arm_keyboard teleoperates the arms of the PR2 by mapping
-key presses into joint angle set points.
-
-<hr>
-
-@section usage Usage
-@verbatim
-$ teleop_arm_keyboard [standard ROS args]
-@endverbatim
-
-Key mappings are printed to screen on startup.
-
-<hr>
-
-@section topic ROS topics
-
-Subscribes to (name/type):
-- None
-
-Publishes to (name / type):
-- @b "cmd_leftarmconfig"/PR2Arm : configuration of the left arm (all the joint angles); sent on every keypress.
-
-<hr>
-
-@section parameters ROS parameters
-
-- None
-
- **/
-
-#include <termios.h>
-#include <signal.h>
-#include <math.h>
-
-#include <ros/node.h>
-#include <std_msgs/MsgPR2Arm.h>
-
-#include <libpr2API/pr2API.h>
-
-#define COMMAND_TIMEOUT_SEC 0.2
-
-using namespace PR2;
-
-
-class TArmK_Node : public ros::node
-{
- private:
- MsgPR2Arm cmd_leftarmconfig;
- MsgPR2Arm cmd_rightarmconfig;
-
- public:
- TArmK_Node() : ros::node("tarmk")
- {
- // cmd_armconfig should probably be initialised
- // with the current joint angles of the arm rather
- // than zeros.
- this->cmd_leftarmconfig.turretAngle = 0;
- this->cmd_leftarmconfig.shoulderLiftAngle = 0;
- this->cmd_leftarmconfig.upperarmRollAngle = 0;
- this->cmd_leftarmconfig.elbowAngle = 0;
- this->cmd_leftarmconfig.forearmRollAngle = 0;
- this->cmd_leftarmconfig.wristPitchAngle = 0;
- this->cmd_leftarmconfig.wristRollAngle = 0;
- this->cmd_leftarmconfig.gripperForceCmd = 1000;
- this->cmd_leftarmconfig.gripperGapCmd = 0;
- this->cmd_rightarmconfig.turretAngle = 0;
- this->cmd_rightarmconfig.shoulderLiftAngle = 0;
- this->cmd_rightarmconfig.upperarmRollAngle = 0;
- this->cmd_rightarmconfig.elbowAngle = 0;
- this->cmd_rightarmconfig.forearmRollAngle = 0;
- this->cmd_rightarmconfig.wristPitchAngle = 0;
- this->cmd_rightarmconfig.wristRollAngle = 0;
- this->cmd_rightarmconfig.gripperForceCmd = 1000;
- this->cmd_rightarmconfig.gripperGapCmd = 0;
- advertise<MsgPR2Arm>("cmd_leftarmconfig");
- advertise<MsgPR2Arm>("cmd_rightarmconfig");
- }
- ~TArmK_Node() { }
- void keyboardLoop();
- void changeJointAngle(PR2_JOINT_ID jointID, bool increment);
-};
-
-TArmK_Node* tarmk;
-int kfd = 0;
-struct termios cooked, raw;
-
-void
-quit(int sig)
-{
-// tbk->stopRobot();
- ros::fini();
- tcsetattr(kfd, TCSANOW, &cooked);
- exit(0);
-}
-
-int
-main(int argc, char** argv)
-{
- ros::init(argc,argv);
-
- tarmk = new TArmK_Node();
-
- signal(SIGINT,quit);
-
- tarmk->keyboardLoop();
-
- return(0);
-}
-
-
-void TArmK_Node::changeJointAngle(PR2_JOINT_ID jointID, bool increment)
-{
- float jointCmdStep = 5*M_PI/180;
- float gripperStep = 0.002;
- if (increment == false)
- {
- jointCmdStep *= -1;
- gripperStep *= -1;
- }
-
- switch(jointID)
- {
- case ARM_L_PAN:
- this->cmd_leftarmconfig.turretAngle += jointCmdStep;
- break;
- case ARM_L_SHOULDER_PITCH:
- this->cmd_leftarmconfig.shoulderLiftAngle += jointCmdStep;
- break;
- case ARM_L_SHOULDER_ROLL:
- this->cmd_leftarmconfig.upperarmRollAngle += jointCmdStep;
- break;
- case ARM_L_ELBOW_PITCH:
- this->cmd_leftarmconfig.elbowAngle += jointCmdStep;
- break;
- case ARM_L_ELBOW_ROLL:
- this->cmd_leftarmconfig.forearmRollAngle += jointCmdStep;
- break;
- case ARM_L_WRIST_PITCH:
- this->cmd_leftarmconfig.wristPitchAngle += jointCmdStep;
- break;
- case ARM_L_WRIST_ROLL:
- this->cmd_leftarmconfig.wristRollAngle += jointCmdStep;
- break;
- case ARM_L_GRIPPER:
- this->cmd_leftarmconfig.gripperGapCmd += gripperStep;
- break;
- case ARM_R_PAN:
- this->cmd_rightarmconfig.turretAngle += jointCmdStep;
- break;
- case ARM_R_SHOULDER_PITCH:
- this->cmd_rightarmconfig.shoulderLiftAngle += jointCmdStep;
- break;
- case ARM_R_SHOULDER_ROLL:
- this->cmd_rightarmconfig.upperarmRollAngle += jointCmdStep;
- break;
- case ARM_R_ELBOW_PITCH:
- this->cmd_rightarmconfig.elbowAngle += jointCmdStep;
- break;
- case ARM_R_ELBOW_ROLL:
- this->cmd_rightarmconfig.forearmRollAngle += jointCmdStep;
- break;
- case ARM_R_WRIST_PITCH:
- this->cmd_rightarmconfig.wristPitchAngle += jointCmdStep;
- break;
- case ARM_R_WRIST_ROLL:
- this->cmd_rightarmconfig.wristRollAngle += jointCmdStep;
- break;
- case ARM_R_GRIPPER:
- this->cmd_rightarmconfig.gripperGapCmd += gripperStep;
- break;
- default:
- printf("This joint is not handled.\n");
- break;
- }
-}
-
-
-void
-TArmK_Node::keyboardLoop()
-{
- char c;
- bool dirty=false;
- PR2_JOINT_ID curr_jointID = ARM_L_PAN; // joint which will be actuated.
- bool right_arm = false;
-
- // get the console in raw mode
- tcgetattr(kfd, &cooked);
- memcpy(&raw, &cooked, sizeof(struct termios));
- raw.c_lflag &=~ (ICANON | ECHO);
- raw.c_cc[VEOL] = 1;
- raw.c_cc[VEOF] = 2;
- tcsetattr(kfd, TCSANOW, &raw);
-
- puts("Reading from keyboard");
- puts("---------------------------");
- printf("Press l/r to operate left/right arm.\n");
- printf("Numbers 1 through 8 to select the joint to operate.\n");
- printf("+ and - will move the joint in different directions by 5 degrees.\n");
- puts("");
- puts("---------------------------");
-
- for(;;)
- {
- // get the next event from the keyboard
- if(read(kfd, &c, 1) < 0)
- {
- perror("read():");
- exit(-1);
- }
-
- switch(c)
- {
- case 'l':
- case 'L':
- right_arm = false;
- printf("Actuating left arm.\n");
- break;
- case 'r':
- case 'R':
- right_arm = true;
- printf("Actuating right arm.\n");
- break;
- case '+':
- case '=':
- changeJointAngle(curr_jointID, true);
- dirty=true;
- break;
- case '_':
- case '-':
- changeJointAngle(curr_jointID, false);
- dirty=true;
- break;
- default:
- break;
- }
-
- if (right_arm==false)
- {
- switch(c)
- {
- case '1':
- curr_jointID = ARM_L_PAN;
- printf("left turret\n");
- break;
- case '2':
- curr_jointID = ARM_L_SHOULDER_PITCH;
- printf("left shoulder pitch\n");
- break;
- case '3':
- curr_jointID = ARM_L_SHOULDER_ROLL;
- printf("left shoulder roll\n");
- break;
- case '4':
- curr_jointID = ARM_L_ELBOW_PITCH;
- printf("left elbow pitch\n");
- break;
- case '5':
- curr_jointID = ARM_L_ELBOW_ROLL;
- printf("left elbow roll\n");
- break;
- case '6':
- curr_jointID = ARM_L_WRIST_PITCH;
- printf("left wrist pitch\n");
- break;
- case '7':
- curr_jointID = ARM_L_WRIST_ROLL;
- printf("left wrist roll\n");
- break;
- case '8':
- curr_jointID = ARM_L_GRIPPER;
- printf("left gripper\n");
- break;
- default:
- break;
- }
- }
- else
- {
- switch(c)
- {
- case '1':
- curr_jointID = ARM_R_PAN;
- printf("right turret\n");
- break;
- case '2':
- curr_jointID = ARM_R_SHOULDER_PITCH;
- printf("right shoulder pitch\n");
- break;
- case '3':
- curr_jointID = ARM_R_SHOULDER_ROLL;
- printf("right shoulder roll\n");
- break;
- case '4':
- curr_jointID = ARM_R_ELBOW_PITCH;
- printf("right elbow pitch\n");
- break;
- case '5':
- curr_jointID = ARM_R_ELBOW_ROLL;
- printf("right elbow roll\n");
- break;
- case '6':
- curr_jointID = ARM_R_WRIST_PITCH;
- printf("right wrist pitch\n");
- break;
- case '7':
- curr_jointID = ARM_R_WRIST_ROLL;
- printf("right wrist roll\n");
- break;
- case '8':
- curr_jointID = ARM_R_GRIPPER;
- printf("right gripper\n");
- break;
- default:
- break;
- }
- }
-
- if (dirty == true)
- {
- dirty=false; // Sending the command only once for each key press.
- publish("cmd_leftarmconfig",cmd_leftarmconfig);
- publish("cmd_rightarmconfig",cmd_rightarmconfig);
- }
- }
-}
-
-
Copied: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc (from rev 787, pkg/trunk/nav/teleop_arm_keyboard/teleop_arm_keyboard.cc)
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc (rev 0)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-06-12 23:05:38 UTC (rev 788)
@@ -0,0 +1,363 @@
+/*
+ * teleop_base_keyboard
+ * 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 <ORGANIZATION> nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b teleop_arm_keyboard teleoperates the arms of the PR2 by mapping
+key presses into joint angle set points.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ teleop_arm_keyboard [standard ROS args]
+@endverbatim
+
+Key mappings are printed to screen on startup.
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- None
+
+Publishes to (name / type):
+- @b "cmd_leftarmconfig"/PR2Arm : configuration of the left arm (all the joint angles); sent on every keypress.
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+ **/
+
+#include <termios.h>
+#include <signal.h>
+#include <math.h>
+
+#include <ros/node.h>
+#include <std_msgs/MsgPR2Arm.h>
+
+#include <libpr2API/pr2API.h>
+
+#define COMMAND_TIMEOUT_SEC 0.2
+
+using namespace PR2;
+
+
+class TArmK_Node : public ros::node
+{
+ private:
+ MsgPR2Arm cmd_leftarmconfig;
+ MsgPR2Arm cmd_rightarmconfig;
+
+ public:
+ TArmK_Node() : ros::node("tarmk")
+ {
+ // cmd_armconfig should probably be initialised
+ // with the current joint angles of the arm rather
+ // than zeros.
+ this->cmd_leftarmconfig.turretAngle = 0;
+ this->cmd_leftarmconfig.shoulderLiftAngle = 0;
+ this->cmd_leftarmconfig.upperarmRollAngle = 0;
+ this->cmd_leftarmconfig.elbowAngle = 0;
+ this->cmd_leftarmconfig.forearmRollAngle = 0;
+ this->cmd_leftarmconfig.wristPitchAngle = 0;
+ this->cmd_leftarmconfig.wristRollAngle = 0;
+ this->cmd_leftarmconfig.gripperForceCmd = 1000;
+ this->cmd_leftarmconfig.gripperGapCmd = 0;
+ this->cmd_rightarmconfig.turretAngle = 0;
+ this->cmd_rightarmconfig.shoulderLiftAngle = 0;
+ this->cmd_rightarmconfig.upperarmRollAngle = 0;
+ this->cmd_rightarmconfig.elbowAngle = 0;
+ this->cmd_rightarmconfig.forearmRollAngle = 0;
+ this->cmd_rightarmconfig.wristPitchAngle = 0;
+ this->cmd_rightarmconfig.wristRollAngle = 0;
+ this->cmd_rightarmconfig.gripperForceCmd = 1000;
+ this->cmd_rightarmconfig.gripperGapCmd = 0;
+ advertise<MsgPR2Arm>("cmd_leftarmconfig");
+ advertise<MsgPR2Arm>("cmd_rightarmconfig");
+ }
+ ~TArmK_Node() { }
+ void keyboardLoop();
+ void changeJointAngle(PR2_JOINT_ID jointID, bool increment);
+};
+
+TArmK_Node* tarmk;
+int kfd = 0;
+struct termios cooked, raw;
+
+void
+quit(int sig)
+{
+// tbk->stopRobot();
+ ros::fini();
+ tcsetattr(kfd, TCSANOW, &cooked);
+ exit(0);
+}
+
+int
+main(int argc, char** argv)
+{
+ ros::init(argc,argv);
+
+ tarmk = new TArmK_Node();
+
+ signal(SIGINT,quit);
+
+ tarmk->keyboardLoop();
+
+ return(0);
+}
+
+
+void TArmK_Node::changeJointAngle(PR2_JOINT_ID jointID, bool increment)
+{
+ float jointCmdStep = 5*M_PI/180;
+ float gripperStep = 0.002;
+ if (increment == false)
+ {
+ jointCmdStep *= -1;
+ gripperStep *= -1;
+ }
+
+ switch(jointID)
+ {
+ case ARM_L_PAN:
+ this->cmd_leftarmconfig.turretAngle += jointCmdStep;
+ break;
+ case ARM_L_SHOULDER_PITCH:
+ this->cmd_leftarmconfig.shoulderLiftAngle += jointCmdStep;
+ break;
+ case ARM_L_SHOULDER_ROLL:
+ this->cmd_leftarmconfig.upperarmRollAngle += jointCmdStep;
+ break;
+ case ARM_L_ELBOW_PITCH:
+ this->cmd_leftarmconfig.elbowAngle += jointCmdStep;
+ break;
+ case ARM_L_ELBOW_ROLL:
+ this->cmd_leftarmconfig.forearmRollAngle += jointCmdStep;
+ break;
+ case ARM_L_WRIST_PITCH:
+ this->cmd_leftarmconfig.wristPitchAngle += jointCmdStep;
+ break;
+ case ARM_L_WRIST_ROLL:
+ this->cmd_leftarmconfig.wristRollAngle += jointCmdStep;
+ break;
+ case ARM_L_GRIPPER:
+ this->cmd_leftarmconfig.gripperGapCmd += gripperStep;
+ break;
+ case ARM_R_PAN:
+ this->cmd_rightarmconfig.turretAngle += jointCmdStep;
+ break;
+ case ARM_R_SHOULDER_PITCH:
+ this->cmd_rightarmconfig.shoulderLiftAngle += jointCmdStep;
+ break;
+ case ARM_R_SHOULDER_ROLL:
+ this->cmd_rightarmconfig.upperarmRollAngle += jointCmdStep;
+ break;
+ case ARM_R_ELBOW_PITCH:
+ this->cmd_rightarmconfig.elbowAngle += jointCmdStep;
+ break;
+ case ARM_R_ELBOW_ROLL:
+ this->cmd_rightarmconfig.forearmRollAngle += jointCmdStep;
+ break;
+ case ARM_R_WRIST_PITCH:
+ this->cmd_rightarmconfig.wristPitchAngle += jointCmdStep;
+ break;
+ case ARM_R_WRIST_ROLL:
+ this->cmd_rightarmconfig.wristRollAngle += jointCmdStep;
+ break;
+ case ARM_R_GRIPPER:
+ this->cmd_rightarmconfig.gripperGapCmd += gripperStep;
+ break;
+ default:
+ printf("This joint is not handled.\n");
+ break;
+ }
+}
+
+
+void
+TArmK_Node::keyboardLoop()
+{
+ char c;
+ bool dirty=false;
+ PR2_JOINT_ID curr_jointID = ARM_L_PAN; // joint which will be actuated.
+ bool right_arm = false;
+
+ // get the console in raw mode
+ tcgetattr(kfd, &cooked);
+ memcpy(&raw, &cooked, sizeof(struct termios));
+ raw.c_lflag &=~ (ICANON | ECHO);
+ raw.c_cc[VEOL] = 1;
+ raw.c_cc[VEOF] = 2;
+ tcsetattr(kfd, TCSANOW, &raw);
+
+ puts("Reading from keyboard");
+ puts("---------------------------");
+ printf("Press l/r to operate left/right arm.\n");
+ printf("Numbers 1 through 8 to select the joint to operate.\n");
+ printf("+ and - will move the joint in different directions by 5 degrees.\n");
+ puts("");
+ puts("---------------------------");
+
+ for(;;)
+ {
+ // get the next event from the keyboard
+ if(read(kfd, &c, 1) < 0)
+ {
+ perror("read():");
+ exit(-1);
+ }
+
+ switch(c)
+ {
+ case 'l':
+ case 'L':
+ right_arm = false;
+ printf("Actuating left arm.\n");
+ break;
+ case 'r':
+ case 'R':
+ right_arm = true;
+ printf("Actuating right arm.\n");
+ break;
+ case '+':
+ case '=':
+ changeJointAngle(curr_jointID, true);
+ dirty=true;
+ break;
+ case '_':
+ case '-':
+ changeJointAngle(curr_jointID, false);
+ dirty=true;
+ break;
+ default:
+ break;
+ }
+
+ if (right_arm==false)
+ {
+ switch(c)
+ {
+ case '1':
+ curr_jointID = ARM_L_PAN;
+ printf("left turret\n");
+ break;
+ case '2':
+ curr_jointID = ARM_L_SHOULDER_PITCH;
+ printf("left shoulder pitch\n");
+ break;
+ case '3':
+ curr_jointID = ARM_L_SHOULDER_ROLL;
+ printf("left shoulder roll\n");
+ break;
+ case '4':
+ curr_jointID = ARM_L_ELBOW_PITCH;
+ printf("left elbow pitch\n");
+ break;
+ case '5':
+ curr_jointID = ARM_L_ELBOW_ROLL;
+ printf("left elbow roll\n");
+ break;
+ case '6':
+ curr_jointID = ARM_L_WRIST_PITCH;
+ printf("left wrist pitch\n");
+ break;
+ case '7':
+ curr_jointID = ARM_L_WRIST_ROLL;
+ printf("left wrist roll\n");
+ break;
+ case '8':
+ curr_jointID = ARM_L_GRIPPER;
+ printf("left gripper\n");
+ break;
+ default:
+ break;
+ }
+ }
+ else
+ {
+ switch(c)
+ {
+ case '1':
+ curr_jointID = ARM_R_PAN;
+ printf("right turret\n");
+ break;
+ case '2':
+ curr_jointID = ARM_R_SHOULDER_PITCH;
+ printf("right shoulder pitch\n");
+ break;
+ case '3':
+ curr_jointID = ARM_R_SHOULDER_ROLL;
+ printf("right shoulder roll\n");
+ break;
+ case '4':
+ curr_jointID = ARM_R_ELBOW_PITCH;
+ printf("right elbow pitch\n");
+ break;
+ case '5':
+ curr_jointID = ARM_R_ELBOW_ROLL;
+ printf("right elbow roll\n");
+ break;
+ case '6':
+ curr_jointID = ARM_R_WRIST_PITCH;
+ printf("right wrist pitch\n");
+ break;
+ case '7':
+ curr_jointID = ARM_R_WRIST_ROLL;
+ printf("right wrist roll\n");
+ break;
+ case '8':
+ curr_jointID = ARM_R_GRIPPER;
+ printf("right gripper\n");
+ break;
+ default:
+ break;
+ }
+ }
+
+ if (dirty == true)
+ {
+ dirty=false; // Sending the command only once for each key press.
+ publish("cmd_leftarmconfig",cmd_leftarmconfig);
+ publish("cmd_rightarmconfig",cmd_rightarmconfig);
+ }
+ }
+}
+
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-13 02:12:52
|
Revision: 800
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=800&view=rev
Author: hsujohnhsu
Date: 2008-06-12 19:13:00 -0700 (Thu, 12 Jun 2008)
Log Message:
-----------
Update GetBasePosition in rosgazebo and libpr2API.
Update roslaunch codes in 2dnav-gazebo.
Move world into 2dnav-gazebo.
Move Pid.h into controllers directory.
Modified Paths:
--------------
pkg/trunk/controllers/src/Pid.cpp
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Added Paths:
-----------
pkg/trunk/controllers/include/controllers/
pkg/trunk/controllers/include/controllers/Pid.h
Removed Paths:
-------------
pkg/trunk/controllers/include/Pid.h
pkg/trunk/simulators/rosgazebo/world/map3.png
pkg/trunk/simulators/rosgazebo/world/pr2.model
pkg/trunk/simulators/rosgazebo/world/robot.world
Deleted: pkg/trunk/controllers/include/Pid.h
===================================================================
--- pkg/trunk/controllers/include/Pid.h 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/controllers/include/Pid.h 2008-06-13 02:13:00 UTC (rev 800)
@@ -1,92 +0,0 @@
-#pragma once
-
-
-/***************************************************/
-/*! \brief A basic pid class.
-
- This class implements a generic structure that
- can be used to create a wide range of pid
- controllers. It can function independently or
- be subclassed to provide more specific controls
- based on a particular control loop.
-
- In particular, this class implements the standard
- pid equation:
-
- command = Pterm + Iterm + Dterm
-
- where: <br>
- <UL TYPE="none">
- <LI> Pterm = pGain * pError
- <LI> Iterm = iGain * iError
- <LI> Dterm = dGain * dError
- <LI> iError = iError + pError * dT
- <LI> dError = dError + pError / dT
- </UL>
-
- given:<br>
- <UL TYPE="none">
- <LI> pError = pTarget - pState.
- </UL>
-
- If the fixedTime input of UpdatePid is set to alpha,
- dT = alpha. Otherwise the time step is computed when
- the function is called.
-
-*/
-/***************************************************/
-class Pid
-{
- public:
-
- /*!
- * \brief Constructor, zeros out Pid values when created and
- * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- Pid(double P = 0.8,double I = 0.5, double D = 0.0, double I1 = 1.0, double I2 =-1.0);
-
- /*!
- * \brief Destructor of Pid class.
- */
- ~Pid( );
-
- /*!
- * \brief Update the Pid loop with nonuniform time step size.
- *
- * \param pState This is the current measured state or position of the object
- * being controlled.
- * \param pTarget This is the set point the controller is trying to reach.
- * \param fixedTime Set to a value for fixed time step of that value
- */
- double UpdatePid( double pError, double dt );
-
- /*!
- * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- void InitPid( double P,double I, double D, double I1, double I2 );
-
-
- private:
- double pError; /**< Derivative state. */
- double dError; /**< Derivative state. */
- double iError; /**< Integrator state. */
- double pGain; /**< Proportional gain. */
- double iGain; /**< Integral gain. */
- double dGain; /**< Derivative gain. */
- double iMax; /**< Maximum allowable integrator state. */
- double iMin; /**< Minimum allowable integrator state. */
- double currentCommand; /**< Current position command. */
-};
-
Copied: pkg/trunk/controllers/include/controllers/Pid.h (from rev 793, pkg/trunk/controllers/include/Pid.h)
===================================================================
--- pkg/trunk/controllers/include/controllers/Pid.h (rev 0)
+++ pkg/trunk/controllers/include/controllers/Pid.h 2008-06-13 02:13:00 UTC (rev 800)
@@ -0,0 +1,92 @@
+#pragma once
+
+
+/***************************************************/
+/*! \brief A basic pid class.
+
+ This class implements a generic structure that
+ can be used to create a wide range of pid
+ controllers. It can function independently or
+ be subclassed to provide more specific controls
+ based on a particular control loop.
+
+ In particular, this class implements the standard
+ pid equation:
+
+ command = Pterm + Iterm + Dterm
+
+ where: <br>
+ <UL TYPE="none">
+ <LI> Pterm = pGain * pError
+ <LI> Iterm = iGain * iError
+ <LI> Dterm = dGain * dError
+ <LI> iError = iError + pError * dT
+ <LI> dError = dError + pError / dT
+ </UL>
+
+ given:<br>
+ <UL TYPE="none">
+ <LI> pError = pTarget - pState.
+ </UL>
+
+ If the fixedTime input of UpdatePid is set to alpha,
+ dT = alpha. Otherwise the time step is computed when
+ the function is called.
+
+*/
+/***************************************************/
+class Pid
+{
+ public:
+
+ /*!
+ * \brief Constructor, zeros out Pid values when created and
+ * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
+ *
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param I1 The integral upper limit.
+ * \param I2 The integral lower limit.
+ */
+ Pid(double P = 0.8,double I = 0.5, double D = 0.0, double I1 = 1.0, double I2 =-1.0);
+
+ /*!
+ * \brief Destructor of Pid class.
+ */
+ ~Pid( );
+
+ /*!
+ * \brief Update the Pid loop with nonuniform time step size.
+ *
+ * \param pState This is the current measured state or position of the object
+ * being controlled.
+ * \param pTarget This is the set point the controller is trying to reach.
+ * \param fixedTime Set to a value for fixed time step of that value
+ */
+ double UpdatePid( double pError, double dt );
+
+ /*!
+ * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
+ *
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param I1 The integral upper limit.
+ * \param I2 The integral lower limit.
+ */
+ void InitPid( double P,double I, double D, double I1, double I2 );
+
+
+ private:
+ double pError; /**< Derivative state. */
+ double dError; /**< Derivative state. */
+ double iError; /**< Integrator state. */
+ double pGain; /**< Proportional gain. */
+ double iGain; /**< Integral gain. */
+ double dGain; /**< Derivative gain. */
+ double iMax; /**< Maximum allowable integrator state. */
+ double iMin; /**< Minimum allowable integrator state. */
+ double currentCommand; /**< Current position command. */
+};
+
Modified: pkg/trunk/controllers/src/Pid.cpp
===================================================================
--- pkg/trunk/controllers/src/Pid.cpp 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/controllers/src/Pid.cpp 2008-06-13 02:13:00 UTC (rev 800)
@@ -1,4 +1,4 @@
-#include "Pid.h"
+#include <controllers/Pid.h>
#include<iostream>
using namespace std;
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-06-13 02:13:00 UTC (rev 800)
@@ -1,10 +1,9 @@
<launch>
<group name="wg">
- <!--node pkg="rosgazebo" type="gazebo" args="world/robot.world" respawn="false" /-->
- <node pkg="rosgazebo" type="run-gazebo.sh" args="$(find rosgazebo)/world/robot.world" respawn="false" />
+ <node pkg="rosgazebo" type="run-gazebo.sh" args="$(find 2dnav-gazebo)/world/robot.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav-gazebo)/map3.png 0.1" respawn="false" />
- <node pkg="rosgazebo" type="run-rosgazebo.sh" args="" respawn="false" />
+ <node pkg="rosgazebo" type="run-rosgazebo.sh" args="" respawn="true" />
<node pkg="amcl_player" type="amcl_player" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" />
<node pkg="nav_view" type="nav_view" respawn="true" />
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-13 02:13:00 UTC (rev 800)
@@ -582,7 +582,7 @@
\param y - sideways (left)
\param w - upward
*/
- public: PR2_ERROR_CODE GetBasePositionActual(double* x, double* y, double* z);
+ public: PR2_ERROR_CODE GetBasePositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
/*! \fn
\brief - Run the robot
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-13 02:13:00 UTC (rev 800)
@@ -23,6 +23,7 @@
static gazebo::PositionIface *pr2LeftWristIface;
static gazebo::PositionIface *pr2RightWristIface;
+static gazebo::PositionIface *pr2BaseIface;
point Rot2D(point p,double theta)
@@ -113,7 +114,8 @@
pr2CameraHeadRightIface = new gazebo::CameraIface();
pr2LeftWristIface = new gazebo::PositionIface();
- pr2RightWristIface = new gazebo::PositionIface();
+ pr2RightWristIface = new gazebo::PositionIface();
+ pr2BaseIface = new gazebo::PositionIface();
int serverId = 0;
@@ -274,6 +276,17 @@
pr2RightWristIface = NULL;
}
+ try
+ {
+ pr2BaseIface->Open(client, "p3d_base_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the base position interface\n"
+ << e << "\n";
+ pr2BaseIface = NULL;
+ }
+
return PR2_ALL_OK;
}
@@ -1317,13 +1330,16 @@
return PR2_ALL_OK;
};
-PR2_ERROR_CODE PR2Robot::GetBasePositionActual(double* x, double* y, double* th)
+PR2_ERROR_CODE PR2Robot::GetBasePositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw)
{
- simIface->Lock(1);
- *x = simIface->data->model_pose.pos.x;
- *y = simIface->data->model_pose.pos.y;
- *th = simIface->data->model_pose.yaw;
- simIface->Unlock();
+ pr2BaseIface->Lock(1);
+ *x = pr2BaseIface->data->pose.pos.x;
+ *y = pr2BaseIface->data->pose.pos.y;
+ *z = pr2BaseIface->data->pose.pos.z;
+ *roll = pr2BaseIface->data->pose.roll;
+ *pitch = pr2BaseIface->data->pose.pitch;
+ *yaw = pr2BaseIface->data->pose.yaw;
+ pr2BaseIface->Unlock();
return PR2_ALL_OK;
};
Modified: pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-13 02:13:00 UTC (rev 800)
@@ -149,7 +149,7 @@
this->myIface->data->actuators_count = this->numJoints;
-// printf("numJoints: %d\n",this->numJoints);
+ //printf("numJoints: %d\n",this->numJoints);
for (int i=0; i < this->numJoints; i++)
{
if(this->joints[i]->GetType() == Joint::SLIDER)
@@ -178,7 +178,7 @@
sjoint->SetParam( dParamVel, this->myIface->data->actuators[i].pGain * positionError + this->myIface->data->actuators[i].dGain * speedError);
sjoint->SetParam( dParamFMax, this->myIface->data->actuators[i].saturationTorque );
}
-// printf("SLIDER:: pErr: %f, pGain: %f, dGain: %f, sT: %f\n",positionError,this->myIface->data->actuators[i].pGain,this->myIface->data->actuators[i].dGain,this->myIface->data->actuators[i].saturationTorque);
+ // printf("SLIDER:: pErr: %f, pGain: %f, dGain: %f, sT: %f\n",positionError,this->myIface->data->actuators[i].pGain,this->myIface->data->actuators[i].dGain,this->myIface->data->actuators[i].saturationTorque);
break;
case PR2::SPEED_CONTROL :
sjoint->SetParam( dParamVel, cmdSpeed);
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-13 02:13:00 UTC (rev 800)
@@ -377,10 +377,10 @@
this->odomMsg.vel.th = vw;
// Get position
- double x,y,th;
- this->myPR2->GetBasePositionActual(&x,&y,&th);
- this->odomMsg.pos.x = x + 256.5;
- this->odomMsg.pos.y = y + 256.5;
+ double x,y,z,roll,pitch,th;
+ this->myPR2->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&th);
+ this->odomMsg.pos.x = x;
+ this->odomMsg.pos.y = y;
this->odomMsg.pos.th = th;
// this->odomMsg.stall = this->positionmodel->Stall();
@@ -496,8 +496,6 @@
int
main(int argc, char** argv)
{
-
- usleep(1000000);
ros::init(argc,argv);
GazeboNode gn(argc,argv,argv[1]);
Deleted: pkg/trunk/simulators/rosgazebo/world/map3.png
===================================================================
(Binary files differ)
Deleted: pkg/trunk/simulators/rosgazebo/world/pr2.model
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-13 02:13:00 UTC (rev 800)
@@ -1,1426 +0,0 @@
-<?xml version="1.0"?>
-
-<!-- PR2 model -->
-<model:physical name="pr2_model"
- 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>
-
- <canonicalBody>base_body</canonicalBody>
-
- <body:box name="base_body">
- <xyz>0.175 0.0 -0.006</xyz>
- <rpy>0.0 0.0 0.0 </rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="base_body_geom">
- <xyz> 0.0 0.00 0.000</xyz>
- <size>0.65 0.65 0.012</size>
- <mass>100.0</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.65 0.65 0.012</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
-
- <geom:box name="base_spine_geom">
- <xyz> -0.175 0.00 0.5 </xyz>
- <size> 0.01 0.05 1.0 </size>
- <mass>0.01</mass>
- <mul>1</mul>
- <visual>
- <size> 0.01 0.05 1.0 </size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
-
- </body:box>
-
-
- <!-- Hokuyo laser body -->
- <canonicalBody>base_laser_body</canonicalBody>
- <body:box name="base_laser_body">
- <xyz>0.5 0.0 0.00</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="base_laser_box">
- <xyz>0.0 0.0 0.02</xyz>
- <rpy>0 0 0</rpy>
- <size>0.05 0.05 0.041</size>
- <mass>0.12</mass>
-
- <visual>
- <scale>0.05 0.05 0.041</scale>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
-
- </geom:box>
- <geom:cylinder name="base_laser_cylinder1">
- <xyz>0.0 0.0 0.041</xyz>
- <rpy>0 0 0</rpy>
- <size>0.02 0.013</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
-
- </geom:cylinder>
-
- <geom:cylinder name="base_laser_cylinder2">
- <xyz>0.0 0.0 0.054</xyz>
- <rpy>0 0 0</rpy>
- <size>0.018 0.009</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
-
- </geom:cylinder>
-
- <geom:cylinder name="base_laser_cylinder3">
- <xyz>0.0 0.0 0.063</xyz>
- <rpy>0 0 0</rpy>
- <size>0.0175 0.008</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
-
- </geom:cylinder>
-
-
- <sensor:ray name="base_laser_1">
- <rayCount>68</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
-
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sicklms200_laser name="base_laser_controller_1">
- <interface:laser name="base_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- <!--
- -->
- <!--
- <sensor:ray2 name="laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
-
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sick2lms200_laser>
- </sensor:ray2>
- -->
-
- </body:box>
-
- <!-- attach hokuyo to torso -->
- <joint:hinge name="hokuyo_base_y_joint">
- <body1>base_body</body1>
- <body2>base_laser_body</body2>
- <anchor>base_laser_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="hokuyo_base_z_joint">
- <body1>base_body</body1>
- <body2>base_laser_body</body2>
- <anchor>base_laser_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
-
-
-
- <!-- neck body -->
- <canonicalBody>neck_body</canonicalBody>
- <body:box name="neck_body">
- <xyz>0.15 0.0 1.08</xyz>
- <rpy>0.0 0.0 0.0</rpy>
- <!--xyz>0.0 0.0 0.0</xyz>
- <rpy>0.0 0.0 0.0</rpy-->
- <geom:box name="neck_geom">
- <size>.20 .38 .14</size>
- <mass>0</mass>
- <visual>
- <mesh>unit_box</mesh>
- <size>.20 .38 .14</size>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- </body:box>
- <!-- attach neck to torso -->
- <!--
- <attach>
- <parentBody>torso_body</parentBody>
- <myBody>neck_body</myBody>
- </attach>
- -->
- <joint:hinge name="fixed_neck_body_1">
- <body1>torso_body</body1>
- <body2>neck_body</body2>
- <anchor>torso_body</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="fixed_neck_body_2">
- <body1>torso_body</body1>
- <body2>neck_body</body2>
- <anchor>torso_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="fixed_neck_body_3">
- <body1>torso_body</body1>
- <body2>neck_body</body2>
- <anchor>torso_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
-
- <canonicalBody>head_base_body</canonicalBody>
- <body:box name="head_base_body">
- <xyz>0.15 0.0 1.18 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_base_geom">
- <xyz> 0.0 0.00 0.000</xyz>
- <size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
-
- <canonicalBody>head_tilt_body</canonicalBody>
- <body:box name="head_tilt_body">
- <xyz>0.15 0.0 1.12 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_tilt_geom">
- <xyz> 0.0 0.00 0.150</xyz>
- <size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_left_geom">
- <xyz> 0.0 -0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_right_geom">
- <xyz> 0.0 0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <!-- attach head_base to head_tilt -->
- <joint:hinge name="head_tilt_joint">
- <body2>head_tilt_body</body2>
- <body1>head_base_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
-
- <!-- attach head to neck -->
- <joint:hinge name="neck_yaw_joint">
- <body1>neck_body</body1>
- <body2>head_base_body</body2>
- <anchor>neck_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
-
-
- <!-- left camera -->
- <body:box name="head_cam_left_body">
- <xyz> 0.20 0.23 1.12 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="head_cam_left_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- <controller:generic_camera name="head_cam_left_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="head_cam_left_iface_0" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="head_cam_left_geom">
- <xyz> 0.00 0.00 0.00</xyz>
- <size>0.05 0.03 0.04</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.03 0.04</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
- <body:box name="head_cam_base_left_body">
- <xyz> 0.20 0.20 1.12 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <geom:box name="head_cam_base_left_geom">
- <xyz> 0.00 0.00 0.00</xyz>
- <size>0.05 0.02 0.02</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.02 0.02</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <!-- right camera -->
- <body:empty name="head_cam_right_body">
- <xyz> 0.20 -0.23 1.12 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="head_cam_right_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- <controller:generic_camera name="head_cam_right_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="head_cam_right_iface_0" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="head_cam_right_geom">
- <xyz> 0.00 0.00 0.00</xyz>
- <size>0.05 0.03 0.04</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.03 0.04</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:empty>
- <body:box name="head_cam_base_right_body">
- <xyz> 0.20 -0.20 1.12 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <geom:box name="head_cam_base_right_geom">
- <xyz> 0.00 0.00 0.00</xyz>
- <size>0.05 0.02 0.02</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.02 0.02</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <!-- attach neck_body to ptz cameras -->
- <joint:hinge name="head_cam_left_yaw_joint">
- <body1>head_cam_left_body</body1>
- <body2>head_cam_base_left_body</body2>
- <anchor>head_cam_left_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
- <joint:hinge name="head_cam_left_pitch_joint">
- <body1>head_cam_base_left_body</body1>
- <body2>neck_body</body2>
- <anchor>head_cam_base_left_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="head_cam_right_yaw_joint">
- <body2>head_cam_right_body</body2>
- <body1>head_cam_base_right_body</body1>
- <anchor>head_cam_right_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
- <joint:hinge name="head_cam_right_pitch_joint">
- <body2>head_cam_base_right_body</body2>
- <body1>neck_body</body1>
- <anchor>head_cam_base_right_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
-
-
- <!-- stereo camera -->
- <body:empty name="stereo_camera_body">
- <xyz> 0.15 0.0 1.3 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <!--sensor:stereocamera name="stereo_camera_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- <saveFrames>false</saveFrames>
- <saveFramePath>frames</saveFramePath>
- <baseline>0.2</baseline>
- <controller:stereo_camera name="stereo_camera_controller">
- <updateRate>15.0</updateRate>
- <interface:stereocamera name="stereo_iface_0" />
- <interface:camera name="camera_iface_0" />
- </controller:stereo_camera>
- </sensor:stereocamera-->
- <geom:box name="stereo_camera_geom">
- <xyz> 0.00 0.00 0.00</xyz>
- <size>0.10 0.15 0.10</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.10 0.15 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:empty>
-
- <!-- attach stereo_camera to head_tilt -->
- <joint:hinge name="stereo_camera_roll_joint">
- <body2>head_tilt_body</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="stereo_camera_pitch_joint">
- <body2>head_tilt_body</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="stereo_camera_yaw_joint">
- <body2>head_tilt_body</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 0.0 010 </axis>
- </joint:hinge>
-
-
-
- <!-- ========== dummy torso for prismatic joint =========== -->
- <canonicalBody>dummytorso_body</canonicalBody>
-
- <body:box name="dummytorso_body">
- <xyz>0.056 0.0 0.5</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="dummy_torso_geom">
- <xyz> 0.0 0.0 0.0 </xyz>
- <size> 0.01 0.01 0.01 </size>
- <mass>1.0</mass>
- <visual>
- <size> 0.01 0.01 0.01</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
-
- </body:box>
-
- <joint:hinge name="tmp1dummy">
- <body1>base_body</body1>
- <body2>dummytorso_body</body2>
- <anchor>dummytorso_body</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="tmp2dummy">
- <body1>base_body</body1>
- <body2>dummytorso_body</body2>
- <anchor>dummytorso_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- <cfm>0.0...
[truncated message content] |
|
From: <hsu...@us...> - 2008-06-13 05:53:38
|
Revision: 804
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=804&view=rev
Author: hsujohnhsu
Date: 2008-06-12 22:53:47 -0700 (Thu, 12 Jun 2008)
Log Message:
-----------
update run-rosgazebo to use rospack find.
update map3 to the latest demo map.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/world/map3.png
pkg/trunk/simulators/rosgazebo/run-rosgazebo.sh
Modified: pkg/trunk/demos/2dnav-gazebo/world/map3.png
===================================================================
(Binary files differ)
Modified: pkg/trunk/simulators/rosgazebo/run-rosgazebo.sh
===================================================================
--- pkg/trunk/simulators/rosgazebo/run-rosgazebo.sh 2008-06-13 05:43:19 UTC (rev 803)
+++ pkg/trunk/simulators/rosgazebo/run-rosgazebo.sh 2008-06-13 05:53:47 UTC (rev 804)
@@ -1,3 +1,3 @@
#!/bin/bash
-. ./setup.bash
-rosgazebo $*
+. `rospack find rosgazebo`/setup.bash
+`rospack find rosgazebo`/rosgazebo $*
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-14 00:26:03
|
Revision: 812
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=812&view=rev
Author: hsujohnhsu
Date: 2008-06-13 17:26:07 -0700 (Fri, 13 Jun 2008)
Log Message:
-----------
trying to fix the path following problem for the PR2 robot.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/world/robot.world
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/demos/2dnav-gazebo/world/robot.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-06-13 23:44:59 UTC (rev 811)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-06-14 00:26:07 UTC (rev 812)
@@ -408,73 +408,73 @@
<updateRate>100.0</updateRate>
<joint name="front_left_caster_steer">
<saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_left_wheel_l_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_left_wheel_r_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_right_caster_steer">
<saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_right_wheel_l_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_right_wheel_r_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_left_caster_steer">
<saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_left_wheel_l_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_left_wheel_r_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_right_caster_steer">
<saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_right_wheel_l_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_right_wheel_r_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-13 23:44:59 UTC (rev 811)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-14 00:26:07 UTC (rev 812)
@@ -561,6 +561,11 @@
public: PR2_ERROR_CODE SetBaseSteeringAngle(double vx, double vy, double vw);
/*! \fn
+ \brief Checks to see if steering angles are lined up
+ */
+ public: bool CheckBaseSteeringAngle(double errorTol);
+
+ /*! \fn
\brief Retrieve commanded speed for the base in cartesian space in body coordinates
\param vx - forward speed
\param vy - sideways speed
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-13 23:44:59 UTC (rev 811)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-14 00:26:07 UTC (rev 812)
@@ -1205,34 +1205,24 @@
point newDriveCenterL, newDriveCenterR;
- double currentRateCmd[NUM_CASTERS];
- double currentRateAct[NUM_CASTERS];
- double currentCmd[NUM_CASTERS];
- double currentAngle[NUM_CASTERS];
- double currentError[NUM_CASTERS];
- const double errorTol = M_PI / 50.0;
-
for(int ii=0; ii < NUM_CASTERS; ii++)
{
ComputePointVelocity(vx,vy,vw,BASE_CASTER_OFFSET[ii].x,BASE_CASTER_OFFSET[ii].y,steerPointVelocity[ii].x,steerPointVelocity[ii].y);
steerAngle[ii] = atan2(steerPointVelocity[ii].y,steerPointVelocity[ii].x);
SetJointServoCmd((PR2_JOINT_ID) (CASTER_FL_STEER+3*ii),steerAngle[ii],0);
-// printf("ii: %d, off: (%f, %f), vel: (%f, %f), angle: %f\n",ii,BASE_CASTER_OFFSET[ii].x,BASE_CASTER_OFFSET[ii].y,steerPointVelocity[ii].x,steerPointVelocity[ii].y,steerAngle[ii]);
+ // printf("ii: %d, off: (%f, %f), vel: (%f, %f), angle: %f\n",ii,BASE_CASTER_OFFSET[ii].x,BASE_CASTER_OFFSET[ii].y,steerPointVelocity[ii].x,steerPointVelocity[ii].y,steerAngle[ii]);
}
for(int ii = 0; ii < NUM_CASTERS; ii++)
{
- // do not move forward unless the wheels are aligned...
- // this is a goofy hack, should put a more complicated version later
- GetJointServoCmd((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tCmd[ii],¤tRateCmd[ii]);
- GetJointServoActual((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tAngle[ii],¤tRateAct[ii]);
- currentError[ii] = fabs(currentCmd[ii] - currentAngle[ii]);
- //std::cout << " e " << ii << " e " << currentError[ii] << std::endl;
- if (currentError[ii] < errorTol)
- {
newDriveCenterL = Rot2D(CASTER_DRIVE_OFFSET[ii*2 ],steerAngle[ii]);
newDriveCenterR = Rot2D(CASTER_DRIVE_OFFSET[ii*2+1],steerAngle[ii]);
+ newDriveCenterL.x += BASE_CASTER_OFFSET[ii].x;
+ newDriveCenterL.y += BASE_CASTER_OFFSET[ii].y;
+ newDriveCenterR.x += BASE_CASTER_OFFSET[ii].x;
+ newDriveCenterR.y += BASE_CASTER_OFFSET[ii].y;
+
ComputePointVelocity(vx,vy,vw,newDriveCenterL.x,newDriveCenterL.y,drivePointVelocityL.x,drivePointVelocityL.y);
ComputePointVelocity(vx,vy,vw,newDriveCenterR.x,newDriveCenterR.y,drivePointVelocityR.x,drivePointVelocityR.y);
@@ -1242,16 +1232,53 @@
// send command
this->SetJointSpeed((PR2_JOINT_ID) (CASTER_FL_DRIVE_L+3*ii),wheelSpeed[ii*2 ]);
this->SetJointSpeed((PR2_JOINT_ID) (CASTER_FL_DRIVE_R+3*ii),wheelSpeed[ii*2+1]);
- this->SetJointTorque((PR2_JOINT_ID) (CASTER_FL_DRIVE_L+3*ii),100.0 );
- this->SetJointTorque((PR2_JOINT_ID) (CASTER_FL_DRIVE_R+3*ii),100.0 );
- }
}
return PR2_ALL_OK;
};
+// PR2_ERROR_CODE PR2Robot::SetBaseUnicycleSpeedCmd(double vx, double vw)
+// {
+// point drivePointVelocityL, drivePointVelocityR;
+// double wheelSpeed[NUM_WHEELS];
+//
+// point steerPointVelocity[NUM_CASTERS];
+// double steerAngle[NUM_CASTERS];
+//
+// point newDriveCenterL, newDriveCenterR;
+//
+// ComputePointVelocity(vx,vy,vw,BASE_CASTER_OFFSET[0].x,BASE_CASTER_OFFSET[0].y,steerPointVelocity[0].x,steerPointVelocity[0].y);
+// steerAngle[0] = atan2(steerPointVelocity[0].y,steerPointVelocity[0].x);
+// ComputePointVelocity(vx,vy,vw,BASE_CASTER_OFFSET[1].x,BASE_CASTER_OFFSET[1].y,steerPointVelocity[1].x,steerPointVelocity[1].y);
+// steerAngle[1] = atan2(steerPointVelocity[1].y,steerPointVelocity[1].x);
+// SetJointServoCmd((PR2_JOINT_ID) (CASTER_FL_STEER),steerAngle[0],0);
+// SetJointServoCmd((PR2_JOINT_ID) (CASTER_FR_STEER),steerAngle[1],0);
+// SetJointServoCmd((PR2_JOINT_ID) (CASTER_RL_STEER), 0,0);
+// SetJointServoCmd((PR2_JOINT_ID) (CASTER_RR_STEER), 0,0);
+// // printf("ii: %d, off: (%f, %f), vel: (%f, %f), angle: %f\n",ii,BASE_CASTER_OFFSET[ii].x,BASE_CASTER_OFFSET[ii].y,steerPointVelocity[ii].x,steerPointVelocity[ii].y,steerAngle[ii]);
+//
+// for(int ii = 0; ii < NUM_CASTERS; ii++)
+// {
+//
+// newDriveCenterL = Rot2D(CASTER_DRIVE_OFFSET[ii*2 ],steerAngle[ii]);
+// newDriveCenterR = Rot2D(CASTER_DRIVE_OFFSET[ii*2+1],steerAngle[ii]);
+//
+// ComputePointVelocity(vx,vy,vw,newDriveCenterL.x,newDriveCenterL.y,drivePointVelocityL.x,drivePointVelocityL.y);
+// ComputePointVelocity(vx,vy,vw,newDriveCenterR.x,newDriveCenterR.y,drivePointVelocityR.x,drivePointVelocityR.y);
+//
+// wheelSpeed[ii*2 ] = -GetMagnitude(drivePointVelocityL.x,drivePointVelocityL.y)/WHEEL_RADIUS;
+// wheelSpeed[ii*2+1] = -GetMagnitude(drivePointVelocityR.x,drivePointVelocityR.y)/WHEEL_RADIUS;
+//
+// // send speed command
+// this->SetJointSpeed((PR2_JOINT_ID) (CASTER_FL_DRIVE_L),wheelSpeed[ii*2 ]);
+// this->SetJointSpeed((PR2_JOINT_ID) (CASTER_FL_DRIVE_R),wheelSpeed[ii*2+1]);
+// }
+//
+// return PR2_ALL_OK;
+// };
+
PR2_ERROR_CODE PR2Robot::SetBaseSteeringAngle(double vx, double vy, double vw)
{
point steerPointVelocity[NUM_CASTERS];
@@ -1270,16 +1297,40 @@
return PR2_ALL_OK;
};
+bool PR2Robot::CheckBaseSteeringAngle(double errorTol)
+{
+ point steerPointVelocity[NUM_CASTERS];
+ double steerAngle[NUM_CASTERS];
+ point newDriveCenterL, newDriveCenterR;
+ double currentRateCmd[NUM_CASTERS];
+ double currentRateAct[NUM_CASTERS];
+ double currentCmd[NUM_CASTERS];
+ double currentAngle[NUM_CASTERS];
+ double currentError[NUM_CASTERS];
+ double errorTotal;
+ errorTotal = 0.0;
+ for(int ii=0; ii < NUM_CASTERS; ii++)
+ {
+ // do not move forward unless the wheels are aligned...
+ // this is a goofy hack, should put a more complicated version later
+ GetJointServoCmd((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tCmd[ii],¤tRateCmd[ii]);
+ GetJointServoActual((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tAngle[ii],¤tRateAct[ii]);
+ currentError[ii] = fabs(currentCmd[ii] - currentAngle[ii]);
+ //std::cout << " e " << ii << " e " << currentError[ii] << std::endl;
+ errorTotal = errorTotal + currentError[ii];
+ }
+ if (errorTotal < errorTol)
+ return true;
+ else
+ return false;
+};
-
-
-
PR2_ERROR_CODE PR2Robot::GetBaseCartesianSpeedCmd(double* vx, double* vy, double* vw)
{
// FIXME: TODO: not implemented
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-13 23:44:59 UTC (rev 811)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-14 00:26:07 UTC (rev 812)
@@ -70,6 +70,12 @@
// for frame transforms, publish frame transforms
rosTFServer tf;
+ // time step calculation
+ double lastTime, simTime;
+
+ // smooth vx, vw commands
+ double vxSmooth, vwSmooth;
+
public:
// Constructor; stage itself needs argc/argv. fname is the .world file
// that stage should load.
@@ -188,12 +194,44 @@
GazeboNode::cmdvelReceived()
{
this->lock.lock();
+ double dt;
+ double w11, w21, w12, w22;
+ w11 = 1.0;
+ w21 = 1.0;
+ w12 = 1.0;
+ w22 = 1.0;
- //printf("received cmd: %.3f %.3f\n",
- // this->velMsg.vx, this->velMsg.vw);
+ // smooth out the commands by time decay
+ // with w1,w2=1, this means equal weighting for new command every second
+ this->myPR2->GetSimTime(&(this->simTime));
+ dt = simTime - lastTime;
+ //this->vxSmooth = (w11 * this->vxSmooth + w21*dt *this->velMsg.vx)/( w11 + w21*dt);
+ //this->vwSmooth = (w12 * this->vwSmooth + w22*dt *this->velMsg.vw)/( w12 + w22*dt);
+ this->vxSmooth = this->velMsg.vx;
+ this->vwSmooth = this->velMsg.vw;
+
+ fprintf(stderr,"received cmd: %.3f %.3f | %.3f %.3f\n", this->velMsg.vx, this->velMsg.vw,this->vxSmooth, this->vwSmooth);
+
+ this->myPR2->SetBaseSteeringAngle (this->vxSmooth,0.0,this->vwSmooth);
+ while (!this->myPR2->CheckBaseSteeringAngle(M_PI/10.0))
+ {
+ // do nothing and wait...
+ usleep(100000);
+ }
// set base velocity
- this->myPR2->SetBaseCartesianSpeedCmd(this->velMsg.vx, 0.0, this->velMsg.vw);
+ this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_R, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_R, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_R, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_R, 1000.0 );
+ this->myPR2->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
+
+ this->lastTime = this->simTime;
+
this->lock.unlock();
}
@@ -224,6 +262,9 @@
*/
this->myPR2->EnableGripperLeft();
this->myPR2->EnableGripperRight();
+
+ this->myPR2->GetSimTime(&(this->lastTime));
+ this->myPR2->GetSimTime(&(this->simTime));
}
void GazeboNode::finalize(int)
@@ -267,7 +308,6 @@
uint32_t intensities_size;
uint32_t intensities_alloc_size;
std_msgs::Point3DFloat32 tmp_cloud_pt;
- double simTime;
/***************************************************************/
/* */
@@ -323,10 +363,8 @@
}
+ this->myPR2->GetSimTime(&(this->simTime));
-
- this->myPR2->GetSimTime(&simTime);
-
/***************************************************************/
/* */
/* laser - base */
@@ -353,8 +391,8 @@
}
this->laserMsg.header.frame_id = FRAMEID_LASER;
- this->laserMsg.header.stamp.sec = (unsigned long)floor(simTime);
- this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( simTime - this->laserMsg.header.stamp.sec) );
+ this->laserMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
+ this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->laserMsg.header.stamp.sec) );
//publish("laser",this->laserMsg); // for laser_view FIXME: can alias this at the commandline or launch script
publish("scan",this->laserMsg); // for rosstage
@@ -387,8 +425,8 @@
// TODO: get the frame ID from somewhere
this->odomMsg.header.frame_id = FRAMEID_ODOM;
- this->odomMsg.header.stamp.sec = (unsigned long)floor(simTime);
- this->odomMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( simTime - this->odomMsg.header.stamp.sec) );
+ this->odomMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
+ this->odomMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->odomMsg.header.stamp.sec) );
publish("odom",this->odomMsg);
@@ -447,10 +485,10 @@
simPitchTimeScale = 2.0*M_PI*simPitchFreq;
simPitchAmp = M_PI / 8.0;
simPitchOffset = -M_PI / 8.0;
- simPitchAngle = simPitchOffset + simPitchAmp * sin(simTime * simPitchTimeScale);
- simPitchRate = simPitchAmp * simPitchTimeScale * cos(simTime * simPitchTimeScale); // TODO: check rate correctness
- this->myPR2->GetSimTime(&simTime);
- //std::cout << "sim time: " << simTime << std::endl;
+ simPitchAngle = simPitchOffset + simPitchAmp * sin(this->simTime * simPitchTimeScale);
+ simPitchRate = simPitchAmp * simPitchTimeScale * cos(this->simTime * simPitchTimeScale); // TODO: check rate correctness
+ this->myPR2->GetSimTime(&this->simTime);
+ //std::cout << "sim time: " << this->simTime << std::endl;
//std::cout << "ang: " << simPitchAngle*180.0/M_PI << "rate: " << simPitchRate*180.0/M_PI << std::endl;
this->myPR2->SetJointTorque(PR2::HEAD_LASER_PITCH , 1000.0);
this->myPR2->SetJointGains(PR2::HEAD_LASER_PITCH, 10.0, 0.0, 0.0);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-14 20:51:07
|
Revision: 816
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=816&view=rev
Author: hsujohnhsu
Date: 2008-06-14 13:51:13 -0700 (Sat, 14 Jun 2008)
Log Message:
-----------
steering and moving separated. this seems to fix the 2dnav-gazebo nav problem for now.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-14 02:20:29 UTC (rev 815)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-14 20:51:13 UTC (rev 816)
@@ -563,7 +563,7 @@
/*! \fn
\brief Checks to see if steering angles are lined up
*/
- public: bool CheckBaseSteeringAngle(double errorTol);
+ public: double BaseSteeringAngleError();
/*! \fn
\brief Retrieve commanded speed for the base in cartesian space in body coordinates
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-14 02:20:29 UTC (rev 815)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-14 20:51:13 UTC (rev 816)
@@ -87,6 +87,68 @@
return sqrt(x*x+y*y);
}
+
+/* --------------- utility functions ---------------- */
+/* --------------- TODO ---------------- */
+/* --------------- move to a library ---------------- */
+
+#define FROM_DEGREES(degrees) (((double)(degrees))/180*M_PI)
+/*
+ * normalize_angle_positive
+ *
+ * Normalizes the angle to be 0 circle to 1 circle
+ * It takes and returns native units.
+ */
+
+double normalize_angle_positive(double angle)
+{
+ return fmod(fmod(angle, FROM_DEGREES(360))+FROM_DEGREES(360), FROM_DEGREES(360));
+}
+/*
+ * normalize
+ *
+ * Normalizes the angle to be -1/2 circle to +1/2 circle
+ * It takes and returns native units.
+ *
+ */
+
+double normalize_angle(double angle)
+{
+ double a=normalize_angle_positive(angle);
+ if (a>FROM_DEGREES(180))
+ a-=FROM_DEGREES(360);
+ return(a);
+}
+/*
+ * shortest_angular_distance
+ *
+ * Given 2 angles, this returns the shortest angular
+ * difference. The inputs and ouputs are of course native
+ * units.
+ *
+ * As an example, if native units are degrees, the result
+ * would always be -180 <= result <= 180. Adding the result
+ * to "from" will always get you an equivelent angle to "to".
+ */
+
+double shortest_angular_distance(double from, double to)
+{
+ double result;
+ result=normalize_angle_positive(
+ normalize_angle_positive(to) - normalize_angle_positive(from));
+
+ if ( result > FROM_DEGREES(180) ) { // If the result > 180,
+ // It's shorter the other way.
+ result=-(FROM_DEGREES(360)-result);
+ }
+ return normalize_angle(result);
+}
+
+
+
+
+
+
PR2Robot::PR2Robot()
{
};
@@ -1297,7 +1359,7 @@
return PR2_ALL_OK;
};
-bool PR2Robot::CheckBaseSteeringAngle(double errorTol)
+double PR2Robot::BaseSteeringAngleError()
{
point steerPointVelocity[NUM_CASTERS];
double steerAngle[NUM_CASTERS];
@@ -1319,15 +1381,14 @@
// this is a goofy hack, should put a more complicated version later
GetJointServoCmd((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tCmd[ii],¤tRateCmd[ii]);
GetJointServoActual((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tAngle[ii],¤tRateAct[ii]);
- currentError[ii] = fabs(currentCmd[ii] - currentAngle[ii]);
- //std::cout << " e " << ii << " e " << currentError[ii] << std::endl;
+ currentError[ii] = fabs(shortest_angular_distance(currentCmd[ii] , currentAngle[ii]));
+ //std::cout << " ii " << ii << " e " << currentError[ii] << std::endl;
+ fprintf(stdout," ii %d e %.3f \n", ii , currentError[ii]);
errorTotal = errorTotal + currentError[ii];
}
+ fprintf(stdout," ----------------------------------\n");
- if (errorTotal < errorTol)
- return true;
- else
- return false;
+ return errorTotal;
};
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-14 02:20:29 UTC (rev 815)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-14 20:51:13 UTC (rev 816)
@@ -196,39 +196,37 @@
this->lock.lock();
double dt;
double w11, w21, w12, w22;
- w11 = 1.0;
- w21 = 1.0;
- w12 = 1.0;
- w22 = 1.0;
// smooth out the commands by time decay
// with w1,w2=1, this means equal weighting for new command every second
this->myPR2->GetSimTime(&(this->simTime));
dt = simTime - lastTime;
- //this->vxSmooth = (w11 * this->vxSmooth + w21*dt *this->velMsg.vx)/( w11 + w21*dt);
- //this->vwSmooth = (w12 * this->vwSmooth + w22*dt *this->velMsg.vw)/( w12 + w22*dt);
- this->vxSmooth = this->velMsg.vx;
- this->vwSmooth = this->velMsg.vw;
+ // smooth if dt is larger than zero
+ if (dt > 0.0)
+ {
+ w11 = 0.0;
+ w21 = 1.0;
+ w12 = 0.0;
+ w22 = 1.0;
+ this->vxSmooth = (w11 * this->vxSmooth + w21*dt *this->velMsg.vx)/( w11 + w21*dt);
+ this->vwSmooth = (w12 * this->vwSmooth + w22*dt *this->velMsg.vw)/( w12 + w22*dt);
+ }
- fprintf(stderr,"received cmd: %.3f %.3f | %.3f %.3f\n", this->velMsg.vx, this->velMsg.vw,this->vxSmooth, this->vwSmooth);
-
- this->myPR2->SetBaseSteeringAngle (this->vxSmooth,0.0,this->vwSmooth);
- while (!this->myPR2->CheckBaseSteeringAngle(M_PI/10.0))
+ // if steering angle is wrong, don't move or move slowly
+ std::cout << "received cmd: vx " << this->velMsg.vx << " vw " << this->velMsg.vw
+ << " vxsmoo " << this->vxSmooth << " vxsmoo " << this->vwSmooth
+ << " | steer erros: " << this->myPR2->BaseSteeringAngleError() << " - " << M_PI/100.0
+ << std::endl;
+ if (this->myPR2->BaseSteeringAngleError() > M_PI/100.0)
{
- // do nothing and wait...
- usleep(100000);
+ this->myPR2->SetBaseSteeringAngle (this->vxSmooth,0.0,this->vwSmooth);
}
- // set base velocity
- this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
- this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_L, 1000.0 );
- this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_L, 1000.0 );
- this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_L, 1000.0 );
- this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_R, 1000.0 );
- this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_R, 1000.0 );
- this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_R, 1000.0 );
- this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_R, 1000.0 );
- this->myPR2->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
+ else
+ {
+ // set base velocity
+ this->myPR2->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
+ }
this->lastTime = this->simTime;
@@ -265,6 +263,16 @@
this->myPR2->GetSimTime(&(this->lastTime));
this->myPR2->GetSimTime(&(this->simTime));
+
+ // set torques for driving the robot wheels
+ this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_R, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_R, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_R, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_R, 1000.0 );
}
void GazeboNode::finalize(int)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-16 15:48:19
|
Revision: 820
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=820&view=rev
Author: hsujohnhsu
Date: 2008-06-16 08:48:18 -0700 (Mon, 16 Jun 2008)
Log Message:
-----------
using controllers/Pid library for gazebo_plugin.
Modified Paths:
--------------
pkg/trunk/controllers/manifest.xml
pkg/trunk/controllers/src/Pid.cpp
pkg/trunk/simulators/gazebo_plugin/Makefile
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/simulators/gazebo_plugin/manifest.xml
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
Modified: pkg/trunk/controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/manifest.xml 2008-06-16 15:45:47 UTC (rev 819)
+++ pkg/trunk/controllers/manifest.xml 2008-06-16 15:48:18 UTC (rev 820)
@@ -9,6 +9,6 @@
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
-<cpp cflags='-I${prefix}/include' lflags='-L${prefix}/lib'/>
+<cpp cflags='-I${prefix}/include' lflags='-L${prefix}/lib -lPid '/>
</export>
</package>
Modified: pkg/trunk/controllers/src/Pid.cpp
===================================================================
--- pkg/trunk/controllers/src/Pid.cpp 2008-06-16 15:45:47 UTC (rev 819)
+++ pkg/trunk/controllers/src/Pid.cpp 2008-06-16 15:48:18 UTC (rev 820)
@@ -62,44 +62,47 @@
if (dt == 0)
{
- throw "dividebyzero";
+ // no update except for lastTime
+ //throw "dividebyzero";
}
+ else
+ {
+ // calculate proportional contribution to command
+ pTerm = pGain * pError;
- // calculate proportional contribution to command
- pTerm = pGain * pError;
+ // CALCULATE THE INTEGRAL ERROR ACCUMULATION WITH APPROPRIATE LIMITING
+ iError = iError + dt * pError;
+ // limit iError
+ if (iError > iMax)
+ {
+ iError = iMax;
+ }
+ else if (iError < iMin)
+ {
+ iError = iMin;
+ }
+ // calculate integral contribution to command
+ iTerm = iGain * iError;
- // CALCULATE THE INTEGRAL ERROR ACCUMULATION WITH APPROPRIATE LIMITING
- iError = iError + dt * pError;
- // limit iError
- if (iError > iMax)
- {
- iError = iMax;
- }
- else if (iError < iMin)
- {
- iError = iMin;
- }
- // calculate integral contribution to command
- iTerm = iGain * iError;
+ // CALCULATE DERIVATIVE ERROR
+ if (dt != 0)
+ {
+ // dError should be d (pError) / dt
+ dError = smooth(dError,(pError - pErrorLast) / dt, dt);
+ // save pError for next time
+ pErrorLast = pError;
+ }
+ // calculate derivative contribution to command
+ dTerm = dGain * dError;
- // CALCULATE DERIVATIVE ERROR
- if (dt != 0)
- {
- // dError should be d (pError) / dt
- dError = smooth(dError,(pError - pErrorLast) / dt, dt);
- // save pError for next time
- pErrorLast = pError;
+ // create current command value
+ currentCmd = pTerm + iTerm + dTerm;
+
+ // command limits
+ currentCmd = (currentCmd > cmdMax) ? cmdMax :
+ ((currentCmd < cmdMin) ? cmdMin : currentCmd);
}
- // calculate derivative contribution to command
- dTerm = dGain * dError;
- // create current command value
- currentCmd = pTerm + iTerm + dTerm;
-
- // command limits
- currentCmd = (currentCmd > cmdMax) ? cmdMax :
- ((currentCmd < cmdMin) ? cmdMin : currentCmd);
-
// save timestamp
lastTime = currentTime;
Modified: pkg/trunk/simulators/gazebo_plugin/Makefile
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/Makefile 2008-06-16 15:45:47 UTC (rev 819)
+++ pkg/trunk/simulators/gazebo_plugin/Makefile 2008-06-16 15:48:18 UTC (rev 820)
@@ -6,6 +6,7 @@
LDFLAGS = $(shell rospack export/cpp/lflags gazebo) \
$(shell rospack export/cpp/lflags pr2Core) \
+ $(shell rospack export/cpp/lflags controllers) \
$(shell rospack export/cpp/lflags newmat10)
LIB_ACT = lib/libPr2_Actarray.a
Modified: pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-06-16 15:45:47 UTC (rev 819)
+++ pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-06-16 15:48:18 UTC (rev 820)
@@ -31,6 +31,9 @@
#include <gazebo/Entity.hh>
#include <pr2Core/pr2Core.h>
+// use controllers
+#include <controllers/Pid.h>
+
namespace gazebo
{
class HingeJoint;
@@ -88,6 +91,9 @@
private: float forces[256];
private: float gains[256];
private: int numJoints;
+
+ // we'll declare a pid controller for each hinger/slider/... joint
+ private: Pid *pids[256];
};
/** \} */
Modified: pkg/trunk/simulators/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/manifest.xml 2008-06-16 15:45:47 UTC (rev 819)
+++ pkg/trunk/simulators/gazebo_plugin/manifest.xml 2008-06-16 15:48:18 UTC (rev 820)
@@ -11,6 +11,7 @@
<depend package="gazebo"/>
<depend package="pr2Core"/>
<depend package="newmat10"/>
+<depend package="controllers"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lPr2_Actarray -lPr2_Gripper -lP3D"/>
</export>
Modified: pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-16 15:45:47 UTC (rev 819)
+++ pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-16 15:48:18 UTC (rev 820)
@@ -57,7 +57,10 @@
}
////////////////////////////////////////////////////////////////////////////////
+//
// Constructor
+//
+////////////////////////////////////////////////////////////////////////////////
Pr2_Actarray::Pr2_Actarray(Entity *parent )
: Controller(parent)
{
@@ -69,13 +72,23 @@
}
////////////////////////////////////////////////////////////////////////////////
+//
// Destructor
+//
+////////////////////////////////////////////////////////////////////////////////
Pr2_Actarray::~Pr2_Actarray()
{
}
////////////////////////////////////////////////////////////////////////////////
+//
// Load the controller
+//
+// loop through all joints
+// check joint type
+// initialize variables
+//
+////////////////////////////////////////////////////////////////////////////////
void Pr2_Actarray::LoadChild(XMLConfigNode *node)
{
XMLConfigNode *jNode;
@@ -110,15 +123,19 @@
this->myIface->data->actuators[i].actualSpeed = hjoint->GetAngleRate();
this->myIface->data->actuators[i].actualEffectorForce = 0.0; //this must be modified using the JointFeedback struct at some point
this->myIface->data->actuators[i].jointType = PR2::ROTARY;
-
}
this->myIface->data->actuators[i].saturationTorque = jNode->GetDouble("saturationTorque",0.0,1);
this->myIface->data->actuators[i].pGain = jNode->GetDouble("pGain",0.0,1);
- this->myIface->data->actuators[i].dGain = jNode->GetDouble("iGain",0.0,1);
+ this->myIface->data->actuators[i].iGain = jNode->GetDouble("iGain",0.0,1);
this->myIface->data->actuators[i].dGain = jNode->GetDouble("dGain",0.0,1);
+ // init a new pid for this joint
+ this->pids[i] = new Pid();
+
+ // get time
this->myIface->data->actuators[i].timestamp = Simulator::Instance()->GetSimTime();
+ // set default control mode to PD
this->myIface->data->actuators[i].controlMode = PR2::PD_CONTROL;
jNode = jNode->GetNext("joint");
@@ -127,112 +144,178 @@
}
////////////////////////////////////////////////////////////////////////////////
+//
// Initialize the controller
+//
+// set all joints to zero velocity and saturation torque
+//
+////////////////////////////////////////////////////////////////////////////////
void Pr2_Actarray::InitChild()
{
for (int i=0; i < this->numJoints; i++)
{
- this->joints[i]->SetParam( dParamVel, 0.0);
- this->joints[i]->SetParam( dParamFMax, this->myIface->data->actuators[i].saturationTorque );
+ // initialize pid stuff
+ this->pids[i]->InitPid( this->myIface->data->actuators[i].pGain,
+ this->myIface->data->actuators[i].iGain,
+ this->myIface->data->actuators[i].dGain,
+ 1.0,
+ -1.0,
+ this->joints[i]->GetHighStop(),
+ this->joints[i]->GetLowStop()
+ );
+ this->pids[i]->SetCurrentCmd(0.0);
+
+ // as a first hack, initialize to zero velocity and saturation torque
+ this->joints[i]->SetParam( dParamVel , this->pids[i]->GetCurrentCmd());
+ this->joints[i]->SetParam( dParamFMax, this->myIface->data->actuators[i].saturationTorque );
}
}
////////////////////////////////////////////////////////////////////////////////
+//
// Update the controller
+//
+// one step in a PID loop
+//
+// ALGORITHM
+// =========
+// go through all joints,
+// check joint type
+// check control type
+// check error
+// compute and set new command
+//
+// ODE I/O
+// =======
+// all we can set in ode is
+// velocity
+// torque
+//
+// Control Modes
+// =============
+// torque control mode
+// set velocity to the right "side" of the current velocity
+// run pid on the torque value
+// pd control
+// set velocity based on position error
+// set torque to saturation torque
+// velocity control
+// set velocity based on velocity error
+// set torque to saturation torque
+//
+////////////////////////////////////////////////////////////////////////////////
void Pr2_Actarray::UpdateChild(UpdateParams ¶ms)
{
float positionError, speedError;
- float hiStop, loStop;
+ HingeJoint *hjoint;
+ SliderJoint *sjoint;
+ double cmdPosition, cmdSpeed;
+ double currentTime;
+ double currentCmd;
this->myIface->Lock(1);
- this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
+ currentTime = Simulator::Instance()->GetSimTime();
+ this->myIface->data->head.time = currentTime;
+
this->myIface->data->actuators_count = this->numJoints;
+ //printf("numJoints: %d\n",this->numJoints);
- //printf("numJoints: %d\n",this->numJoints);
+ //////////////////////////////////////////////////////////////////////
+ //
+ // LOOP THROUGH ALL THE CONTROLLABLE DOF'S IN THIS INTERFACE
+ //
+ //////////////////////////////////////////////////////////////////////
for (int i=0; i < this->numJoints; i++)
{
- if(this->joints[i]->GetType() == Joint::SLIDER)
+ switch(this->joints[i]->GetType())
{
- SliderJoint *sjoint = dynamic_cast<SliderJoint*>(this->joints[i]);
- double cmdPosition = this->myIface->data->actuators[i].cmdPosition;
- double cmdSpeed = this->myIface->data->actuators[i].cmdSpeed;
+ case Joint::SLIDER:
+ sjoint = dynamic_cast<SliderJoint*>(this->joints[i]);
+ cmdPosition = this->myIface->data->actuators[i].cmdPosition;
+ cmdSpeed = this->myIface->data->actuators[i].cmdSpeed;
- switch(this->myIface->data->actuators[i].controlMode)
- {
- case PR2::TORQUE_CONTROL :
- // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
- sjoint->SetSliderForce(this->myIface->data->actuators[i].cmdEffectorForce);
- break;
- case PR2::PD_CONTROL :
- if (cmdPosition > sjoint->GetHighStop())
- cmdPosition = sjoint->GetHighStop();
- else if (cmdPosition < sjoint->GetLowStop())
- cmdPosition = sjoint->GetLowStop();
+ switch(this->myIface->data->actuators[i].controlMode)
+ {
+ case PR2::TORQUE_CONTROL :
+ // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
+ sjoint->SetSliderForce(this->myIface->data->actuators[i].cmdEffectorForce);
+ break;
+ case PR2::PD_CONTROL :
+ if (cmdPosition > sjoint->GetHighStop())
+ cmdPosition = sjoint->GetHighStop();
+ else if (cmdPosition < sjoint->GetLowStop())
+ cmdPosition = sjoint->GetLowStop();
- positionError = cmdPosition - sjoint->GetPosition();
- speedError = cmdSpeed - sjoint->GetPositionRate();
+ positionError = cmdPosition - sjoint->GetPosition();
+ speedError = cmdSpeed - sjoint->GetPositionRate();
+ currentCmd = this->pids[i]->UpdatePid(positionError,currentTime);
- if (fabs(positionError) > 0.01)
- {
- sjoint->SetParam( dParamVel, this->myIface->data->actuators[i].pGain * positionError + this->myIface->data->actuators[i].dGain * speedError);
- sjoint->SetParam( dParamFMax, this->myIface->data->actuators[i].saturationTorque );
- }
- // printf("SLIDER:: pErr: %f, pGain: %f, dGain: %f, sT: %f\n",positionError,this->myIface->data->actuators[i].pGain,this->myIface->data->actuators[i].dGain,this->myIface->data->actuators[i].saturationTorque);
- break;
- case PR2::SPEED_CONTROL :
- sjoint->SetParam( dParamVel, cmdSpeed);
- sjoint->SetParam( dParamFMax,this->myIface->data->actuators[i].saturationTorque );
- break;
+ //if (fabs(positionError) > 0.01)
+ //{
+ sjoint->SetParam( dParamVel , currentCmd );
+ sjoint->SetParam( dParamFMax, this->myIface->data->actuators[i].saturationTorque );
+ //}
+ // printf("SLIDER:: pErr: %f, pGain: %f, dGain: %f, sT: %f\n",
+ // positionError,this->myIface->data->actuators[i].pGain,this->myIface->data->actuators[i].dGain,
+ // this->myIface->data->actuators[i].saturationTorque);
+ break;
+ case PR2::SPEED_CONTROL :
+ sjoint->SetParam( dParamVel, cmdSpeed);
+ sjoint->SetParam( dParamFMax,this->myIface->data->actuators[i].saturationTorque );
+ break;
- default:
- break;
- }
+ default:
+ break;
+ }
- this->myIface->data->actuators[i].actualPosition = sjoint->GetPosition();
- this->myIface->data->actuators[i].actualSpeed = sjoint->GetPositionRate();
- this->myIface->data->actuators[i].actualEffectorForce = 0.0; //this must be modified using the JointFeedback struct at some point
- }
- else
- {
- HingeJoint *hjoint = dynamic_cast<HingeJoint*>(this->joints[i]);
- double cmdPosition = this->myIface->data->actuators[i].cmdPosition;
- double cmdSpeed = this->myIface->data->actuators[i].cmdSpeed;
+ this->myIface->data->actuators[i].actualPosition = sjoint->GetPosition();
+ this->myIface->data->actuators[i].actualSpeed = sjoint->GetPositionRate();
+ this->myIface->data->actuators[i].actualEffectorForce = 0.0; //this must be modified using the JointFeedback struct at some point
+ break;
- switch(this->myIface->data->actuators[i].controlMode)
- {
- case PR2::TORQUE_CONTROL:
- // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
- hjoint->SetTorque(this->myIface->data->actuators[i].cmdEffectorForce);
- break;
- case PR2::PD_CONTROL:
- if (cmdPosition > hjoint->GetHighStop())
- cmdPosition = hjoint->GetHighStop();
- else if (cmdPosition < hjoint->GetLowStop())
- cmdPosition = hjoint->GetLowStop();
+ case Joint::HINGE:
+ hjoint = dynamic_cast<HingeJoint*>(this->joints[i]);
+ cmdPosition = this->myIface->data->actuators[i].cmdPosition;
+ cmdSpeed = this->myIface->data->actuators[i].cmdSpeed;
+ switch(this->myIface->data->actuators[i].controlMode)
+ {
+ case PR2::TORQUE_CONTROL:
+ // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
+ hjoint->SetTorque(this->myIface->data->actuators[i].cmdEffectorForce);
+ break;
+ case PR2::PD_CONTROL:
+ if (cmdPosition > hjoint->GetHighStop())
+ cmdPosition = hjoint->GetHighStop();
+ else if (cmdPosition < hjoint->GetLowStop())
+ cmdPosition = hjoint->GetLowStop();
- positionError = ModNPi2Pi(cmdPosition - hjoint->GetAngle());
- speedError = cmdSpeed - hjoint->GetAngleRate();
+ positionError = ModNPi2Pi(cmdPosition - hjoint->GetAngle());
+ speedError = cmdSpeed - hjoint->GetAngleRate();
+ currentCmd = this->pids[i]->UpdatePid(positionError,currentTime);
-// printf("ROTARY:: pErr: %f, cmd: %f, actual: %f, pGain: %f, dGain: %f, sT: %f\n",positionError,cmdPosition, hjoint->GetAngle(),this->myIface->data->actuators[i].pGain,this->myIface->data->actuators[i].dGain,this->myIface->data->actuators[i].saturationTorque);
- if (fabs(positionError) > 0.01)
- {
- hjoint->SetParam( dParamVel, this->myIface->data->actuators[i].pGain * positionError + this->myIface->data->actuators[i].dGain * speedError);
- hjoint->SetParam( dParamFMax,this->myIface->data->actuators[i].saturationTorque );
- }
- break;
- case PR2::SPEED_CONTROL:
- hjoint->SetParam( dParamVel, cmdSpeed);
- hjoint->SetParam( dParamFMax, this->myIface->data->actuators[i].saturationTorque );
- break;
- default:
- break;
- }
- this->myIface->data->actuators[i].actualPosition = hjoint->GetAngle();
- this->myIface->data->actuators[i].actualSpeed = hjoint->GetAngleRate();
- this->myIface->data->actuators[i].actualEffectorForce = 0.0; //this must be modified using the JointFeedback struct at some point
- }
-
+ //if (fabs(positionError) > 0.01)
+ //{
+ hjoint->SetParam( dParamVel, currentCmd );
+ hjoint->SetParam( dParamFMax,this->myIface->data->actuators[i].saturationTorque );
+ //}
+ break;
+ case PR2::SPEED_CONTROL:
+ hjoint->SetParam( dParamVel, cmdSpeed);
+ hjoint->SetParam( dParamFMax, this->myIface->data->actuators[i].saturationTorque );
+ break;
+ default:
+ break;
+ }
+ this->myIface->data->actuators[i].actualPosition = hjoint->GetAngle();
+ this->myIface->data->actuators[i].actualSpeed = hjoint->GetAngleRate();
+ this->myIface->data->actuators[i].actualEffectorForce = 0.0; //this must be modified using the JointFeedback struct at some point
+ break;
+ case Joint::HINGE2:
+ case Joint::BALL:
+ case Joint::UNIVERSAL:
+ break;
+ }
}
this->myIface->data->new_cmd = 0;
@@ -240,7 +323,10 @@
}
////////////////////////////////////////////////////////////////////////////////
+//
// Finalize the controller
+//
+////////////////////////////////////////////////////////////////////////////////
void Pr2_Actarray::FiniChild()
{
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-06-17 20:47:05
|
Revision: 834
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=834&view=rev
Author: jleibs
Date: 2008-06-17 13:47:07 -0700 (Tue, 17 Jun 2008)
Log Message:
-----------
Axis_cam takes a ride on the rosbus, cv_bridge modified with options for optionally correcting colorspace and maxdepth issues.
Modified Paths:
--------------
pkg/trunk/drivers/cam/axis_cam/manifest.xml
pkg/trunk/drivers/cam/axis_cam/src/Makefile
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/Makefile
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/util/image_utils/include/image_utils/cv_bridge.h
pkg/trunk/util/image_utils/include/image_utils/jpeg_wrapper.h
pkg/trunk/vision/cv_view/cv_view.cpp
Modified: pkg/trunk/drivers/cam/axis_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-06-17 18:48:53 UTC (rev 833)
+++ pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-06-17 20:47:07 UTC (rev 834)
@@ -11,12 +11,12 @@
<author>Morgan Quigley (email: mqu...@cs...)</author>
<license>BSD</license>
<url>http://stair.stanford.edu</url>
+<depend package="roscpp"/>
<depend package="std_msgs"/>
-<!--<depend package="image_viewer"/>-->
<depend package="rosthread"/>
<depend package="string_utils"/>
<export>
- <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -laxis_cam"/>
+ <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -laxis_cam -lcurl"/>
</export>
</package>
Modified: pkg/trunk/drivers/cam/axis_cam/src/Makefile
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/Makefile 2008-06-17 18:48:53 UTC (rev 833)
+++ pkg/trunk/drivers/cam/axis_cam/src/Makefile 2008-06-17 20:47:07 UTC (rev 834)
@@ -1,3 +1,3 @@
-SUBDIRS = libaxis_cam
+SUBDIRS = axis_cam libaxis_cam
include $(shell rospack find mk)/recurse_subdirs.mk
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_cam/Makefile
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_cam/Makefile 2008-06-17 18:48:53 UTC (rev 833)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_cam/Makefile 2008-06-17 20:47:07 UTC (rev 834)
@@ -1,4 +1,4 @@
SRC = axis_cam.cpp
OUT = ../../nodes/axis_cam
PKG = axis_cam
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
+include $(shell rospack find mk)/node.mk
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-06-17 18:48:53 UTC (rev 833)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-06-17 20:47:07 UTC (rev 834)
@@ -27,68 +27,75 @@
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
-#include "ros/ros_slave.h"
-#include "common_flows/FlowImage.h"
+#include "ros/node.h"
+#include "std_msgs/Image.h"
+
#include "axis_cam/axis_cam.h"
-class AxisCamNode : public ROS_Slave
+class Axis_cam_node : public ros::node
{
public:
- FlowImage *image;
+ std_msgs::Image image;
+
string axis_host;
AxisCam *cam;
int frame_id;
- AxisCamNode() : ROS_Slave(), cam(NULL), frame_id(0)
+ Axis_cam_node() : ros::node("axis_ptz"), cam(NULL), frame_id(0)
{
- register_source(image = new FlowImage("image"));
- register_with_master();
- if (!get_string_param(".host", axis_host))
- {
- printf("host parameter not specified; defaulting to 192.168.0.90\n");
- axis_host = "192.168.0.90";
- }
+ advertise<std_msgs::Image>("image");
+
+ param("host", axis_host, string("192.168.0.90"));
printf("axis_cam host set to [%s]\n", axis_host.c_str());
- get_int_param(".frame_id", &frame_id);
+
cam = new AxisCam(axis_host);
- printf("package path is [%s]\n", get_my_package_path().c_str());
}
- virtual ~AxisCamNode()
+
+ virtual ~Axis_cam_node()
{
if (cam)
delete cam;
}
+
bool take_and_send_image()
{
uint8_t *jpeg;
uint32_t jpeg_size;
+
if (!cam->get_jpeg(&jpeg, &jpeg_size))
{
- log(ROS::ERROR, "woah! AxisCam::get_jpeg returned an error");
+ log(ros::ERROR, "woah! AxisCam::get_jpeg returned an error");
return false;
}
- //printf("sending %d-byte jpeg block\n", jpeg_size);
- image->set_data_size(jpeg_size);
- memcpy(image->data, jpeg, jpeg_size);
- image->width = 704;
- image->height = 480;
- image->compression = "jpeg";
- image->colorspace = "rgb24";
- image->frame_id = frame_id;
- image->publish();
+
+ image.set_data_size(jpeg_size);
+ memcpy(image.data, jpeg, jpeg_size);
+
+ //TODO: Read width and height from somewhere else
+ image.width = 704;
+ image.height = 480;
+ image.compression = "jpeg";
+ image.colorspace = "rgb24";
+
+ publish("image", image);
+
return true;
}
};
int main(int argc, char **argv)
{
- AxisCamNode a;
- while (a.happy())
+ ros::init(argc, argv);
+
+ Axis_cam_node a;
+ while (a.ok())
if (!a.take_and_send_image())
{
- a.log(ROS::ERROR,"couldn't take image.");
+ a.log(ros::ERROR,"couldn't take image.");
break;
}
+
+ ros::fini();
return 0;
}
Modified: pkg/trunk/util/image_utils/include/image_utils/cv_bridge.h
===================================================================
--- pkg/trunk/util/image_utils/include/image_utils/cv_bridge.h 2008-06-17 18:48:53 UTC (rev 833)
+++ pkg/trunk/util/image_utils/include/image_utils/cv_bridge.h 2008-06-17 20:47:07 UTC (rev 834)
@@ -35,12 +35,16 @@
template <class T>
class CvBridge : public ImageCodec<T>
{
+ uint32_t options;
+
public:
- CvBridge(T *msg) :
- ImageCodec<T>(msg)
- {
- }
+ static const uint32_t CORRECT_BGR = 0x0001;
+ static const uint32_t MAXDEPTH_8U = 0x0002;
+ CvBridge(T *msg, uint32_t _options = 0) :
+ ImageCodec<T>(msg),
+ options(_options) { }
+
bool from_cv(IplImage *cv_image, int compression_quality = 90)
{
this->msg->width = cv_image->width;
@@ -77,11 +81,14 @@
int channels = 1;
int depth = IPL_DEPTH_8U;
int elem_size = 1;
+ bool needs_swap_rb = false;
+ bool needs_depth_reduced = false;
if (this->msg->colorspace == "rgb24") {
channels = 3;
elem_size = 1;
depth = IPL_DEPTH_8U;
+ needs_swap_rb = true;
} else if (this->msg->colorspace == "mono8") {
channels = 1;
elem_size = 1;
@@ -90,14 +97,30 @@
channels = 1;
elem_size = 2;
depth = IPL_DEPTH_16U;
+ needs_depth_reduced = true;
}
*cv_image = cvCreateImage(cvSize(this->msg->width, this->msg->height),
depth, channels);
+
// todo: ensure row alignment is ok (copy in one scanline at a time)
for (size_t row = 0; row < this->msg->height; row++)
memcpy((*cv_image)->imageData + row * (*cv_image)->widthStep,
this->raster + row * this->msg->width * channels * elem_size, this->msg->width*channels * elem_size);
+
+ if ((options & CORRECT_BGR) && needs_swap_rb)
+ cvCvtColor(*cv_image, *cv_image, CV_RGB2BGR);
+
+ if ((options & MAXDEPTH_8U) && needs_depth_reduced)
+ {
+ IplImage* cv_image_sc = cvCreateImage(cvSize(this->msg->width, this->msg->height),
+ IPL_DEPTH_8U, channels);
+ cvCvtScale(*cv_image, cv_image_sc, 0.0625, 0);
+ cvReleaseImage(cv_image);
+ *cv_image = cv_image_sc;
+ }
+
+
return true;
}
};
Modified: pkg/trunk/util/image_utils/include/image_utils/jpeg_wrapper.h
===================================================================
--- pkg/trunk/util/image_utils/include/image_utils/jpeg_wrapper.h 2008-06-17 18:48:53 UTC (rev 833)
+++ pkg/trunk/util/image_utils/include/image_utils/jpeg_wrapper.h 2008-06-17 20:47:07 UTC (rev 834)
@@ -64,7 +64,7 @@
if (dinfo.out_color_space == JCS_GRAYSCALE) {
raster_type = RT_MONO8;
}
- else if (dinfo.out_color_space == JCS_GRAYSCALE)
+ else if (dinfo.out_color_space == JCS_RGB)
{
raster_type = RT_RGB24;
}
Modified: pkg/trunk/vision/cv_view/cv_view.cpp
===================================================================
--- pkg/trunk/vision/cv_view/cv_view.cpp 2008-06-17 18:48:53 UTC (rev 833)
+++ pkg/trunk/vision/cv_view/cv_view.cpp 2008-06-17 20:47:07 UTC (rev 834)
@@ -20,13 +20,13 @@
ros::thread::mutex cv_mutex;
IplImage *cv_image;
- IplImage *cv_image_sc;
char dir_name[256];
int img_cnt;
bool made_dir;
- CvView() : node("cv_view"), cv_bridge(&image_msg), cv_image(0), cv_image_sc(0), img_cnt(0), made_dir(false)
+ CvView() : node("cv_view"), cv_bridge(&image_msg, CvBridge<std_msgs::Image>::CORRECT_BGR | CvBridge<std_msgs::Image>::MAXDEPTH_8U),
+ cv_image(0), img_cnt(0), made_dir(false)
{
cvNamedWindow("cv_view", CV_WINDOW_AUTOSIZE);
subscribe("image", image_msg, &CvView::image_cb, 1);
@@ -43,30 +43,19 @@
~CvView()
{
- free_images();
- }
-
- void free_images()
- {
if (cv_image)
cvReleaseImage(&cv_image);
- if (cv_image_sc)
- cvReleaseImage(&cv_image_sc);
}
void image_cb()
{
cv_mutex.lock();
- free_images();
+ if (cv_image)
+ cvReleaseImage(&cv_image);
+
if (cv_bridge.to_cv(&cv_image))
{
- cv_image_sc = cvCreateImage(cvSize(cv_image->width, cv_image->height),
- IPL_DEPTH_8U, 1);
- if (cv_image->depth == IPL_DEPTH_16U)
- cvCvtScale(cv_image, cv_image_sc, 0.0625, 0);
- else
- cvCvtScale(cv_image, cv_image_sc, 1, 0);
- cvShowImage("cv_view", cv_image_sc);
+ cvShowImage("cv_view", cv_image);
}
cv_mutex.unlock();
}
@@ -93,7 +82,7 @@
}
std::ostringstream oss;
oss << dir_name << "/Img" << img_cnt++ << ".png";
- cvSaveImage(oss.str().c_str(), cv_image_sc);
+ cvSaveImage(oss.str().c_str(), cv_image);
}
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-06-17 22:08:00
|
Revision: 836
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=836&view=rev
Author: jleibs
Date: 2008-06-17 15:08:05 -0700 (Tue, 17 Jun 2008)
Log Message:
-----------
Allow for population of width/height/colorspace of jpeg images without needing to decompress the full image.
Modified Paths:
--------------
pkg/trunk/drivers/cam/axis_cam/manifest.xml
pkg/trunk/drivers/cam/axis_cam/src/Makefile
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/util/image_utils/include/image_utils/image_codec.h
pkg/trunk/util/image_utils/include/image_utils/jpeg_wrapper.h
Modified: pkg/trunk/drivers/cam/axis_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-06-17 22:00:23 UTC (rev 835)
+++ pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-06-17 22:08:05 UTC (rev 836)
@@ -15,6 +15,7 @@
<depend package="std_msgs"/>
<depend package="rosthread"/>
<depend package="string_utils"/>
+<depend package="image_utils"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -laxis_cam -lcurl"/>
</export>
Modified: pkg/trunk/drivers/cam/axis_cam/src/Makefile
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/Makefile 2008-06-17 22:00:23 UTC (rev 835)
+++ pkg/trunk/drivers/cam/axis_cam/src/Makefile 2008-06-17 22:08:05 UTC (rev 836)
@@ -1,3 +1,3 @@
-SUBDIRS = axis_cam libaxis_cam
+SUBDIRS = libaxis_cam axis_cam
include $(shell rospack find mk)/recurse_subdirs.mk
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-06-17 22:00:23 UTC (rev 835)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-06-17 22:08:05 UTC (rev 836)
@@ -29,6 +29,7 @@
#include "ros/node.h"
#include "std_msgs/Image.h"
+#include "image_utils/image_codec.h"
#include "axis_cam/axis_cam.h"
@@ -36,12 +37,13 @@
{
public:
std_msgs::Image image;
+ ImageCodec<std_msgs::Image> codec;
string axis_host;
AxisCam *cam;
int frame_id;
- Axis_cam_node() : ros::node("axis_ptz"), cam(NULL), frame_id(0)
+ Axis_cam_node() : ros::node("axis_ptz"), codec(&image), cam(NULL), frame_id(0)
{
advertise<std_msgs::Image>("image");
@@ -71,12 +73,9 @@
image.set_data_size(jpeg_size);
memcpy(image.data, jpeg, jpeg_size);
- //TODO: Read width and height from somewhere else
- image.width = 704;
- image.height = 480;
image.compression = "jpeg";
- image.colorspace = "rgb24";
+ codec.inflate_header();
publish("image", image);
return true;
Modified: pkg/trunk/util/image_utils/include/image_utils/image_codec.h
===================================================================
--- pkg/trunk/util/image_utils/include/image_utils/image_codec.h 2008-06-17 22:00:23 UTC (rev 835)
+++ pkg/trunk/util/image_utils/include/image_utils/image_codec.h 2008-06-17 22:08:05 UTC (rev 836)
@@ -83,6 +83,24 @@
return raster;
}
+ bool inflate_header()
+ {
+ msg->lock();
+
+ // perform decompression and/or colorspace conversion if necessary
+ if (msg->compression == string("jpeg"))
+ decompress_jpeg_header();
+ else
+ {
+ printf("unimplemented image header compression: [%s]\n",
+ msg->compression.c_str());
+ msg->unlock();
+ return false;
+ }
+ msg->unlock();
+ return true;
+ }
+
uint32_t get_raster_size()
{
int bpp = 0;
@@ -92,6 +110,10 @@
bpp = 1;
else if (msg->colorspace == "mono16")
bpp = 2;
+ else {
+ printf("Undefined colorspace [%s]. Defaulting to 3 byte per pixel.\n", msg->colorspace.c_str());
+ bpp = 3;
+ }
return msg->width * msg->height * bpp;
}
@@ -162,6 +184,21 @@
memcpy(raster, jpeg->get_raster(), jpeg->raster_size());
return true;
}
+
+ bool decompress_jpeg_header()
+ {
+ if (!jpeg)
+ jpeg = new JpegWrapper();
+ if (!jpeg->decompress_jpeg_header((char *)msg->data, msg->get_data_size()))
+ return false;
+
+ msg->width = jpeg->width();
+ msg->height = jpeg->height();
+ msg->compression = "jpeg";
+ msg->colorspace = jpeg->color_space();
+
+ return true;
+ }
bool decompress_raw()
{
Modified: pkg/trunk/util/image_utils/include/image_utils/jpeg_wrapper.h
===================================================================
--- pkg/trunk/util/image_utils/include/image_utils/jpeg_wrapper.h 2008-06-17 22:00:23 UTC (rev 835)
+++ pkg/trunk/util/image_utils/include/image_utils/jpeg_wrapper.h 2008-06-17 22:08:05 UTC (rev 836)
@@ -9,6 +9,7 @@
}
#include <cstdlib>
#include <cstring>
+#include <string>
class JpegWrapper
{
@@ -44,6 +45,18 @@
inline int width() { return raster_width; }
inline int height() { return raster_height; }
+
+ std::string color_space()
+ {
+ if (raster_type == RT_MONO8)
+ return std::string("mono8");
+ else if (raster_type == RT_RGB24)
+ return std::string("rgb24");
+ else
+ return std::string("unknown");
+ }
+
+
inline int raster_size()
{
if (raster_type == RT_RGB24)
@@ -53,6 +66,7 @@
else
return 0;
}
+
inline uint8_t *get_raster() { return raster; }
bool decompress_jpeg_buf(char *data, uint32_t data_len)
@@ -86,6 +100,31 @@
ros_jpeg_mutex_unlock();
return true;
}
+
+ bool decompress_jpeg_header(char *data, uint32_t data_len)
+ {
+ ros_jpeg_mutex_lock();
+ jpeg_buffer_src(&dinfo, data, data_len);
+ jpeg_read_header(&dinfo, TRUE);
+ jpeg_start_decompress(&dinfo);
+ if (dinfo.out_color_space == JCS_GRAYSCALE) {
+ raster_type = RT_MONO8;
+ }
+ else if (dinfo.out_color_space == JCS_RGB)
+ {
+ raster_type = RT_RGB24;
+ }
+ else {
+ printf("woah! this jpeg buffer uses an unexpected color space.\n");
+ return false;
+ }
+ raster_width = dinfo.output_width;
+ raster_height = dinfo.output_height;
+ jpeg_abort_decompress(&dinfo);
+ ros_jpeg_mutex_unlock();
+ return true;
+ }
+
uint32_t compress_to_jpeg(uint8_t *raster, uint32_t w, uint32_t h,
raster_type_t raster_type = RT_RGB24,
int32_t quality = 95)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <adv...@us...> - 2008-06-17 23:54:27
|
Revision: 839
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=839&view=rev
Author: advaitjain
Date: 2008-06-17 16:54:34 -0700 (Tue, 17 Jun 2008)
Log Message:
-----------
demo of picking an object in rosgazebo.
cannot pick cylinders, but cuboids are ok.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp
pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp
Added Paths:
-----------
pkg/trunk/simulators/rosgazebo/world/pickobj.world
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp 2008-06-17 23:12:09 UTC (rev 838)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_kin_controllers.cpp 2008-06-17 23:54:34 UTC (rev 839)
@@ -94,20 +94,20 @@
void object_pose()
{
Rotation r = Rotation::RotZ(DTOR(90));
- Vector v(0.568,0.01,0.06);
+ Vector v(0.578,0.01,0.06);
myKinCon.go(v,r,0.05);
}
void go_down()
{
Rotation r = Rotation::RotZ(DTOR(90))*Rotation::RotY(DTOR(30));
- Vector v(0.568,0.01,0.01);
+ Vector v(0.578,0.01,-0.01);
myKinCon.go(v,r,0.05);
}
void close_gripper()
{
- myKinCon.myPR2->CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.02, 10000);
+ myKinCon.myPR2->CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.0, 10000);
}
void open_gripper()
Added: pkg/trunk/simulators/rosgazebo/world/pickobj.world
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/pickobj.world (rev 0)
+++ pkg/trunk/simulators/rosgazebo/world/pickobj.world 2008-06-17 23:54:34 UTC (rev 839)
@@ -0,0 +1,450 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
+
+ <verbosity>5</verbosity>
+
+<!-- cfm is 1e-5 for single precision -->
+<!-- erp is typically .1-.8 -->
+ <physics:ode>
+ <stepTime>0.01</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.001</cfm>
+ <erp>0.8</erp>
+ <mu1>100</mu1>
+ </physics:ode>
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <size>1024 800</size>
+ <pos>0 0</pos>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>off</grid>
+ <desiredFPS>100</desiredFPS>
+ </rendering:ogre>
+
+
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <!-- map3.png -->
+ <material>PR2/floor_texture</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+<!--
+-->
+
+ <!-- The camera -->
+ <model:physical name="cam1_model">
+ <xyz> 2.0 0.25 1.17</xyz>
+ <rpy> 5.5 25.71 -152.72</rpy>
+ <static>true</static>
+
+ <body:empty name="cam1_body">
+ <sensor:camera name="cam1_sensor">
+ <imageSize>1024 800</imageSize>
+ <hfov>90</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>20</farClip>
+ <saveFrames>false</saveFrames>
+ <saveFramePath>frames</saveFramePath>
+ <controller:generic_camera name="cam1_controller">
+ <interface:camera name="cam1_iface_0" />
+ </controller:generic_camera>
+ </sensor:camera>
+ </body:empty>
+ </model:physical>
+
+
+ <!-- The "desk"-->
+ <model:physical name="desk1_model">
+ <xyz> 0.98 -0.21 -0.10</xyz>
+ <rpy> 0.0 0.0 0.00</rpy>
+ <body:box name="desk1_legs_body">
+ <geom:box name="desk1_legs_geom">
+ <xyz> 0.0 0.0 0.50</xyz>
+ <mesh>default</mesh>
+ <size>0.5 1.0 0.75</size>
+ <mass> 10.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.5 1.0 0.75</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ <geom:box name="desk1_top_geom">
+ <xyz> 0.0 0.0 0.90</xyz>
+ <mesh>default</mesh>
+ <size>0.75 1.5 0.05</size>
+ <mass> 10.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.75 1.5 0.05</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The small cuboidal "cup" -->
+ <model:physical name="cylinder1_model">
+ <xyz> 0.78 0.0 0.9</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:box name="cylinder1_body">
+ <geom:box name="cylinder1_geom">
+ <mesh>default</mesh>
+ <size>0.05 0.05 0.15</size>
+ <mass> 0.5</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.05 0.05 0.15</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The small cylindrical "cup" -->
+ <!--
+ <model:physical name="cylinder1_model">
+ <xyz> 0.78 0.0 0.9</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:cylinder name="cylinder1_body">
+ <geom:cylinder name="cylinder1_geom">
+ <mesh>default</mesh>
+ <size>0.05 0.15</size>
+ <mass> 0.5</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.05 0.05 0.15</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_cylinder</mesh>
+ </visual>
+ </geom:cylinder>
+ </body:cylinder>
+ </model:physical>
+ -->
+
+
+ <model:physical name="robot_model1">
+ <xyz>0.0 0.0 0.262</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <!-- base, torso and arms -->
+ <include embedded="true">
+ <xi:include href="pr2.model" />
+ </include>
+
+ <controller:pr2_actarray name="pr2_head_controller" plugin="libPr2_Actarray.so">
+ <joint name="neck_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="head_tilt_joint">
+ <saturationTorque>10</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="hokuyo_pitch_joint">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="head_cam_left_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="head_cam_left_pitch_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="head_cam_right_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="head_cam_right_pitch_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <interface:pr2array name="pr2_head_iface"/>
+ </controller:pr2_actarray>
+
+ <!-- use actarray for the rest of the model -->
+
+ <controller:pr2_actarray name="pr2_controller" plugin="libPr2_Actarray.so">
+ <joint name="front_left_caster_steer">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="front_left_wheel_l_drive">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="front_left_wheel_r_drive">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="front_right_caster_steer">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="front_right_wheel_l_drive">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="front_right_wheel_r_drive">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="rear_left_caster_steer">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="rear_left_wheel_l_drive">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="rear_left_wheel_r_drive">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="rear_right_caster_steer">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="rear_right_wheel_l_drive">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="rear_right_wheel_r_drive">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <!-- ========================================= -->
+ <joint name="torso_spine_slider">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <!-- ========================================= -->
+ <joint name="shoulder_pan_left">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="upperarm_pitch_joint_left">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="upperarm_roll_joint_left">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="elbow_pitch_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="forearm_roll_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="wrist_pitch_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="palm_roll_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <pGain>1</pGain>
+ </joint>
+ <!-- ========================================= -->
+ <joint name="shoulder_pan_right">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="upperarm_pitch_joint_right">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="upperarm_roll_joint_right">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="elbow_pitch_joint_right">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="forearm_roll_joint_right">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="wrist_pitch_joint_right">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <joint name="palm_roll_joint_right">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ </joint>
+ <!-- ========================================= -->
+ <interface:pr2array name="pr2_iface"/>
+ <!-- ========================================= -->
+ </controller:pr2_actarray>
+
+ <controller:pr2_gripper name="pr2_gripper_left_controller" plugin="libPr2_Gripper.so">
+ <leftJoint>finger_2_slider_left</leftJoint>
+ <rightJoint>finger_1_slider_left</rightJoint>
+ <gripperForce>100.0</gripperForce>
+ <pGain>1.0</pGain>
+ <dGain>0.0</dGain>
+ <iGain>0.0</iGain>
+ <interface:pr2gripper name="pr2_gripper_left_iface"/>
+ </controller:pr2_gripper>
+
+ <controller:pr2_gripper name="pr2_gripper_right_controller" plugin="libPr2_Gripper.so">
+ <leftJoint>finger_2_slider_right</leftJoint>
+ <rightJoint>finger_1_slider_right</rightJoint>
+ <gripperForce>100.0</gripperForce>
+ <pGain>1.0</pGain>
+ <dGain>0.0</dGain>
+ <iGain>0.0</iGain>
+ <interface:pr2gripper name="pr2_gripper_right_iface"/>
+ </controller:pr2_gripper>
+
+ <controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
+ <bodyName>palm_right</bodyName>
+ <interface:position name="p3d_right_wrist_position"/>
+ </controller:P3D>
+
+ <controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
+ <bodyName>palm_left</bodyName>
+ <interface:position name="p3d_left_wrist_position"/>
+ </controller:P3D>
+
+ <controller:P3D name="p3d_base_controller" plugin="libP3D.so">
+ <updateRate>100.0</updateRate>
+ <bodyName>base_body</bodyName>
+ <interface:position name="p3d_base_position"/>
+ </controller:P3D>
+
+ </model:physical>
+
+
+ <!-- White Directional light -->
+ <model:renderable name="directional_white">
+ <light>
+ <type>directional</type>
+ <direction>0 -0.5 -0.5</direction>
+ <diffuseColor>0.4 0.4 0.4</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
+ <attenuation>1 0.0 1.0 0.4</attenuation>
+ </light>
+ </model:renderable>
+
+
+</gazebo:world>
Modified: pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-06-17 23:12:09 UTC (rev 838)
+++ pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-06-17 23:54:34 UTC (rev 839)
@@ -78,7 +78,7 @@
for(int i=0;i<nJnts;i++)
{
q(i) = angle_within_mod180(q(i));
- printf(".. %f ..", q(i));
+// printf(".. %f ..", q(i));
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-06-18 00:58:40
|
Revision: 840
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=840&view=rev
Author: jleibs
Date: 2008-06-17 17:58:47 -0700 (Tue, 17 Jun 2008)
Log Message:
-----------
Added polled_images as a service, and a polled cv_viewer with which to test it.
Modified Paths:
--------------
pkg/trunk/drivers/cam/axis_cam/manifest.xml
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/vision/cv_view/manifest.xml
pkg/trunk/vision/cv_view/minimal/cv_view.cpp
Added Paths:
-----------
pkg/trunk/vision/cv_view/service/
pkg/trunk/vision/cv_view/service/Makefile
pkg/trunk/vision/cv_view/service/cv_view.cpp
Modified: pkg/trunk/drivers/cam/axis_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-06-17 23:54:34 UTC (rev 839)
+++ pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-06-18 00:58:47 UTC (rev 840)
@@ -13,6 +13,7 @@
<url>http://stair.stanford.edu</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
+<depend package="std_srvs"/>
<depend package="rosthread"/>
<depend package="string_utils"/>
<depend package="image_utils"/>
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-06-17 23:54:34 UTC (rev 839)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-06-18 00:58:47 UTC (rev 840)
@@ -29,6 +29,7 @@
#include "ros/node.h"
#include "std_msgs/Image.h"
+#include "std_srvs/PolledImage.h"
#include "image_utils/image_codec.h"
#include "axis_cam/axis_cam.h"
@@ -46,6 +47,7 @@
Axis_cam_node() : ros::node("axis_ptz"), codec(&image), cam(NULL), frame_id(0)
{
advertise<std_msgs::Image>("image");
+ advertise_service("polled_image", &Axis_cam_node::polled_image_cb);
param("host", axis_host, string("192.168.0.90"));
printf("axis_cam host set to [%s]\n", axis_host.c_str());
@@ -59,6 +61,15 @@
delete cam;
}
+ bool polled_image_cb(std_srvs::PolledImage::request &req,
+ std_srvs::PolledImage::response &res )
+ {
+ image.lock();
+ res.image = image;
+ image.unlock();
+ return true;
+ }
+
bool take_and_send_image()
{
uint8_t *jpeg;
@@ -76,6 +87,7 @@
image.compression = "jpeg";
codec.inflate_header();
+
publish("image", image);
return true;
@@ -97,4 +109,3 @@
ros::fini();
return 0;
}
-
Modified: pkg/trunk/vision/cv_view/manifest.xml
===================================================================
--- pkg/trunk/vision/cv_view/manifest.xml 2008-06-17 23:54:34 UTC (rev 839)
+++ pkg/trunk/vision/cv_view/manifest.xml 2008-06-18 00:58:47 UTC (rev 840)
@@ -7,6 +7,7 @@
<license>BSD</license>
<depend package="opencv"/>
<depend package="std_msgs"/>
+ <depend package="std_srvs"/>
<depend package="image_utils"/>
<depend package="roscpp"/>
</package>
Modified: pkg/trunk/vision/cv_view/minimal/cv_view.cpp
===================================================================
--- pkg/trunk/vision/cv_view/minimal/cv_view.cpp 2008-06-17 23:54:34 UTC (rev 839)
+++ pkg/trunk/vision/cv_view/minimal/cv_view.cpp 2008-06-18 00:58:47 UTC (rev 840)
@@ -16,7 +16,7 @@
std_msgs::Image image_msg;
CvBridge<std_msgs::Image> cv_bridge;
- CvView() : node("cv_view"), cv_bridge(&image_msg)
+ CvView() : node("cv_view"), cv_bridge(&image_msg, CvBridge<std_msgs::Image>::CORRECT_BGR)
{
cvNamedWindow("cv_view", CV_WINDOW_AUTOSIZE);
subscribe("image", image_msg, &CvView::image_cb);
Added: pkg/trunk/vision/cv_view/service/Makefile
===================================================================
--- pkg/trunk/vision/cv_view/service/Makefile (rev 0)
+++ pkg/trunk/vision/cv_view/service/Makefile 2008-06-18 00:58:47 UTC (rev 840)
@@ -0,0 +1,4 @@
+SRC = cv_view.cpp
+OUT = cv_view
+PKG = cv_view
+include $(shell rospack find mk)/node.mk
Added: pkg/trunk/vision/cv_view/service/cv_view.cpp
===================================================================
--- pkg/trunk/vision/cv_view/service/cv_view.cpp (rev 0)
+++ pkg/trunk/vision/cv_view/service/cv_view.cpp 2008-06-18 00:58:47 UTC (rev 840)
@@ -0,0 +1,73 @@
+#include <cstdio>
+#include <vector>
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+#include "ros/node.h"
+#include "std_msgs/Image.h"
+#include "std_srvs/PolledImage.h"
+#include "image_utils/cv_bridge.h"
+
+#include <sys/stat.h>
+
+using namespace std;
+
+class CvView : public ros::node
+{
+public:
+ CvBridge<std_msgs::Image> cv_bridge;
+
+ std_srvs::PolledImage::request req;
+ std_srvs::PolledImage::response res;
+
+ ros::thread::mutex cv_mutex;
+
+ IplImage *cv_image;
+
+ CvView() : node("cv_view"), cv_bridge(&(res.image), CvBridge<std_msgs::Image>::CORRECT_BGR | CvBridge<std_msgs::Image>::MAXDEPTH_8U),
+ cv_image(0)
+ {
+ cvNamedWindow("cv_view", CV_WINDOW_AUTOSIZE);
+ }
+
+ ~CvView()
+ {
+ if (cv_image)
+ cvReleaseImage(&cv_image);
+ }
+
+ void check_keys()
+ {
+ cv_mutex.lock();
+ if (cvWaitKey(3) == 10)
+ {
+ if (cv_image)
+ cvReleaseImage(&cv_image);
+
+ if (ros::service::call("polled_image", req, res))
+ {
+ if (cv_bridge.to_cv(&cv_image))
+ {
+ cvShowImage("cv_view", cv_image);
+ }
+ }
+ else
+ printf("error calling the polled_image service\n");
+ }
+ cv_mutex.unlock();
+ }
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ CvView view;
+ while (view.ok())
+ {
+ usleep(10000);
+ view.check_keys();
+ }
+ ros::fini();
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-19 21:42:58
|
Revision: 858
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=858&view=rev
Author: hsujohnhsu
Date: 2008-06-19 14:42:52 -0700 (Thu, 19 Jun 2008)
Log Message:
-----------
added some components for torque control.
partial updates to libpr2HW. compiled but not used yet.
add robot_force.world for torque_control.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/world/pr2.model
pkg/trunk/demos/2dnav-gazebo/world/robot.world
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
pkg/trunk/simulators/rosgazebo/setup.bash
pkg/trunk/simulators/rosgazebo/setup.tcsh
Added Paths:
-----------
pkg/trunk/demos/2dnav-gazebo/world/robot_force.world
Modified: pkg/trunk/demos/2dnav-gazebo/world/pr2.model
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-06-19 21:42:52 UTC (rev 858)
@@ -1203,7 +1203,7 @@
<body2>front_left_wheel_l</body2>
<anchor>front_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1212,7 +1212,7 @@
<body2>front_left_wheel_r</body2>
<anchor>front_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1221,7 +1221,7 @@
<body2>front_right_wheel_l</body2>
<anchor>front_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1230,7 +1230,7 @@
<body2>front_right_wheel_r</body2>
<anchor>front_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1239,7 +1239,7 @@
<body2>rear_left_wheel_l</body2>
<anchor>rear_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1248,7 +1248,7 @@
<body2>rear_left_wheel_r</body2>
<anchor>rear_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1257,7 +1257,7 @@
<body2>rear_right_wheel_l</body2>
<anchor>rear_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1266,7 +1266,7 @@
<body2>rear_right_wheel_r</body2>
<anchor>rear_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1278,7 +1278,7 @@
<body2>base_body</body2>
<anchor>front_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1287,7 +1287,7 @@
<body2>base_body</body2>
<anchor>front_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1296,7 +1296,7 @@
<body2>base_body</body2>
<anchor>rear_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1305,7 +1305,7 @@
<body2>base_body</body2>
<anchor>rear_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.01</cfm>
<erp>0.6</erp>
</joint:hinge>
Modified: pkg/trunk/demos/2dnav-gazebo/world/robot.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-06-19 17:19:53 UTC (rev 857)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-06-19 21:42:52 UTC (rev 858)
@@ -360,44 +360,51 @@
<joint name="neck_yaw_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_tilt_joint">
<saturationTorque>10</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="hokuyo_pitch_joint">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_cam_left_yaw_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_cam_left_pitch_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_cam_right_yaw_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="head_cam_right_pitch_joint">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<interface:pr2array name="pr2_head_iface"/>
</controller:pr2_actarray>
@@ -411,72 +418,84 @@
<pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_left_wheel_l_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_left_wheel_r_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_right_caster_steer">
<saturationTorque>1000</saturationTorque>
<pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_right_wheel_l_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="front_right_wheel_r_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_left_caster_steer">
<saturationTorque>1000</saturationTorque>
<pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_left_wheel_l_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_left_wheel_r_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_right_caster_steer">
<saturationTorque>1000</saturationTorque>
<pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_right_wheel_l_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="rear_right_wheel_r_drive">
<saturationTorque>5</saturationTorque>
<pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
<joint name="torso_spine_slider">
@@ -484,6 +503,7 @@
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
<joint name="shoulder_pan_left">
@@ -491,42 +511,49 @@
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="upperarm_pitch_joint_left">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="upperarm_roll_joint_left">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="elbow_pitch_joint_left">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="forearm_roll_joint_left">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="wrist_pitch_joint_left">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="palm_roll_joint_left">
<saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
- <pGain>1</pGain>
+ <iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
<joint name="shoulder_pan_right">
@@ -534,42 +561,49 @@
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="upperarm_pitch_joint_right">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="upperarm_roll_joint_right">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="elbow_pitch_joint_right">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="forearm_roll_joint_right">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="wrist_pitch_joint_right">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<joint name="palm_roll_joint_right">
<saturationTorque>100</saturationTorque>
<pGain>1</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
+ <iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
<interface:pr2array name="pr2_iface"/>
Added: pkg/trunk/demos/2dnav-gazebo/world/robot_force.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot_force.world (rev 0)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot_force.world 2008-06-19 21:42:52 UTC (rev 858)
@@ -0,0 +1,668 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
+
+ <verbosity>5</verbosity>
+
+<!-- cfm is 1e-5 for single precision -->
+<!-- erp is typically .1-.8 -->
+ <physics:ode>
+ <stepTime>0.01</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.001</cfm>
+ <erp>0.8</erp>
+ </physics:ode>
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <size>1024 800</size>
+ <pos>0 0</pos>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>off</grid>
+ <desiredFPS>10</desiredFPS>
+ </rendering:ogre>
+
+
+<!--
+ <model:physical name="gbuilding">
+ <body:heightmap name="building">
+ <geom:heightmap name="building">
+ <image>map.png</image>
+ <worldTexture>map2.jpg</worldTexture>
+ <size>2 2 0.5</size>
+ </geom:heightmap>
+ </body:heightmap>
+ </model:physical>
+-->
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <!-- map3.png -->
+ <material>PR2/floor_texture</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+<!--
+-->
+
+
+
+ <!-- The camera -->
+ <model:physical name="cam1_model">
+ <xyz> 3.0 0.0 1.0</xyz>
+ <rpy> 0.0 0.0 180.0</rpy>
+ <static>true</static>
+
+ <body:empty name="cam1_body">
+ <sensor:camera name="cam1_sensor">
+ <imageSize>1024 800</imageSize>
+ <hfov>90</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>20</farClip>
+ <saveFrames>false</saveFrames>
+ <saveFramePath>frames</saveFramePath>
+ <controller:generic_camera name="cam1_controller">
+ <updateRate>10.0</updateRate>
+ <interface:camera name="cam1_iface_0" />
+ </controller:generic_camera>
+ </sensor:camera>
+ </body:empty>
+ </model:physical>
+
+
+ <!-- test laser model -->
+ <!--
+ <model:physical name="test_laser_model">
+ <xyz> -2.0 0.0 0.15</xyz>
+ <rpy> 0.0 0.0 90.0</rpy>
+ <static>true</static>
+ <body:box name="test_laser_body">
+ <xyz>0.0 0.0 0.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <geom:box name="test_laser_box">
+ <xyz>0.0 0.0 -0.05</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.15 0.02</size>
+ <mass>0.0</mass>
+ <visual>
+ <scale>0.05 0.15 0.02</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
+ </geom:box>
+
+ <sensor:ray name="laser_1">
+ <rayCount>100</rayCount>
+ <rangeCount>100</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.10</origin>
+
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
+
+ <minRange>0.036</minRange>
+ <maxRange>10.0</maxRange>
+
+ <updateRate>10.0</updateRate>
+
+ <controller:sicklms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sicklms200_laser>
+ </sensor:ray>
+
+ </body:box>
+ </model:physical>
+ -->
+
+ <!-- The "desk"-->
+ <model:physical name="desk1_model">
+ <xyz> 2.28 -0.21 -0.10</xyz>
+ <rpy> 0.0 0.0 0.00</rpy>
+ <body:box name="desk1_legs_body">
+ <geom:box name="desk1_legs_geom">
+ <xyz> 0.0 0.0 0.50</xyz>
+ <mesh>default</mesh>
+ <size>0.5 1.0 0.75</size>
+ <mass> 10.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.5 1.0 0.75</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ <geom:box name="desk1_top_geom">
+ <xyz> 0.0 0.0 0.90</xyz>
+ <mesh>default</mesh>
+ <size>0.75 1.5 0.05</size>
+ <mass> 10.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.75 1.5 0.05</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The second "desk"-->
+ <model:physical name="desk2_model">
+ <xyz> 2.25 -3.0 -0.10</xyz>
+ <rpy> 0.0 0.0 0.00</rpy>
+ <body:box name="desk2_legs_body">
+ <geom:box name="desk2_legs_geom">
+ <xyz> 0.0 0.0 0.50</xyz>
+ <mesh>default</mesh>
+ <size>0.5 1.0 0.75</size>
+ <mass> 10.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.5 1.0 0.75</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ <geom:box name="desk2_top_geom">
+ <xyz> 0.0 0.0 0.90</xyz>
+ <mesh>default</mesh>
+ <size>0.75 1.5 0.05</size>
+ <mass> 10.0</mass>
+ <cfm>0.001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.75 1.5 0.05</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The small cylinder "cup" -->
+ <model:physical name="cylinder1_model">
+ <xyz> 2.5 0.0 0.9</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <cfm>0.001</cfm>
+ <erp>0.5</erp>
+ <body:cylinder name="cylinder1_body">
+ <geom:cylinder name="cylinder1_geom">
+ <mesh>default</mesh>
+ <size>0.025 0.075</size>
+ <mass> 0.05</mass>
+ <visual>
+ <size> 0.05 0.05 0.075</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_cylinder</mesh>
+ </visual>
+ </geom:cylinder>
+ <geom:box name="cylinder1_base_geom">
+ <mesh>default</mesh>
+ <xyz>0.0 0.0 -0.033</xyz>
+ <size>0.05 0.05 0.01</size>
+ <mass> 0.01</mass>
+ <visual>
+ <size> 0.05 0.05 0.01</size>
+ <material>Gazebo/Fish</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:cylinder>
+ </model:physical>
+
+
+ <!-- The small ball -->
+ <model:physical name="sphere1_model">
+ <xyz> 2.5 -2.8 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:sphere name="sphere1_body">
+ <geom:sphere name="sphere1_geom">
+ <mesh>default</mesh>
+ <size> 0.15</size>
+ <mass> 1.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 0.3 0.3 0.3</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_sphere</mesh>
+ </visual>
+ </geom:sphere>
+ </body:sphere>
+ </model:physical>
+
+ <!-- The large ball map3.png -->
+ <model:physical name="sphere2_model">
+ <xyz> 5.85 4.35 1.55</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:sphere name="sphere2_body">
+ <geom:sphere name="sphere2_geom">
+ <mesh>default</mesh>
+ <size> 1.0</size>
+ <mass> 1.0</mass>
+ <cfm>0.000001</cfm>
+ <erp>0.8</erp>
+ <visual>
+ <size> 2.0 2.0 2.0</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_sphere</mesh>
+ </visual>
+ </geom:sphere>
+ </body:sphere>
+ </model:physical>
+
+ <!-- The wall in front map3.png -->
+ <model:physical name="wall_2_model">
+ <xyz> 11.6 -1.55 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_2_body">
+ <geom:box name="wall_2_geom">
+ <mesh>default</mesh>
+ <size>2.1 32.8 2.0</size>
+ <visual>
+ <size>2.1 32.8 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The wall behind -->
+ <model:physical name="wall_1_model">
+ <xyz> -11.3 -1.45 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_1_body">
+ <geom:box name="wall_1_geom">
+ <mesh>default</mesh>
+ <size>0.4 24.0 2.0</size>
+ <visual>
+ <size>0.4 24.0 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The wall 3 -->
+ <model:physical name="wall_3_model">
+ <xyz> 6.7 8.05 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_3_body">
+ <geom:box name="wall_3_geom">
+ <mesh>default</mesh>
+ <size>7.5 1.2 2.0</size>
+ <visual>
+ <size>7.5 1.2 2.0</size>
+ <material>Gazebo/Chrome</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+
+ <model:physical name="robot_model1">
+ <xyz>0.0 0.0 0.262</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <!-- base, torso and arms -->
+ <include embedded="true">
+ <xi:include href="pr2.model" />
+ </include>
+
+ <controller:pr2_actarray name="pr2_head_controller" plugin="libPr2_Actarray.so">
+ <updateRate>100.0</updateRate>
+ <joint name="neck_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_tilt_joint">
+ <saturationTorque>10</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="hokuyo_pitch_joint">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_cam_left_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_cam_left_pitch_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_cam_right_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="head_cam_right_pitch_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <interface:pr2array name="pr2_head_iface"/>
+ </controller:pr2_actarray>
+
+ <!-- use actarray for the rest of the model -->
+
+ <controller:pr2_actarray name="pr2_controller" plugin="libPr2_Actarray.so">
+ <updateRate>100.0</updateRate>
+ <joint name="front_left_caster_steer">
+ <saturationTorque>3</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>.5</iGain>
+ <iClamp>3</iClamp>
+ </joint>
+ <joint name="front_left_wheel_l_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="front_left_wheel_r_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="front_right_caster_steer">
+ <saturationTorque>3</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>.5</iGain>
+ <iClamp>3</iClamp>
+ </joint>
+ <joint name="front_right_wheel_l_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="front_right_wheel_r_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="rear_left_caster_steer">
+ <saturationTorque>3</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>.1</iGain>
+ <iClamp>1</iClamp>
+ </joint>
+ <joint name="rear_left_wheel_l_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="rear_left_wheel_r_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="rear_right_caster_steer">
+ <saturationTorque>3</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>.1</iGain>
+ <iClamp>1</iClamp>
+ </joint>
+ <joint name="rear_right_wheel_l_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="rear_right_wheel_r_drive">
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <!-- ========================================= -->
+ <joint name="torso_spine_slider">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <!-- ========================================= -->
+ <joint name="shoulder_pan_left">
+ <saturationTorque>50000</saturationTorque>
+ <pGain>20000</pGain>
+ <dGain>0</dGain>
+ <iGain>300</iGain>
+ <iClamp>1000</iClamp>
+ </joint>
+ <joint name="upperarm_pitch_joint_left">
+ <saturationTorque>20000</saturationTorque>
+ <pGain>20000</pGain>
+ <dGain>0</dGain>
+ <iGain>100</iGain>
+ <iClamp>1000</iClamp>
+ </joint>
+ <joint name="upperarm_roll_joint_left">
+ <saturationTorque>10000</saturationTorque>
+ <pGain>500</pGain>
+ <dGain>0</dGain>
+ <iGain>10</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="elbow_pitch_joint_left">
+ <saturationTorque>1000</saturationTorque>
+ <pGain>500</pGain>
+ <dGain>0</dGain>
+ <iGain>10</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="forearm_roll_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <pGain>100</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="wrist_pitch_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <pGain>100</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</iClamp>
+ </joint>
+ <joint name="palm_roll_joint_left">
+ <saturationTorque>100</saturationTorque>
+ <pGain>100</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>100</...
[truncated message content] |
|
From: <is...@us...> - 2008-06-19 23:33:44
|
Revision: 859
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=859&view=rev
Author: isucan
Date: 2008-06-19 16:33:52 -0700 (Thu, 19 Jun 2008)
Log Message:
-----------
initial commit for a 3D map construction node
Added Paths:
-----------
pkg/trunk/mapping/
pkg/trunk/mapping/world_3d_map/
pkg/trunk/mapping/world_3d_map/Makefile
pkg/trunk/mapping/world_3d_map/bin/
pkg/trunk/mapping/world_3d_map/manifest.xml
pkg/trunk/mapping/world_3d_map/src/
pkg/trunk/mapping/world_3d_map/src/Makefile
pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
Added: pkg/trunk/mapping/world_3d_map/Makefile
===================================================================
--- pkg/trunk/mapping/world_3d_map/Makefile (rev 0)
+++ pkg/trunk/mapping/world_3d_map/Makefile 2008-06-19 23:33:52 UTC (rev 859)
@@ -0,0 +1,2 @@
+SUBDIRS = src
+include $(shell rospack find mk)/recurse_subdirs.mk
Added: pkg/trunk/mapping/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/world_3d_map/manifest.xml (rev 0)
+++ pkg/trunk/mapping/world_3d_map/manifest.xml 2008-06-19 23:33:52 UTC (rev 859)
@@ -0,0 +1,12 @@
+<package>
+<description>3D Map Construction</description>
+<author>Willow Garage</author>
+<license>BSD</license>
+<depend package="roscpp" />
+<depend package="std_msgs" />
+<depend package="std_srvs" />
+<depend package="xmlparam" />
+<export>
+ <cpp cflags="-I${prefix}/msg/cpp"/>
+</export>
+</package>
Added: pkg/trunk/mapping/world_3d_map/src/Makefile
===================================================================
--- pkg/trunk/mapping/world_3d_map/src/Makefile (rev 0)
+++ pkg/trunk/mapping/world_3d_map/src/Makefile 2008-06-19 23:33:52 UTC (rev 859)
@@ -0,0 +1,4 @@
+SRC = world_3d_map.cpp
+OUT = ../bin/world_3d_map
+PKG = world_3d_map
+include $(shell rospack find mk)/node.mk
Added: pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp (rev 0)
+++ pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp 2008-06-19 23:33:52 UTC (rev 859)
@@ -0,0 +1,84 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b world_3d_map is a node capable of building 3D maps out of point
+cloud data. The code is incomplete: it currently only forwards cloud
+data.
+
+**/
+
+#include "ros/node.h"
+#include "std_msgs/PointCloudFloat32.h"
+using namespace std_msgs;
+
+class World3DMap : public ros::node
+{
+public:
+
+ World3DMap(void) : ros::node("World3DMap")
+ {
+ subscribe("full_cloud", cloud, &World3DMap::pointCloudCallback);
+ advertise<PointCloudFloat32>("world_3d_map");
+ }
+
+ void pointCloudCallback(void)
+ {
+ printf("received %d points\n", cloud.pts_size);
+ // this should do something smarter here....
+ // build a 3D representation of the world
+ publish("world_3d_map", cloud);
+ }
+
+private:
+
+ PointCloudFloat32 cloud;
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ World3DMap map;
+ map.spin();
+ map.shutdown();
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-06-22 09:13:16
|
Revision: 887
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=887&view=rev
Author: isucan
Date: 2008-06-20 21:06:10 -0700 (Fri, 20 Jun 2008)
Log Message:
-----------
added MPK stub; actual package is missing (no right to distribute yet)
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
Added Paths:
-----------
pkg/trunk/3rdparty/MPK/
pkg/trunk/3rdparty/MPK/MPK
pkg/trunk/3rdparty/MPK/Makefile
pkg/trunk/3rdparty/MPK/manifest.xml
Added: pkg/trunk/3rdparty/MPK/MPK
===================================================================
--- pkg/trunk/3rdparty/MPK/MPK (rev 0)
+++ pkg/trunk/3rdparty/MPK/MPK 2008-06-21 04:06:10 UTC (rev 887)
@@ -0,0 +1 @@
+link /u/isucan/repos/scratch/MPK/
\ No newline at end of file
Property changes on: pkg/trunk/3rdparty/MPK/MPK
___________________________________________________________________
Name: svn:special
+ *
Added: pkg/trunk/3rdparty/MPK/Makefile
===================================================================
--- pkg/trunk/3rdparty/MPK/Makefile (rev 0)
+++ pkg/trunk/3rdparty/MPK/Makefile 2008-06-21 04:06:10 UTC (rev 887)
@@ -0,0 +1,2 @@
+SUBDIRS = MPK
+include $(shell rospack find mk)/recurse_subdirs.mk
Added: pkg/trunk/3rdparty/MPK/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/MPK/manifest.xml (rev 0)
+++ pkg/trunk/3rdparty/MPK/manifest.xml 2008-06-21 04:06:10 UTC (rev 887)
@@ -0,0 +1,17 @@
+<package>
+
+<description brief="Proximity Query Package">
+PQP (Proximity Query Package) is a library for performing three types
+of proximity queries on a pair of geometric models composed of
+triangles.
+</description>
+
+<author>Mitul Saha et al.</author>
+<license>LGPL</license>
+<url>http://robotics.stanford.edu/~mitul/mpk/</url>
+
+<export>
+ <cpp cflags="-I${prefix}/MPK -I${prefix}/MPK/basic -I${prefix}/MPK/gui -I${prefix}/MPK/pqp -I${prefix}/MPK/sbl -I${prefix}/MPK/robots" lflags="-L${prefix}/MPK/lib -lPQP -lbasic -lgui -lrob -lsbl"/>
+</export>
+
+</package>
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-06-21 03:36:42 UTC (rev 886)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-06-21 04:06:10 UTC (rev 887)
@@ -6,7 +6,7 @@
<depend package="std_msgs" />
<depend package="std_srvs" />
<depend package="xmlparam" />
-<depend package="mpk" />
+<depend package="MPK" />
<export>
<cpp cflags="-I${prefix}/msg/cpp"/>
</export>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-06-21 03:36:42 UTC (rev 886)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-06-21 04:06:10 UTC (rev 887)
@@ -48,6 +48,10 @@
#include "std_msgs/KinematicPath.h"
#include "std_srvs/KinematicMotionPlan.h"
+// HACK
+#include "rosplan.h"
+// END HACK
+
using namespace std_msgs;
using namespace std_srvs;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-06-22 09:38:10
|
Revision: 879
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=879&view=rev
Author: isucan
Date: 2008-06-20 17:29:25 -0700 (Fri, 20 Jun 2008)
Log Message:
-----------
initial kinematic motion planning stub
Added Paths:
-----------
pkg/trunk/motion_planning/
pkg/trunk/motion_planning/Makefile
pkg/trunk/motion_planning/kinematic_planning/
pkg/trunk/motion_planning/kinematic_planning/Makefile
pkg/trunk/motion_planning/kinematic_planning/bin/
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/
pkg/trunk/motion_planning/kinematic_planning/src/Makefile
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
Added: pkg/trunk/motion_planning/Makefile
===================================================================
--- pkg/trunk/motion_planning/Makefile (rev 0)
+++ pkg/trunk/motion_planning/Makefile 2008-06-21 00:29:25 UTC (rev 879)
@@ -0,0 +1,2 @@
+SUBDIRS = kinematic_planning
+include $(shell rospack find mk)/recurse_subdirs.mk
Added: pkg/trunk/motion_planning/kinematic_planning/Makefile
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/Makefile (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/Makefile 2008-06-21 00:29:25 UTC (rev 879)
@@ -0,0 +1,2 @@
+SUBDIRS = src
+include $(shell rospack find mk)/recurse_subdirs.mk
Added: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-06-21 00:29:25 UTC (rev 879)
@@ -0,0 +1,13 @@
+<package>
+<description>3D Map Construction</description>
+<author>Willow Garage</author>
+<license>BSD</license>
+<depend package="roscpp" />
+<depend package="std_msgs" />
+<depend package="std_srvs" />
+<depend package="xmlparam" />
+<depend package="mpk" />
+<export>
+ <cpp cflags="-I${prefix}/msg/cpp"/>
+</export>
+</package>
Added: pkg/trunk/motion_planning/kinematic_planning/src/Makefile
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/Makefile (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/src/Makefile 2008-06-21 00:29:25 UTC (rev 879)
@@ -0,0 +1,4 @@
+SRC = kinematic_planning.cpp
+OUT = ../bin/kinematic_planning
+PKG = kinematic_planning
+include $(shell rospack find mk)/node.mk
Added: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-06-21 00:29:25 UTC (rev 879)
@@ -0,0 +1,92 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b KinematicPlanning is a node capable of planning kinematic paths for
+a set of robot models.
+
+**/
+
+#include "ros/node.h"
+#include "std_msgs/PointCloudFloat32.h"
+#include "std_msgs/KinematicPath.h"
+#include "std_srvs/KinematicPlan.h"
+
+using namespace std_msgs;
+using namespace std_srvs;
+
+class KinematicPlanning : public ros::node
+{
+public:
+
+ KinematicPlanning(void) : ros::node("KinematicPlanning")
+ {
+ advertise_service<KinematicMotionPlan>("plan_kinematic_path", &KinematicPlanning::plan);
+
+ subscribe("world_3d_map", cloud, &KinematicPlanning::pointCloudCallback);
+
+ }
+
+ void pointCloudCallback(void)
+ {
+ printf("received %d points\n", cloud.pts_size);
+ }
+
+ bool plan(KinematicMotionPlan::request &req, KinematicMotionPlan::response &res)
+ {
+
+ }
+
+private:
+
+ PointCloudFloat32 cloud;
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ KinematicPlanning planner;
+ planner.spin();
+ planner.shutdown();
+
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-06-24 01:46:22
|
Revision: 911
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=911&view=rev
Author: isucan
Date: 2008-06-23 18:46:30 -0700 (Mon, 23 Jun 2008)
Log Message:
-----------
added documentation to my packages
Modified Paths:
--------------
pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/test_kinematic_planning/src/test_kinematic_planning.cpp
Modified: pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp 2008-06-24 00:40:09 UTC (rev 910)
+++ pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp 2008-06-24 01:46:30 UTC (rev 911)
@@ -38,10 +38,47 @@
@htmlinclude manifest.html
-@b World3dMap is a node capable of building 3D maps out of point
+@b world_3d_map is a node capable of building 3D maps out of point
cloud data. The code is incomplete: it currently only forwards cloud
data.
+<hr>
+
+@section usage Usage
+@verbatim
+$ world_3d_map [standard ROS args]
+@endverbatim
+
+@par Example
+
+@verbatim
+$ world_3d_map
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- full_cloud/PointCloudFloat32 : point cloud with data from a complete laser scan (top to bottom)
+
+Publishes to (name/type):
+- @b "world_3d_map"/PointCloudFloat32 : point cloud describing the 3D environment
+- @b "roserr"/Log : output log messages
+
+<hr>
+
+Uses (name/type):
+- None
+
+Provides (name/type):
+- None
+
+<hr>
+
+@section parameters ROS parameters
+- @b "world_3d_map/max_publish_frequency" : @b [double] the maximum frequency (Hz) at which the data in the built 3D map is to be sent (default 0.5)
+
**/
#include "ros/node.h"
@@ -53,20 +90,18 @@
using namespace std_msgs;
using namespace ros::thread::member_thread;
-static const char MAP_PUBLISH_TOPIC[] = "world_3d_map";
-
class World3DMap : public ros::node
{
public:
- World3DMap(void) : ros::node("World3DMap")
+ World3DMap(void) : ros::node("world_3d_map")
{
- advertise<PointCloudFloat32>(MAP_PUBLISH_TOPIC);
+ advertise<PointCloudFloat32>("world_3d_map");
advertise<Log>("roserr");
// subscribe to stereo vision point cloud as well... when it becomes available
subscribe("full_cloud", inputCloud, &World3DMap::pointCloudCallback);
- param((string(MAP_PUBLISH_TOPIC)+"/max_publish_frequency").c_str(), maxPublishFrequency, 0.5);
+ param("world_3d_map/max_publish_frequency", maxPublishFrequency, 0.5);
/* create a thread that does the processing of the input data.
* and one that handles the publishing of the data */
@@ -162,7 +197,7 @@
{
worldDataMutex.lock();
if (active)
- publish(MAP_PUBLISH_TOPIC, toProcess);
+ publish("world_3d_map", toProcess);
shouldPublish = false;
worldDataMutex.unlock();
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-06-24 00:40:09 UTC (rev 910)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-06-24 01:46:30 UTC (rev 911)
@@ -41,6 +41,44 @@
@b KinematicPlanning is a node capable of planning kinematic paths for
a set of robot models.
+<hr>
+
+@section usage Usage
+@verbatim
+$ kinematic_planning [standard ROS args]
+@endverbatim
+
+@par Example
+
+@verbatim
+$ kinematic_planning
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- world_3d_map/PointCloudFloat32 : point cloud with data describing the 3D environment
+
+Publishes to (name/type):
+- None
+
+<hr>
+
+@section services ROS services
+
+Uses (name/type):
+- None
+
+Provides (name/type):
+- @b "plan_kinematic_path"/KinematicMotionPlan : given a robot model, starting ang goal states, this service computes a collision free path
+
+<hr>
+
+@section parameters ROS parameters
+- None
+
**/
#include "ros/node.h"
@@ -59,7 +97,7 @@
{
public:
- KinematicPlanning(void) : ros::node("KinematicPlanning")
+ KinematicPlanning(void) : ros::node("kinematic_planning")
{
advertise_service("plan_kinematic_path", &KinematicPlanning::plan);
subscribe("world_3d_map", cloud, &KinematicPlanning::pointCloudCallback);
Modified: pkg/trunk/motion_planning/test_kinematic_planning/src/test_kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/test_kinematic_planning/src/test_kinematic_planning.cpp 2008-06-24 00:40:09 UTC (rev 910)
+++ pkg/trunk/motion_planning/test_kinematic_planning/src/test_kinematic_planning.cpp 2008-06-24 01:46:30 UTC (rev 911)
@@ -38,9 +38,49 @@
@htmlinclude manifest.html
-@b TestKinematicPlanning is a node that can be used to test a
+@b test_kinematic_planning is a node that can be used to test a
kinematic planning node.
+<hr>
+
+@section usage Usage
+@verbatim
+$ test_kinematic_planning [standard ROS args]
+@endverbatim
+
+@par Example
+
+@verbatim
+$ test_kinematic_planning
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- None
+
+Publishes to (name/type):
+- @b "cmd_leftarmconfig"/PR2Arm : issue joint angle commands to the left arm
+- @b "cmd_rightarmconfig"/PR2Arm : issue joint angle commands to the right arm
+
+<hr>
+
+@section services ROS services
+
+Uses (name/type):
+- @b "plan_kinematic_path"/KinematicMotionPlan : given a robot model, starting ang goal states, this service computes a collision free path
+
+Provides (name/type):
+- None
+
+<hr>
+
+@section parameters ROS parameters
+- None
+
+
**/
#include "ros/node.h"
@@ -57,7 +97,7 @@
{
public:
- TestKinematicPlanning(void) : ros::node("TestKinematicPlanning")
+ TestKinematicPlanning(void) : ros::node("test_kinematic_planning")
{
advertise<PR2Arm>("cmd_leftarmconfig");
advertise<PR2Arm>("cmd_rightarmconfig");
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-24 17:47:48
|
Revision: 927
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=927&view=rev
Author: hsujohnhsu
Date: 2008-06-24 10:47:55 -0700 (Tue, 24 Jun 2008)
Log Message:
-----------
bugfix in rosgazebo for joint position calls. added gripper state request calls.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-06-24 17:44:12 UTC (rev 926)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-06-24 17:47:55 UTC (rev 927)
@@ -267,6 +267,16 @@
public: PR2_ERROR_CODE CloseGripper(PR2_MODEL_ID id,double gap, double force);
/*! \fn
+ \brief - Get gripper gap and force setpoint
+ */
+ public: PR2_ERROR_CODE GetGripperCmd(PR2_MODEL_ID id,double *gap,double *force);
+
+ /*! \fn
+ \brief - Get gripper gap and force status
+ */
+ public: PR2_ERROR_CODE GetGripperActual(PR2_MODEL_ID id,double *gap,double *force);
+
+ /*! \fn
\brief - Set gripper p,i,d gains
*/
public: PR2_ERROR_CODE SetGripperGains(PR2_MODEL_ID id,double p,double i, double d);
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-06-24 17:44:12 UTC (rev 926)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-06-24 17:47:55 UTC (rev 927)
@@ -857,6 +857,44 @@
return PR2_ALL_OK;
};
+PR2_ERROR_CODE PR2HW::GetGripperCmd(PR2_MODEL_ID id,double *gap,double *force)
+{
+ switch(id)
+ {
+ case PR2_LEFT_GRIPPER:
+ *force = pr2GripperLeftIface->data->cmdForce;
+ *gap = pr2GripperLeftIface->data->cmdGap ;
+ break;
+ case PR2_RIGHT_GRIPPER:
+ *force = pr2GripperRightIface->data->cmdForce;
+ *gap = pr2GripperRightIface->data->cmdGap ;
+ break;
+ default:
+ break;
+ }
+ return PR2_ALL_OK;
+};
+
+PR2_ERROR_CODE PR2HW::GetGripperActual(PR2_MODEL_ID id,double *gap,double *force)
+{
+ switch(id)
+ {
+ case PR2_LEFT_GRIPPER:
+ *force = pr2GripperLeftIface->data->gripperForce; // FIXME: this is saturation force
+ *gap = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
+ +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
+ break;
+ case PR2_RIGHT_GRIPPER:
+ *force = pr2GripperRightIface->data->gripperForce; // FIXME: this is saturation force
+ *gap = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
+ +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
+ break;
+ default:
+ break;
+ }
+ return PR2_ALL_OK;
+};
+
PR2_ERROR_CODE PR2HW::SetGripperGains(PR2_MODEL_ID id,double p,double i, double d)
{
switch(id)
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-24 17:44:12 UTC (rev 926)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-24 17:47:55 UTC (rev 927)
@@ -223,10 +223,10 @@
// when running with the 2dnav stack, we need to refrain from moving when steering angles are off.
// when operating with the keyboard, we need instantaneous setting of both velocity and angular velocity.
- std::cout << "received cmd: vx " << this->velMsg.vx << " vw " << this->velMsg.vw
- << " vxsmoo " << this->vxSmooth << " vxsmoo " << this->vwSmooth
- << " | steer erros: " << this->myPR2->BaseSteeringAngleError() << " - " << M_PI/100.0
- << std::endl;
+ // std::cout << "received cmd: vx " << this->velMsg.vx << " vw " << this->velMsg.vw
+ // << " vxsmoo " << this->vxSmooth << " vxsmoo " << this->vwSmooth
+ // << " | steer erros: " << this->myPR2->BaseSteeringAngleError() << " - " << M_PI/100.0
+ // << std::endl;
// 2dnav: if steering angle is wrong, don't move or move slowly
if (this->myPR2->BaseSteeringAngleError() > M_PI/100.0)
@@ -596,39 +596,45 @@
std_msgs::PR2Arm larm, rarm;
/* get left arm position */
- this->myPR2->GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
larm.turretAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
larm.shoulderLiftAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
larm.upperarmRollAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
larm.elbowAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
larm.forearmRollAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
larm.wristPitchAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
larm.wristRollAngle = position;
// JOHN: We need to set the gripperForceCmd and gripperGapCmd as well; I think an API call is missing from libPR2API
+ this->myPR2->hw.GetGripperActual (PR2::PR2_LEFT_GRIPPER, &position, &velocity);
+ larm.gripperForceCmd = velocity;
+ larm.gripperGapCmd = position;
publish("left_pr2arm_pos", larm);
/* get left arm position */
- this->myPR2->GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
rarm.turretAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
rarm.shoulderLiftAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
rarm.upperarmRollAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
rarm.elbowAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
rarm.forearmRollAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
rarm.wristPitchAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
rarm.wristRollAngle = position;
// JOHN: We need to set the gripperForceCmd and gripperGapCmd as well; I think an API call is missing from libPR2API
+ this->myPR2->hw.GetGripperActual (PR2::PR2_RIGHT_GRIPPER, &position, &velocity);
+ rarm.gripperForceCmd = velocity;
+ rarm.gripperGapCmd = position;
publish("right_pr2arm_pos", rarm);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-25 19:02:55
|
Revision: 978
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=978&view=rev
Author: hsujohnhsu
Date: 2008-06-25 12:03:01 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
added some high level passthrough api's for the gripper and time.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-25 19:01:17 UTC (rev 977)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-25 19:03:01 UTC (rev 978)
@@ -614,6 +614,21 @@
*/
public: PR2_ERROR_CODE StopRobot();
+
+ public: PR2_ERROR_CODE GetLeftGripperCmd(double *gap,double *force);
+
+ public: PR2_ERROR_CODE GetLeftGripperActual(double *gap,double *force);
+
+ public: PR2_ERROR_CODE GetRightGripperCmd(double *gap,double *force);
+
+ public: PR2_ERROR_CODE GetRightGripperActual(double *gap,double *force);
+
+ /*! \fn
+ \brief - returns current time from simulator
+ */
+ public: PR2_ERROR_CODE GetTime(double *time);
+
+
protected: PR2_CONTROL_MODE armControlMode[2];
protected: PR2_CONTROL_MODE baseControlMode;
protected: PR2Arm myArm;
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-25 19:01:17 UTC (rev 977)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-25 19:03:01 UTC (rev 978)
@@ -804,5 +804,33 @@
};
+PR2_ERROR_CODE PR2Robot::GetLeftGripperCmd(double *gap,double *force)
+{
+ hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
+ return PR2_ALL_OK;
+}
+PR2_ERROR_CODE PR2Robot::GetLeftGripperActual(double *gap,double *force)
+{
+ hw.GetGripperActual((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
+ return PR2_ALL_OK;
+}
+PR2_ERROR_CODE PR2Robot::GetRightGripperCmd(double *gap,double *force)
+{
+ hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_RIGHT_GRIPPER,gap,force);
+ return PR2_ALL_OK;
+}
+
+PR2_ERROR_CODE PR2Robot::GetRightGripperActual(double *gap,double *force)
+{
+ hw.GetGripperActual((PR2_MODEL_ID)PR2::PR2_RIGHT_GRIPPER,gap,force);
+ return PR2_ALL_OK;
+}
+
+
+PR2_ERROR_CODE PR2Robot::GetTime(double *time)
+{
+ return hw.GetSimTime(time);
+}
+
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-25 19:01:17 UTC (rev 977)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-25 19:03:01 UTC (rev 978)
@@ -206,7 +206,7 @@
// smooth out the commands by time decay
// with w1,w2=1, this means equal weighting for new command every second
- this->myPR2->hw.GetSimTime(&(this->simTime));
+ this->myPR2->GetTime(&(this->simTime));
dt = simTime - lastTime;
// smooth if dt is larger than zero
@@ -289,8 +289,8 @@
this->myPR2->EnableGripperLeft();
this->myPR2->EnableGripperRight();
- this->myPR2->hw.GetSimTime(&(this->lastTime));
- this->myPR2->hw.GetSimTime(&(this->simTime));
+ this->myPR2->GetTime(&(this->lastTime));
+ this->myPR2->GetTime(&(this->simTime));
// set torques for driving the robot wheels
// this->myPR2->hw.SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
@@ -441,7 +441,7 @@
}
- this->myPR2->hw.GetSimTime(&(this->simTime));
+ this->myPR2->GetTime(&(this->simTime));
/***************************************************************/
/* */
@@ -569,7 +569,7 @@
simPitchOffset = -M_PI / 8.0;
simPitchAngle = simPitchOffset + simPitchAmp * sin(this->simTime * simPitchTimeScale);
simPitchRate = simPitchAmp * simPitchTimeScale * cos(this->simTime * simPitchTimeScale); // TODO: check rate correctness
- this->myPR2->hw.GetSimTime(&this->simTime);
+ this->myPR2->GetTime(&this->simTime);
//std::cout << "sim time: " << this->simTime << std::endl;
//std::cout << "ang: " << simPitchAngle*180.0/M_PI << "rate: " << simPitchRate*180.0/M_PI << std::endl;
this->myPR2->hw.SetJointTorque(PR2::HEAD_LASER_PITCH , 1000.0);
@@ -610,7 +610,7 @@
larm.wristPitchAngle = position;
this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
larm.wristRollAngle = position;
- this->myPR2->hw.GetGripperActual (PR2::PR2_LEFT_GRIPPER, &position, &velocity);
+ this->myPR2->GetLeftGripperActual ( &position, &velocity);
larm.gripperForceCmd = velocity;
larm.gripperGapCmd = position;
publish("left_pr2arm_pos", larm);
@@ -630,7 +630,7 @@
rarm.wristPitchAngle = position;
this->myPR2->hw.GetJointPositionActual(PR2::ARM_R_WRIST_ROLL, &position, &velocity);
rarm.wristRollAngle = position;
- this->myPR2->hw.GetGripperActual (PR2::PR2_RIGHT_GRIPPER, &position, &velocity);
+ this->myPR2->GetRightGripperActual ( &position, &velocity);
rarm.gripperForceCmd = velocity;
rarm.gripperGapCmd = position;
publish("right_pr2arm_pos", rarm);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-06-26 02:17:01
|
Revision: 1009
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1009&view=rev
Author: morgan_quigley
Date: 2008-06-25 19:17:07 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
updates to sicktoolbox manifest and to bumblehack
Modified Paths:
--------------
pkg/trunk/3rdparty/drivers/sicktoolbox/manifest.xml
pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/bumblebee_bridge.cpp
pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
Modified: pkg/trunk/3rdparty/drivers/sicktoolbox/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/drivers/sicktoolbox/manifest.xml 2008-06-26 02:15:44 UTC (rev 1008)
+++ pkg/trunk/3rdparty/drivers/sicktoolbox/manifest.xml 2008-06-26 02:17:07 UTC (rev 1009)
@@ -8,7 +8,7 @@
<license>BSD</license>
<url>http://sicktoolbox.sourceforge.net</url>
<export>
- <cpp lflags="-rpath ${prefix}/lib -L${prefix}/sicktoolbox/lib -lsicktoolbox-1.0" cflags="-I${prefix}/sicktoolbox/include"/>
+ <cpp lflags="-rpath ${prefix}/lib -L${prefix}/sicktoolbox/lib -lsicklms-1.0" cflags="-I${prefix}/sicktoolbox/include"/>
</export>
</package>
Modified: pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/bumblebee_bridge.cpp
===================================================================
--- pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/bumblebee_bridge.cpp 2008-06-26 02:15:44 UTC (rev 1008)
+++ pkg/trunk/drivers/cam/ptgrey/bumblebee_bridge/bumblebee_bridge.cpp 2008-06-26 02:17:07 UTC (rev 1009)
@@ -39,8 +39,7 @@
#include "opencv/highgui.h"
using namespace XmlRpc;
-const char *BUMBLEBEE_HOST = "171.66.34.119"; // temporary IP since our SAIL
- // net is trashed right now
+const char *BUMBLEBEE_HOST = "stair-vision";
const int BUMBLEBEE_PORT = 11411; // a palindromic prime, naturally
class BumblebeeBridge : public ros::node
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2008-06-26 02:15:44 UTC (rev 1008)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2008-06-26 02:17:07 UTC (rev 1009)
@@ -6,4 +6,6 @@
<author>Morgan Quigley</author>
<license>BSD</license>
<depend package="sicktoolbox"/>
+ <depend package="roscpp"/>
+ <depend package="std_msgs"/>
</manifest>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-26 22:18:39
|
Revision: 1034
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1034&view=rev
Author: hsujohnhsu
Date: 2008-06-26 15:18:48 -0700 (Thu, 26 Jun 2008)
Log Message:
-----------
switching to new gazebo repository from SF.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/world/Media/materials/scripts/Gazebo.material
pkg/trunk/demos/2dnav-gazebo/world/pr2.model
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/simulators/gazebo_plugin/Makefile
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Gripper.hh
pkg/trunk/simulators/gazebo_plugin/manifest.xml
pkg/trunk/simulators/gazebo_plugin/src/P3D.cc
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Gripper.cc
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
pkg/trunk/simulators/rosgazebo/setup.bash
pkg/trunk/simulators/rosgazebo/setup.tcsh
Added Paths:
-----------
pkg/trunk/3rdparty/gazebo/
pkg/trunk/3rdparty/gazebo/Makefile
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/3rdparty/gazebo/jointforce.diff
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/3rdparty/gazebo/patches/
pkg/trunk/3rdparty/gazebo/patches/GLWindow.cc.diff
pkg/trunk/3rdparty/gazebo/patches/GazeboConfig.cc.diff
pkg/trunk/3rdparty/gazebo/patches/Generic_Camera.cc.diff
pkg/trunk/3rdparty/gazebo/patches/HingeJoint.cc.diff
pkg/trunk/3rdparty/gazebo/patches/Iface.cc.diff
pkg/trunk/3rdparty/gazebo/patches/Server.cc.diff
pkg/trunk/3rdparty/gazebo/patches/SliderJoint.cc.diff
pkg/trunk/3rdparty/gazebo/patches/World.cc.diff
pkg/trunk/3rdparty/gazebo/patches/World.hh.diff
pkg/trunk/3rdparty/gazebo/patches/gazebo.h.diff
pkg/trunk/3rdparty/gazebo/setup.bash
pkg/trunk/3rdparty/gazebo/setup.tcsh
Removed Paths:
-------------
pkg/trunk/3rdparty/gazebo/Makefile
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/3rdparty/gazebo/setup.bash
pkg/trunk/3rdparty/gazebo/setup.tcsh
pkg/trunk/3rdparty/gazebo2/
Copied: pkg/trunk/3rdparty/gazebo (from rev 1021, pkg/trunk/3rdparty/gazebo2)
Deleted: pkg/trunk/3rdparty/gazebo/Makefile
===================================================================
--- pkg/trunk/3rdparty/gazebo2/Makefile 2008-06-26 16:54:16 UTC (rev 1021)
+++ pkg/trunk/3rdparty/gazebo/Makefile 2008-06-26 22:18:48 UTC (rev 1034)
@@ -1,25 +0,0 @@
-all: gazebo2
-
-#REVISION = -r 6641
-
-OGRE_PATH = $(shell rospack find ogre)/ogre
-ODE_PATH = $(shell rospack find opende)/opende
-ROOT = $(shell rospack find gazebo2)/gazebo
-
-gazebo-checkout:
- @if [ ! -d gazebo-svn ]; then svn co $(REVISION) https://playerstage.svn.sf.net/svnroot/playerstage/code/gazebo/trunk gazebo-svn; fi
- -cd gazebo-svn && svn up $(REVISION)
-
-gazebo2: gazebo-checkout
- if [ ! -d gazebo ]; then mkdir -p gazebo; fi
- cd gazebo-svn && export PKG_CONFIG_PATH=${OGRE_PATH}/lib/pkgconfig:${PKG_CONFIG_PATH} && export PATH=${ODE_PATH}/bin:${PATH} && scons prefix=$(ROOT) && scons prefix=$(ROOT) install
- touch gazebo
-
-clean:
- cd gazebo-svn && scons --clean
- rm -rf gazebo
-wipe:
- rm -rf gazebo-svn gazebo
-
-download: gazebo-checkout
-
Copied: pkg/trunk/3rdparty/gazebo/Makefile (from rev 1033, pkg/trunk/3rdparty/gazebo2/Makefile)
===================================================================
--- pkg/trunk/3rdparty/gazebo/Makefile (rev 0)
+++ pkg/trunk/3rdparty/gazebo/Makefile 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,26 @@
+all: gazebo
+
+REVISION = -r 6696
+
+OGRE_PATH = $(shell rospack find ogre)/ogre
+ODE_PATH = $(shell rospack find opende)/opende
+ROOT = $(shell rospack find gazebo)/gazebo
+
+gazebo-checkout:
+ @if [ ! -d gazebo-svn ]; then svn co $(REVISION) https://playerstage.svn.sf.net/svnroot/playerstage/code/gazebo/trunk gazebo-svn; fi
+ -cd gazebo-svn && svn up $(REVISION)
+ -cd gazebo-svn/server && patch -N -p0 < gazebo_patch.diff
+
+gazebo: gazebo-checkout
+ if [ ! -d gazebo ]; then mkdir -p gazebo; fi
+ cd gazebo-svn && export PKG_CONFIG_PATH=${OGRE_PATH}/lib/pkgconfig:${PKG_CONFIG_PATH} && export PATH=${ODE_PATH}/bin:${PATH} && scons prefix=$(ROOT) && scons prefix=$(ROOT) install
+ touch gazebo
+
+clean:
+ cd gazebo-svn && scons --clean
+ rm -rf gazebo
+wipe:
+ rm -rf gazebo-svn gazebo
+
+download: gazebo-checkout
+
Added: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,721 @@
+Index: gazebo-svn/libgazebo/Server.cc
+===================================================================
+--- gazebo-svn/libgazebo/Server.cc (revision 6696)
++++ gazebo-svn/libgazebo/Server.cc (working copy)
+@@ -38,6 +38,7 @@
+ #include <sys/sem.h>
+ #include <sstream>
+ #include <iostream>
++#include <signal.h>
+
+ #include "gazebo.h"
+
+@@ -92,6 +93,42 @@
+
+ std::cout << "creating " << this->filename << "\n";
+
++ // check to see if there is already a directory created.
++ struct stat astat;
++ if (stat(this->filename.c_str(), &astat) == 0) {
++ // directory already exists, check gazebo.pid to see if
++ // another gazebo is already running.
++
++ std::string pidfn = this->filename + "/gazebo.pid";
++
++ FILE *fp = fopen(pidfn.c_str(), "r");
++ if(fp) {
++ int pid;
++ fscanf(fp, "%d", &pid);
++ fclose(fp);
++ std::cout << "found a pid file: pid=" << pid << "\n";
++
++ if(kill(pid, 0) == 0) {
++ // a gazebo process is still alive.
++ errStream << "directory [" << this->filename
++ << "] already exists (previous crash?)\n"
++ << "gazebo (pid=" << pid << ") is still running.";
++ throw(errStream.str());
++ } else {
++ // the gazebo process is not alive.
++ // remove directory.
++ std::cout << "The gazebo process is not alive.\n";
++
++ // remove the existing directory.
++ std::string cmd = "rm -rf '" + this->filename + "'";
++ if(system(cmd.c_str()) != 0) {
++ errStream << "couldn't remove directory [" << this->filename << "]";
++ throw(errStream.str());
++ }
++ }
++ }
++ }
++
+ // Create the directory
+ if (mkdir(this->filename.c_str(), S_IRUSR | S_IWUSR | S_IXUSR) != 0)
+ {
+@@ -108,7 +145,17 @@
+ << strerror(errno) << "]";
+ throw(errStream.str());
+ }
++
+ }
++
++ // write the PID to a file
++ std::string pidfn = this->filename + "/gazebo.pid";
++
++ FILE *fp = fopen(pidfn.c_str(), "w");
++ if(fp) {
++ fprintf(fp, "%d\n", getpid());
++ fclose(fp);
++ }
+ }
+
+
+@@ -120,6 +167,15 @@
+
+ std::cout << "deleting " << this->filename << "\n";
+
++ // unlink the pid file
++ std::string pidfn = this->filename + "/gazebo.pid";
++ if (unlink(pidfn.c_str()) < 0)
++ {
++ std::ostringstream stream;
++ stream << "error deleting pid file: " << strerror(errno);
++ throw(stream.str());
++ }
++
+ // Delete the server dir
+ if (rmdir(this->filename.c_str()) != 0)
+ {
+Index: gazebo-svn/libgazebo/Iface.cc
+===================================================================
+--- gazebo-svn/libgazebo/Iface.cc (revision 6696)
++++ gazebo-svn/libgazebo/Iface.cc (working copy)
+@@ -55,6 +55,8 @@
+ GZ_REGISTER_IFACE("factory", FactoryIface);
+ GZ_REGISTER_IFACE("gripper", GripperIface);
+ GZ_REGISTER_IFACE("actarray", ActarrayIface);
++GZ_REGISTER_IFACE("pr2array", PR2ArrayIface);
++GZ_REGISTER_IFACE("pr2gripper", PR2GripperIface);
+ GZ_REGISTER_IFACE("ptz", PTZIface);
+ GZ_REGISTER_IFACE("stereocamera", StereoCameraIface);
+
+Index: gazebo-svn/libgazebo/gazebo.h
+===================================================================
+--- gazebo-svn/libgazebo/gazebo.h (revision 6696)
++++ gazebo-svn/libgazebo/gazebo.h (working copy)
+@@ -550,7 +550,7 @@
+
+
+ /// Maximum image pixels (width x height)
+-#define GAZEBO_CAMERA_MAX_IMAGE_SIZE 640 * 480 * 3
++#define GAZEBO_CAMERA_MAX_IMAGE_SIZE 1024 * 1024 * 3
+
+ /// \brief Camera interface data
+ class CameraData
+@@ -1057,6 +1057,30 @@
+
+ /// Lift down flag
+ public: int lift_down;
++
++ /// Enable flag
++ public: int cmdEnableMotor;
++
++ /// Gripper Position Rate Command
++ public: float cmdPositionRate;
++ /// Gripper Force Command
++ public: float cmdForce;
++ /// Gripper Gap Command
++ public: float cmdGap;
++
++ /// Gripper force
++ public: double gripperForce;
++ /// Gripper controller gains
++ public: double pGain;
++ public: double dGain;
++ public: double iGain;
++
++ /// Position
++ public: float actualFingerPosition[2];
++ /// Position Rate
++ public: float actualFingerPositionRate[2];
++
++
+ };
+
+ /// \brief Gripper interface
+@@ -1254,9 +1278,327 @@
+ /// \} */
+
+
++
++
+ /***************************************************************************/
+ /// \addtogroup libgazebo_iface
+ /// \{
++/** \defgroup gripper_iface gripper
++
++ \brief PR2 Gripper interface
++
++The gripper interface allows control of a simple pseudo-1-DOF PR2 gripper, such as
++that found on the Pioneer series robots.
++
++
++\{
++*/
++
++/** Gripper state: open */
++#define GAZEBO_PR2GRIPPER_STATE_OPEN 1
++/** Gripper state: closed */
++#define GAZEBO_PR2GRIPPER_STATE_CLOSED 2
++/** Gripper state: moving */
++#define GAZEBO_PR2GRIPPER_STATE_MOVING 3
++/** Gripper state: error */
++#define GAZEBO_PR2GRIPPER_STATE_ERROR 4
++
++/** Gripper command: open */
++#define GAZEBO_PR2GRIPPER_CMD_OPEN 1
++/** Gripper command: close */
++#define GAZEBO_PR2GRIPPER_CMD_CLOSE 2
++/** Gripper command: stop */
++#define GAZEBO_PR2GRIPPER_CMD_STOP 3
++/** Gripper command: store */
++#define GAZEBO_PR2GRIPPER_CMD_STORE 4
++/** Gripper command: retrieve */
++#define GAZEBO_PR2GRIPPER_CMD_RETRIEVE 5
++
++
++/// \brief Fudicial interface data
++class PR2GripperData
++{
++ public: GazeboData head;
++
++ /// \brief Current command for the gripper
++ public: int cmd;
++
++ /// Current state of the gripper
++ public: int state;
++
++ /// Gripped limit reached flag
++ public: int grip_limit_reach;
++
++ /// Lift limit reached flag
++ public: int lift_limit_reach;
++
++ /// Outer beam obstruct flag
++ public: int outer_beam_obstruct;
++
++ /// control mode, TODO: yet to be defined
++ public: int controlMode;
++
++ /// Inner beam obstructed flag
++ public: int inner_beam_obstruct;
++
++ /// Left paddle open flag
++ public: int left_paddle_open;
++
++ /// Right paddle open flag
++ public: int right_paddle_open;
++
++ /// Lift up flag
++ public: int lift_up;
++
++ /// Lift down flag
++ public: int lift_down;
++
++ /// Enable flag
++ public: int cmdEnableMotor;
++
++ /// Gripper Position Rate Command
++ public: float cmdPositionRate;
++ /// Gripper Force Command
++ public: float cmdForce;
++ /// Gripper Gap Command
++ public: float cmdGap;
++
++ /// Gripper force
++ public: double gripperForce;
++ /// Gripper controller gains
++ public: double pGain;
++ public: double dGain;
++ public: double iGain;
++
++ /// Position
++ public: float actualFingerPosition[2];
++ /// Position Rate
++ public: float actualFingerPositionRate[2];
++
++
++};
++
++/// \brief Gripper interface
++class PR2GripperIface : public Iface
++{
++ /// \brief Constructor
++ public: PR2GripperIface():Iface("pr2gripper", sizeof(PR2GripperIface)+sizeof(PR2GripperData)) {}
++
++ /// \brief Destructor
++ public: virtual ~PR2GripperIface() {this->data = NULL;}
++
++ /// \brief Create the server
++ public: virtual void Create(Server *server, std::string id)
++ {
++ Iface::Create(server,id);
++ this->data = (PR2GripperData*)this->mMap;
++ }
++
++ /// \brief Open the iface
++ public: virtual void Open(Client *client, std::string id)
++ {
++ Iface::Open(client,id);
++ this->data = (PR2GripperData*)this->mMap;
++ }
++
++ /// Pointer to the gripper data
++ public: PR2GripperData *data;
++};
++
++/** \} */
++/// \}
++
++
++/***************************************************************************/
++/// \addtogroup libgazebo_iface
++/// \{
++/** \defgroup pr2array_iface actarray
++
++ \brief PR2 Array
++
++The PR2 array interface allows a user to control a set of actuators.
++
++\{
++*/
++
++/// maximum number of actuators
++#define GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS 64
++#define GAZEBO_PR2ARRAY_JOINT_POSITION_MODE 0
++#define GAZEBO_PR2ARRAY_JOINT_SPEED_MODE 1
++#define GAZEBO_PR2ARRAY_JOINT_CURRENT_MODE 2
++
++//Actuator states
++/// Idle state
++#define GAZEBO_PR2ARRAY_ACTSTATE_IDLE 1
++
++/// Moving state
++#define GAZEBO_PR2ARRAY_ACTSTATE_MOVING 2
++
++/// Braked state
++#define GAZEBO_PR2ARRAY_ACTSTATE_BRAKED 3
++
++/// Stalled state
++#define GAZEBO_PR2ARRAY_ACTSTATE_STALLED 4
++
++/// Linear type
++#define GAZEBO_PR2ARRAY_TYPE_LINEAR 1
++/// Rotary type
++#define GAZEBO_PR2ARRAY_TYPE_ROTARY 2
++
++/// Request subtype: power
++#define GAZEBO_PR2ARRAY_POWER_REQ 1
++/// Request subtype: brakes
++#define GAZEBO_PR2ARRAY_BRAKES_REQ 2
++/// Request subtype: get geometry
++#define GAZEBO_PR2ARRAY_GET_GEOM_REQ 3
++/// Request subtype: speed
++#define GAZEBO_PR2ARRAY_SPEED_REQ 4
++
++/// Command subtype: position
++#define GAZEBO_PR2ARRAY_POS_CMD 1
++/// Command subtype: speed
++#define GAZEBO_PR2ARRAY_SPEED_CMD 2
++/// Command subtype: home
++#define GAZEBO_PR2ARRAY_HOME_CMD 3
++
++
++/// \brief Actuator geometry
++class PR2ArrayActuatorGeom
++{
++
++/// Data subtype: state
++#define GAZEBO_PR2ARRAY_DATA_STATE 1
++
++
++ /// The type of the actuator - linear or rotary.
++ public: uint8_t type;
++
++ /// Min range of motion (m or rad depending on the type)
++ public: float min;
++
++ /// Center position (m or rad)
++ public: float center;
++
++ /// Max range of motion (m or rad depending on the type)
++ public: float max;
++
++ /// Home position (m or rad depending on the type)
++ public: float home;
++
++ /// The configured speed - different from current speed.
++ public: float config_speed;
++
++ /// The maximum achievable speed of the actuator.
++ public: float max_speed;
++
++ /// If the actuator has brakes or not.
++ public: uint8_t hasbrakes;
++};
++
++/// \brief Structure containing a single actuator's information
++class PR2ArrayActuator
++{
++ /// The position of the actuator in m or rad depending on the type.
++ public: float actualPosition;
++ /// The speed of the actuator in m/s or rad/s depending on the type.
++ public: float actualSpeed;
++ /// The current state of the actuator.
++ public: uint8_t state;
++};
++
++ typedef struct
++ {
++ double timestamp;
++ double actualPosition;
++ double actualSpeed;
++ double actualEffectorForce;
++
++ int controlMode;
++ int jointType;
++
++ double cmdPosition;
++ double cmdSpeed;
++ double cmdEffectorForce;
++
++ int cmdEnableMotor;
++
++ double pGain;
++ double iGain;
++ double dGain;
++ double iClamp;
++ double saturationTorque;
++ } JointData;
++
++/// \brief The actuator array data packet.
++class PR2ArrayData
++{
++ public: GazeboData head;
++
++ /// The number of actuators in the array.
++ public: unsigned int actuators_count;
++
++ /// The actuator data.
++// public: PR2ArrayActuator actuators[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++ public: JointData actuators[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// The actuators geoms
++ public: PR2ArrayActuatorGeom actuator_geoms[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// position commands
++ public: float cmd_pos[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// speed commands
++ public: float cmd_speed[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++ /// bad command flag - (speed to high set for the actuators or position not reachable)
++ public: int bad_cmd;
++
++ /// True if new command
++ public: bool new_cmd;
++
++ /// position / speed comand
++ public: unsigned int joint_mode[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
++
++};
++
++/// \brief The PR2Array interface
++class PR2ArrayIface : public Iface
++{
++ /// \brief Create an interface
++ public: PR2ArrayIface():Iface("pr2array", sizeof(PR2ArrayIface)+sizeof(PR2ArrayData)) {}
++
++ /// \brief Destroy and Interface
++ public: virtual ~PR2ArrayIface() {this->data = NULL;}
++
++ /// \brief Create the interface (used by Gazebo server)
++ /// \param server Pointer to the server
++ /// \param id Id of the interface
++ public: virtual void Create(Server *server, std::string id)
++ {
++ Iface::Create(server,id);
++ this->data = (PR2ArrayData*)this->mMap;
++ }
++
++ /// \brief Open an existing interface
++ /// \param client Pointer to the client
++ /// \param id Id of the interface
++ public: virtual void Open(Client *client, std::string id)
++ {
++ Iface::Open(client,id);
++ this->data = (PR2ArrayData*)this->mMap;
++ }
++
++ /// Pointer to the act array data
++ public: PR2ArrayData *data;
++};
++
++/** \} */
++/// \} */
++
++
++/***************************************************************************/
++/// \addtogroup libgazebo_iface
++/// \{
+ /** \defgroup ptz_iface ptz
+
+ \brief PTZ interface
+Index: gazebo-svn/server/physics/HingeJoint.cc
+===================================================================
+--- gazebo-svn/server/physics/HingeJoint.cc (revision 6696)
++++ gazebo-svn/server/physics/HingeJoint.cc (working copy)
+@@ -38,6 +38,7 @@
+ : Joint()
+ {
+ this->jointId = dJointCreateHinge( worldId, NULL );
++ this->type = Joint::HINGE;
+ }
+
+
+Index: gazebo-svn/server/physics/SliderJoint.cc
+===================================================================
+--- gazebo-svn/server/physics/SliderJoint.cc (revision 6696)
++++ gazebo-svn/server/physics/SliderJoint.cc (working copy)
+@@ -35,6 +35,8 @@
+ : Joint()
+ {
+ this->jointId = dJointCreateSlider( worldId, NULL );
++ this->type = Joint::SLIDER;
++ fprintf(stderr," slider jointId %d\n",this->jointId);
+ }
+
+
+Index: gazebo-svn/server/GazeboConfig.cc
+===================================================================
+--- gazebo-svn/server/GazeboConfig.cc (revision 6696)
++++ gazebo-svn/server/GazeboConfig.cc (working copy)
+@@ -56,6 +56,17 @@
+
+ cfgFile.open(rcFilename.c_str(), std::ios::in);
+
++ char *ogre_resource_path = getenv("OGRE_RESOURCE_PATH");
++ if(ogre_resource_path) {
++ this->ogrePaths.push_back(ogre_resource_path);
++ }
++ char *gazebo_resource_path = getenv("GAZEBO_RESOURCE_PATH");
++ if(gazebo_resource_path) {
++ this->gazeboPaths.push_back(gazebo_resource_path);
++ }
++ // if both paths are set, don't check the config file or use the defaults.
++ if(ogre_resource_path && gazebo_resource_path) return;
++
+ if (cfgFile)
+ {
+ XMLConfig rc;
+Index: gazebo-svn/server/gui/GLWindow.cc
+===================================================================
+--- gazebo-svn/server/gui/GLWindow.cc (revision 6696)
++++ gazebo-svn/server/gui/GLWindow.cc (working copy)
+@@ -66,7 +66,11 @@
+ this->directionVec.x = 0;
+ this->directionVec.y = 0;
+ this->directionVec.z = 0;
++ this->leftMousePressed = false;
++ this->rightMousePressed = false;
++ this->middleMousePressed = false;
+
++
+ this->keys.clear();
+
+ if (activeWin == NULL)
+@@ -235,6 +239,21 @@
+ this->activeCamera->RotateYaw(DTOR(-d.x * this->rotateAmount));
+ this->activeCamera->RotatePitch(DTOR(d.y * this->rotateAmount));
+ }
++ if (this->rightMousePressed)
++ {
++ Vector2<int> d = this->mousePos - this->prevMousePos;
++ this->directionVec.x = 0;
++ this->directionVec.y = d.x * this->moveAmount;
++ this->directionVec.z = d.y * this->moveAmount;
++ }
++ if (this->middleMousePressed)
++ {
++ Vector2<int> d = this->mousePos - this->prevMousePos;
++ this->directionVec.x = d.y * this->moveAmount;
++ this->directionVec.y = 0;
++ this->directionVec.z = 0;
++ }
++
+ }
+
+ this->mouseDrag = true;
+@@ -247,12 +266,28 @@
+ std::map<int,int>::iterator iter;
+ this->keys[keyNum] = 1;
+
++ // loop through the keys to find the modifiers -- swh
++ float moveAmount = this->moveAmount;
+ for (iter = this->keys.begin(); iter!= this->keys.end(); iter++)
+ {
+ if (iter->second == 1)
+ {
+ switch (iter->first)
+ {
++ case FL_Control_L:
++ case FL_Control_R:
++ moveAmount = this->moveAmount * 10;
++ break;
++ }
++ }
++ }
++
++ for (iter = this->keys.begin(); iter!= this->keys.end(); iter++)
++ {
++ if (iter->second == 1)
++ {
++ switch (iter->first)
++ {
+ case '=':
+ case '+':
+ this->moveAmount *= 2;
+Index: gazebo-svn/server/World.hh
+===================================================================
+--- gazebo-svn/server/World.hh (revision 6696)
++++ gazebo-svn/server/World.hh (working copy)
+@@ -91,6 +91,26 @@
+ /// \return Pointer to the physics engine
+ public: PhysicsEngine *GetPhysicsEngine() const;
+
++ /// Get the simulation time
++ /// \return The simulation time
++ public: double GetSimTime() const;
++
++ /// Get the pause time
++ /// \return The pause time
++ public: double GetPauseTime() const;
++
++ /// Get the start time
++ /// \return The start time
++ public: double GetStartTime() const;
++
++ /// Get the real time (elapsed time)
++ /// \return The real time
++ public: double GetRealTime() const;
++
++ /// \brief Get the wall clock time
++ /// \return The wall clock time
++ public: double GetWallTime() const;
++
+ /// \brief Load all entities
+ /// \param node XMLConfg node pointer
+ /// \param parent Parent of the model to load
+@@ -185,6 +205,9 @@
+ /// Simulation interface
+ private: SimulationIface *simIface;
+
++ /// Current simulation time
++ private: double simTime, pauseTime, startTime;
++
+ private: friend class DestroyerT<World>;
+ private: friend class SingletonT<World>;
+ };
+Index: gazebo-svn/server/World.cc
+===================================================================
+--- gazebo-svn/server/World.cc (revision 6696)
++++ gazebo-svn/server/World.cc (working copy)
+@@ -27,6 +27,7 @@
+ #include <assert.h>
+ #include <sstream>
+ #include <fstream>
++#include <sys/time.h> //gettimeofday
+
+ #include "Global.hh"
+ #include "GazeboError.hh"
+@@ -57,6 +58,9 @@
+ this->server = NULL;
+ this->simIface = NULL;
+
++ this->simTime = 0.0;
++ this->pauseTime = 0.0;
++ this->startTime = 0.0;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -157,6 +161,7 @@
+
+ this->physicsEngine->Init();
+
++ this->startTime = this->GetWallTime();
+ this->toAddModels.clear();
+ this->toDeleteModels.clear();
+
+@@ -170,6 +175,8 @@
+ std::vector< Model* >::iterator miter;
+ std::vector< Model* >::iterator miter2;
+
++ this->simTime += this->physicsEngine->GetStepTime();
++
+ // Update all the models
+ for (miter=this->models.begin(); miter!=this->models.end(); miter++)
+ {
+@@ -183,6 +190,10 @@
+ {
+ this->physicsEngine->Update();
+ }
++ else
++ {
++ this->pauseTime += this->physicsEngine->GetStepTime();
++ }
+
+ this->UpdateSimulationIface();
+
+@@ -250,6 +261,41 @@
+ return this->physicsEngine;
+ }
+
++////////////////////////////////////////////////////////////////////////////////
++// Get the simulation time
++double World::GetSimTime() const
++{
++ return this->simTime;
++}
++////////////////////////////////////////////////////////////////////////////////
++// Get the pause time
++double World::GetPauseTime() const
++{
++ return this->pauseTime;
++}
++
++////////////////////////////////////////////////////////////////////////////////
++/// Get the start time
++double World::GetStartTime() const
++{
++ return this->startTime;
++}
++////////////////////////////////////////////////////////////////////////////////
++/// Get the real time (elapsed time)
++double World::GetRealTime() const
++{
++ return this->GetWallTime() - this->startTime;
++}
++
++////////////////////////////////////////////////////////////////////////////////
++/// Get the wall clock time
++double World::GetWallTime() const
++{
++ struct timeval tv;
++ gettimeofday(&tv, NULL);
++ return tv.tv_sec + tv.tv_usec * 1e-6;
++}
++
+ ///////////////////////////////////////////////////////////////////////////////
+ // Load a model
+ int World::LoadEntities(XMLConfigNode *node, Model *parent)
Added: pkg/trunk/3rdparty/gazebo/jointforce.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/jointforce.diff (rev 0)
+++ pkg/trunk/3rdparty/gazebo/jointforce.diff 2008-06-26 22:18:48 UTC (rev 1034)
@@ -0,0 +1,300 @@
+Index: server/physics/Joint.hh
+===================================================================
+--- server/physics/Joint.hh (Revision 6545)
++++ server/physics/Joint.hh (Arbeitskopie)
+@@ -124,6 +124,9 @@
+ /// \brief Get the CFM of this joint
+ public: double GetCFM();
+
++ /// \brief Get the feedback data structure for this joint, if set
++ public: dJointFeedback *GetFeedback();
++
+ /// \brief Get the high stop of an axis(index).
+ public: double GetHighStop(int index=0);
+
+@@ -145,6 +148,9 @@
+ /// \brief Name of this joint
+ private: std::string name;
+
++ /// \brief Feedback data for this joint
++ private: dJointFeedback* feedback;
++
+ private: OgreVisual *visual;
+
+ private: Model *model;
+Index: server/physics/Joint.cc
+===================================================================
+--- server/physics/Joint.cc (Revision 6545)
++++ server/physics/Joint.cc (Arbeitskopie)
+@@ -88,6 +88,10 @@
+ this->line1->AddPoint(Vector3(0,0,0));
+ this->line2->AddPoint(Vector3(0,0,0));
+ this->line2->AddPoint(Vector3(0,0,0));
++ if(node->GetBool("providefeedback", false, 0)) {
++ this->feedback = new dJointFeedback;
++ dJointSetFeedback(this->jointId, this->feedback);
++ }
+
+ }
+
+@@ -258,6 +262,13 @@
+ return this->GetParam(dParamSuspensionCFM);
+ }
+
++///////////////////////////////////////////////////////////////////////////////
++/// Get the feedback data structure of this joint
++dJointFeedback *Joint::GetFeedback()
++{
++ return dJointGetFeedback(this->jointId);
++}
++
+ ////////////////////////////////////////////////////////////////////////////////
+ /// Get the high stop of an axis(index).
+ double Joint::GetHighStop(int index)
+Index: server/controllers/opaque/jointforce/JointForce.hh
+===================================================================
+--- server/controllers/opaque/jointforce/JointForce.hh (Revision 0)
++++ server/controllers/opaque/jointforce/JointForce.hh (Revision 0)
+@@ -0,0 +1,92 @@
++/*
++ * Gazebo - Outdoor Multi-Robot Simulator
++ * Copyright (C) 2003
++ * Nate Koenig & Andrew Howard
++ *
++ * 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 2 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, write to the Free Software
++ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
++ *
++ */
++/*
++ * Desc: Joint Force Controller
++ * Author: Benjamin Kloster
++ * Date: 13 March 2008
++ */
++#ifndef JOINTFORCE_CONTROLLER_HH
++#define JOINTFORCE_CONTROLLER_HH
++
++/// Maximum number of joints that can be watched by one controller
++#define GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS 16
++
++#include "Controller.hh"
++#include "Entity.hh"
++#include <ode/ode.h>
++#include <sys/time.h>
++
++
++namespace gazebo
++{
++/// \addtogroup gazebo_controller
++/// \{
++/** \defgroup jointforce_controller jointforce
++
++ \brief A controller that measures forces and torques exerted by joints
++
++ \{
++*/
++
++/// \brief A JointForce controller
++class JointForce : public Controller
++{
++ /// Constructor
++ public: JointForce(Entity *parent );
++
++ /// Destructor
++ public: virtual ~JointForce();
++
++ /// Load the controller
++ /// \param node XML config node
++ /// \return 0 on success
++ protected: virtual void LoadChild(XMLConfigNode *node);
++
++ /// Init the controller
++ /// \return 0 on success
++ protected: virtual void InitChild();
++
++ /// Update the controller
++ /// \return 0 on success
++ protected: virtual void UpdateChild(UpdateParams ¶ms);
++
++ /// Finalize the controller
++ /// \return 0 on success
++ protected: virtual void FiniChild();
++
++ /// The parent Model
++ private: Model *myParent;
++
++ /// The Iface. The dJointFeedback structs are rather arbitrary, so we use an Opaque Interface
++ private: OpaqueIface *myIface;
++ /// The joint feedbacks
++ private: dJointFeedback *jointfeedbacks[GAZEBO_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS];
++ /// The number of joints we are watching
++ private: int n_joints;
++};
++
++/** \} */
++/// \}
++
++}
++
++#endif
++
+Index: server/controllers/opaque/jointforce/JointForce.cc
+===================================================================
+--- server/controllers/opaque/jointforce/JointForce.cc (Revision 0)
++++ server/controllers/opaque/jointforce/JointForce.cc (Revision 0)
+@@ -0,0 +1,121 @@
++/*
++ * Gazebo - Outdoor Multi-Robot Simulator
++ * Copyright (C) 2003
++ * Nate Koenig & Andrew Howard
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under t...
[truncated message content] |
|
From: <hsu...@us...> - 2008-06-27 22:24:02
|
Revision: 1061
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1061&view=rev
Author: hsujohnhsu
Date: 2008-06-27 15:24:07 -0700 (Fri, 27 Jun 2008)
Log Message:
-----------
moved controllers around. Sorry David.
Modified Paths:
--------------
pkg/trunk/controllers/genericControllers/src/Pid.cpp
pkg/trunk/simulators/gazebo_plugin/Makefile
pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/simulators/gazebo_plugin/manifest.xml
Added Paths:
-----------
pkg/trunk/controllers/genericControllers/
pkg/trunk/controllers/genericControllers/Makefile
pkg/trunk/controllers/genericControllers/bin/
pkg/trunk/controllers/genericControllers/include/
pkg/trunk/controllers/genericControllers/include/genericControllers/
pkg/trunk/controllers/genericControllers/lib/
pkg/trunk/controllers/genericControllers/manifest.xml
pkg/trunk/controllers/genericControllers/nodes/
pkg/trunk/controllers/genericControllers/src/
pkg/trunk/controllers/genericControllers/test/
pkg/trunk/controllers/pr2Controllers/
pkg/trunk/controllers/pr2Controllers/Makefile
pkg/trunk/controllers/pr2Controllers/bin/
pkg/trunk/controllers/pr2Controllers/include/
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/Controller.h
pkg/trunk/controllers/pr2Controllers/lib/
pkg/trunk/controllers/pr2Controllers/manifest.xml
pkg/trunk/controllers/pr2Controllers/src/
pkg/trunk/controllers/pr2Controllers/src/.Controller.cpp.swp
pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp
pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp
pkg/trunk/controllers/pr2Controllers/src/Controller.cpp
pkg/trunk/controllers/pr2Controllers/src/GripperController.cpp
pkg/trunk/controllers/pr2Controllers/src/HeadController.cpp
pkg/trunk/controllers/pr2Controllers/src/JointController.cpp
pkg/trunk/controllers/pr2Controllers/src/LaserScannerController.cpp
pkg/trunk/controllers/pr2Controllers/src/SpineController.cpp
Removed Paths:
-------------
pkg/trunk/controllers/Makefile
pkg/trunk/controllers/bin/
pkg/trunk/controllers/genericControllers/include/controllers/
pkg/trunk/controllers/genericControllers/src/Makefile
pkg/trunk/controllers/include/
pkg/trunk/controllers/lib/
pkg/trunk/controllers/manifest.xml
pkg/trunk/controllers/nodes/
pkg/trunk/controllers/src/
pkg/trunk/controllers/test/
Deleted: pkg/trunk/controllers/Makefile
===================================================================
--- pkg/trunk/controllers/Makefile 2008-06-27 22:22:39 UTC (rev 1060)
+++ pkg/trunk/controllers/Makefile 2008-06-27 22:24:07 UTC (rev 1061)
@@ -1,3 +0,0 @@
-SUBDIRS = src
-include $(shell rospack find mk)/recurse_subdirs.mk
-
Copied: pkg/trunk/controllers/genericControllers/Makefile (from rev 1057, pkg/trunk/controllers/Makefile)
===================================================================
--- pkg/trunk/controllers/genericControllers/Makefile (rev 0)
+++ pkg/trunk/controllers/genericControllers/Makefile 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,5 @@
+SRC = src/Pid.cpp
+OUT = lib/libPid.a
+PKG = genericControllers
+include $(shell rospack find mk)/lib.mk
+
Copied: pkg/trunk/controllers/genericControllers/bin (from rev 1057, pkg/trunk/controllers/bin)
Copied: pkg/trunk/controllers/genericControllers/include (from rev 1057, pkg/trunk/controllers/include)
Copied: pkg/trunk/controllers/genericControllers/include/genericControllers (from rev 1057, pkg/trunk/controllers/include/controllers)
Copied: pkg/trunk/controllers/genericControllers/lib (from rev 1057, pkg/trunk/controllers/lib)
Copied: pkg/trunk/controllers/genericControllers/manifest.xml (from rev 1057, pkg/trunk/controllers/manifest.xml)
===================================================================
--- pkg/trunk/controllers/genericControllers/manifest.xml (rev 0)
+++ pkg/trunk/controllers/genericControllers/manifest.xml 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,13 @@
+<package>
+ <description brief='Generic Controller Library'>
+ Library for doing generic controls.
+ </description>
+ <author>Melonee Wise</author>
+ <license>BSD</license>
+ <depend package="pr2Core" />
+ <url>http://pr.willowgarage.com</url>
+ <repository>http://pr.willowgarage.com/repos</repository>
+ <export>
+ <cpp cflags='-I${prefix}/include' lflags='-L${prefix}/lib -lPid '/>
+ </export>
+</package>
Copied: pkg/trunk/controllers/genericControllers/nodes (from rev 1057, pkg/trunk/controllers/nodes)
Copied: pkg/trunk/controllers/genericControllers/src (from rev 1057, pkg/trunk/controllers/src)
Deleted: pkg/trunk/controllers/genericControllers/src/Makefile
===================================================================
--- pkg/trunk/controllers/src/Makefile 2008-06-27 17:36:32 UTC (rev 1057)
+++ pkg/trunk/controllers/genericControllers/src/Makefile 2008-06-27 22:24:07 UTC (rev 1061)
@@ -1,5 +0,0 @@
-SRC = Pid.cpp
-OUT = ../lib/libPid.a
-PKG = controllers
-include $(shell rospack find mk)/lib.mk
-
Modified: pkg/trunk/controllers/genericControllers/src/Pid.cpp
===================================================================
--- pkg/trunk/controllers/src/Pid.cpp 2008-06-27 17:36:32 UTC (rev 1057)
+++ pkg/trunk/controllers/genericControllers/src/Pid.cpp 2008-06-27 22:24:07 UTC (rev 1061)
@@ -1,4 +1,4 @@
-#include <controllers/Pid.h>
+#include <genericControllers/Pid.h>
#include<iostream>
using namespace std;
Copied: pkg/trunk/controllers/genericControllers/test (from rev 1057, pkg/trunk/controllers/test)
Deleted: pkg/trunk/controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/manifest.xml 2008-06-27 22:22:39 UTC (rev 1060)
+++ pkg/trunk/controllers/manifest.xml 2008-06-27 22:24:07 UTC (rev 1061)
@@ -1,14 +0,0 @@
-<package>
-<description brief='Controller Library'>
-
-Library for doing controls.
-
-</description>
-<author>Melonee Wise</author>
-<license>BSD</license>
-<url>http://pr.willowgarage.com</url>
-<repository>http://pr.willowgarage.com/repos</repository>
-<export>
-<cpp cflags='-I${prefix}/include' lflags='-L${prefix}/lib -lPid '/>
-</export>
-</package>
Added: pkg/trunk/controllers/pr2Controllers/Makefile
===================================================================
--- pkg/trunk/controllers/pr2Controllers/Makefile (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/Makefile 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,5 @@
+SRC = src/Controller.cpp src/BaseController.cpp src/HeadController.cpp src/SpineController.cpp src/ArmController.cpp src/LaserScannerController.cpp src/JointController.cpp src/GripperController.cpp
+OUT = lib/libPr2Controllers.a
+PKG = pr2Controllers
+include $(shell rospack find mk)/lib.mk
+
Added: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,97 @@
+#pragma once
+/***************************************************/
+/*! \brief A PR2 Base controller
+
+ This class implements controller loops for
+ PR2 Base Control
+
+*/
+/***************************************************/
+//#include <newmat10/newmat.h>
+//#include <libKinematics/ik.h>
+//#include <sys/types.h>
+//#include <stdint.h>
+//#include <string>
+//#include <libKDL/kdl_kinematics.h> // for kinematics using KDL -- util/kinematics/libKDL
+
+#include <pr2Core/pr2Core.h>
+#include <libpr2HW/pr2HW.h>
+
+
+class BaseController
+{
+ public:
+
+ /*!
+ * \brief Constructor,
+ *
+ * \param
+ */
+ BaseController();
+
+ /*!
+ * \brief Destructor of Pid class.
+ */
+ ~BaseController( );
+
+ /*!
+ * \brief Drive robot on a course
+ *
+ * Give the course in the robot frame, with theta=0 pointing forward.
+ * e.g. setting theta=0 puts robot in a car-like mode.
+ *
+ *
+ */
+ setCourse(double v , double theta);
+
+ /*!
+ * \brief Drive robot on a course
+ *
+ * Same as setCourse except the inputs are the x and y components of velocities.
+ *
+ */
+ setCourseXY(double vx, double vy);
+
+ /*!
+ * \brief Set target point in Global Frame
+ */
+ setTarget(double x,double y, double theta, double vx, double vy, double vw);
+
+ /*!
+ * \brief Set target points (trajectory list) in Global Frame
+ */
+ setTraj(int num_pts, double x[],double y[], double theta[], double vx[], double vy[], double vw[]);
+
+ /*!
+ * \brief Heading pose for the robot
+ *
+ * Robot assume a stationary rotation mode, and achieve heading in Global Frame
+ * theta=0 implies +x-axis direction,
+ * +theta implies counter-clockwise
+ *
+ */
+ setHeading(double theta);
+
+ /*!
+ * \brief Set force (linear summation of all wheels) in global frame
+ */
+ setForce(double fx, double fy);
+
+ /*!
+ * \brief Set parameters for this controller
+ *
+ * user can set maximum velocity
+ * and maximum acceleration
+ * constraints for this controller
+ *
+ * e.g. setParam('maxVel',10);
+ * or setParam('maxAcc',10);
+ *
+ */
+ setParam(string label,double value);
+
+ private:
+ PR2::PR2_CONTROL_MODE controlMode; /**< Base controller control mode >*/
+};
+
+
Added: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/Controller.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/Controller.h (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/Controller.h 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,27 @@
+#pragma once
+/***************************************************/
+/*! \brief A PR2 Base controller
+
+ This class implements controller loops for
+ PR2 Base Control
+
+*/
+/***************************************************/
+
+#include <pr2Core/pr2Core.h>
+#include <libpr2HW/pr2HW.h>
+
+#include <genericControllers/Pid.h>
+
+class Controller
+{
+ public:
+ Controller();
+ ~Controller();
+
+ private:
+
+
+
+};
+
Added: pkg/trunk/controllers/pr2Controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2Controllers/manifest.xml (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/manifest.xml 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,15 @@
+<package>
+ <description brief='PR2 Specific Controller Library'>
+ Library for controlling PR2 main modules. Namely:
+ Base, Head, Spine, Arm and LaserScanner, with base Joint Controllers as the base class.
+ Also, a controller for the PR2 Gripper.
+ </description>
+ <author>Melonee Wise</author>
+ <license>BSD</license>
+ <depend package="pr2Core" />
+ <url>http://pr.willowgarage.com</url>
+ <repository>http://pr.willowgarage.com/repos</repository>
+ <export>
+ <cpp cflags='-I${prefix}/include' lflags='-L${prefix}/lib -lPr2Controllers'/>
+ </export>
+</package>
Added: pkg/trunk/controllers/pr2Controllers/src/.Controller.cpp.swp
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/controllers/pr2Controllers/src/.Controller.cpp.swp
___________________________________________________________________
Name: svn:mime-type
+ application/octet-stream
Added: pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,5 @@
+
+#include <controller/ArmController.h>
+
+
+
Added: pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,5 @@
+
+#include <controller/BaseController.h>
+
+
+
Added: pkg/trunk/controllers/pr2Controllers/src/Controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/Controller.cpp (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/src/Controller.cpp 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,22 @@
+
+#include <pr2Controllers/Controller.h>
+
+/*
+ *
+ * Base Class for PR2 Controllers
+ *
+ */
+
+Controller::Controller()
+{
+
+}
+
+Controller::~Controller()
+{
+
+}
+
+
+
+
Added: pkg/trunk/controllers/pr2Controllers/src/GripperController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/GripperController.cpp (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/src/GripperController.cpp 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,5 @@
+
+#include <controller/GripperController.h>
+
+
+
Added: pkg/trunk/controllers/pr2Controllers/src/HeadController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/HeadController.cpp (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/src/HeadController.cpp 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,5 @@
+
+#include <controller/HeadController.h>
+
+
+
Added: pkg/trunk/controllers/pr2Controllers/src/JointController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/JointController.cpp (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/src/JointController.cpp 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,5 @@
+
+#include <controller/JointController.h>
+
+
+
Added: pkg/trunk/controllers/pr2Controllers/src/LaserScannerController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/LaserScannerController.cpp (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/src/LaserScannerController.cpp 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,5 @@
+
+#include <controller/LaserScannerController.h>
+
+
+
Added: pkg/trunk/controllers/pr2Controllers/src/SpineController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/SpineController.cpp (rev 0)
+++ pkg/trunk/controllers/pr2Controllers/src/SpineController.cpp 2008-06-27 22:24:07 UTC (rev 1061)
@@ -0,0 +1,5 @@
+
+#include <controller/SpineController.h>
+
+
+
Modified: pkg/trunk/simulators/gazebo_plugin/Makefile
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/Makefile 2008-06-27 22:22:39 UTC (rev 1060)
+++ pkg/trunk/simulators/gazebo_plugin/Makefile 2008-06-27 22:24:07 UTC (rev 1061)
@@ -9,7 +9,7 @@
LDFLAGS = $(shell rospack export/cpp/lflags gazebo) \
$(shell rospack export/cpp/lflags opende) \
$(shell rospack export/cpp/lflags pr2Core) \
- $(shell rospack export/cpp/lflags controllers) \
+ $(shell rospack export/cpp/lflags genericControllers) \
$(shell rospack export/cpp/lflags newmat10)
LIB_ACT = lib/libPr2_Actarray.a
Modified: pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-06-27 22:22:39 UTC (rev 1060)
+++ pkg/trunk/simulators/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-06-27 22:24:07 UTC (rev 1061)
@@ -32,7 +32,7 @@
#include <pr2Core/pr2Core.h>
#include <vector>
-#include <controllers/Pid.h>
+#include <genericControllers/Pid.h>
namespace gazebo
{
Modified: pkg/trunk/simulators/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/manifest.xml 2008-06-27 22:22:39 UTC (rev 1060)
+++ pkg/trunk/simulators/gazebo_plugin/manifest.xml 2008-06-27 22:24:07 UTC (rev 1061)
@@ -12,7 +12,7 @@
<depend package="opende"/>
<depend package="pr2Core"/>
<depend package="newmat10"/>
-<depend package="controllers"/>
+<depend package="genericControllers"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lPr2_Actarray -lPr2_Gripper -lP3D"/>
</export>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-01 18:06:46
|
Revision: 1140
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1140&view=rev
Author: hsujohnhsu
Date: 2008-07-01 11:06:44 -0700 (Tue, 01 Jul 2008)
Log Message:
-----------
update updateRate control for sensors and controllers.
sensors are not enabled unless an Iface is opened.
gazebo patch updated. please recompile gazebo.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/demos/2dnav-gazebo/world/pr2.model
pkg/trunk/demos/2dnav-gazebo/world/robot.world
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-07-01 17:50:06 UTC (rev 1139)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-07-01 18:06:44 UTC (rev 1140)
@@ -1,6 +1,6 @@
Index: gazebo-svn/player/SConscript
===================================================================
---- gazebo-svn/player/SConscript (revision 6696)
+--- gazebo-svn/player/SConscript (revision 6715)
+++ gazebo-svn/player/SConscript (working copy)
@@ -1,7 +1,8 @@
import os
@@ -14,7 +14,7 @@
'GazeboClient.cc',
Index: gazebo-svn/libgazebo/Server.cc
===================================================================
---- gazebo-svn/libgazebo/Server.cc (revision 6696)
+--- gazebo-svn/libgazebo/Server.cc (revision 6715)
+++ gazebo-svn/libgazebo/Server.cc (working copy)
@@ -38,6 +38,7 @@
#include <sys/sem.h>
@@ -103,7 +103,7 @@
{
Index: gazebo-svn/libgazebo/Iface.cc
===================================================================
---- gazebo-svn/libgazebo/Iface.cc (revision 6696)
+--- gazebo-svn/libgazebo/Iface.cc (revision 6715)
+++ gazebo-svn/libgazebo/Iface.cc (working copy)
@@ -55,6 +55,8 @@
GZ_REGISTER_IFACE("factory", FactoryIface);
@@ -116,7 +116,7 @@
Index: gazebo-svn/libgazebo/gazebo.h
===================================================================
---- gazebo-svn/libgazebo/gazebo.h (revision 6696)
+--- gazebo-svn/libgazebo/gazebo.h (revision 6715)
+++ gazebo-svn/libgazebo/gazebo.h (working copy)
@@ -550,7 +550,7 @@
@@ -127,8 +127,80 @@
/// \brief Camera interface data
class CameraData
-@@ -1057,6 +1057,30 @@
+@@ -578,6 +578,9 @@
+ /// Pose of the camera
+ public: Pose camera_pose;
+
++ /// Is camera stream opened?
++ public: bool opened;
++
+ };
+ /// \brief The camera interface
+@@ -596,6 +599,7 @@
+ {
+ Iface::Create(server,id);
+ this->data = (CameraData*)this->mMap;
++ this->data->opened=false;
+ }
+
+ /// \brief Open an existing interface
+@@ -605,8 +609,18 @@
+ {
+ Iface::Open(client,id);
+ this->data = (CameraData*)this->mMap;
++ this->data->opened=true;
+ }
+
++ /// \brief Close an existing interface
++ /// \param client Pointer to the client
++ /// \param id Id of the interface
++ public: virtual void Close()
++ {
++ Iface::Close();
++ this->data->opened=false;
++ }
++
+ /// Pointer to the camera data
+ public: CameraData *data;
+ };
+@@ -825,6 +839,9 @@
+
+ /// Commaned range count
+ public: int cmd_range_count;
++
++ /// is laser interface opened?
++ public: bool opened;
+ };
+
+ /// \brief Laser interface
+@@ -843,6 +860,7 @@
+ {
+ Iface::Create(server,id);
+ this->data = (LaserData*)this->mMap;
++ this->data->opened=false;
+ }
+
+ /// \brief Open an existing interface
+@@ -852,8 +870,16 @@
+ {
+ Iface::Open(client,id);
+ this->data = (LaserData*)this->mMap;
++ this->data->opened=true;
+ }
+
++ /// \brief Close an existing interface
++ public: virtual void Close()
++ {
++ Iface::Close();
++ this->data->opened=false;
++ }
++
+ /// Pointer to the laser data
+ public: LaserData *data;
+ };
+@@ -1057,6 +1083,30 @@
+
/// Lift down flag
public: int lift_down;
+
@@ -158,7 +230,7 @@
};
/// \brief Gripper interface
-@@ -1254,9 +1278,327 @@
+@@ -1254,9 +1304,327 @@
/// \} */
@@ -486,9 +558,53 @@
/** \defgroup ptz_iface ptz
\brief PTZ interface
+@@ -1334,7 +1702,7 @@
+ \{
+ */
+
+-#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 640 * 480 * 3
++#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 640 * 480 * 9
+ #define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 640 * 480
+
+ /// \brief Stereo data
+@@ -1384,6 +1752,9 @@
+ /// Right disparity (float)
+ public: float right_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE];
+
++ /// Is camera stream opened?
++ public: bool opened;
++
+ };
+
+
+@@ -1401,6 +1772,7 @@
+ {
+ Iface::Create(server,id);
+ this->data = (StereoCameraData*)this->mMap;
++ this->data->opened=false;
+ }
+
+ /// \brief Open the iface
+@@ -1408,8 +1780,16 @@
+ {
+ Iface::Open(client,id);
+ this->data = (StereoCameraData*)this->mMap;
++ this->data->opened=true;
+ }
+
++ /// \brief Close the iface
++ public: virtual void Close()
++ {
++ Iface::Close();
++ this->data->opened=false;
++ }
++
+ /// Pointer to the stereo data
+ public: StereoCameraData *data;
+ };
Index: gazebo-svn/server/physics/HingeJoint.cc
===================================================================
---- gazebo-svn/server/physics/HingeJoint.cc (revision 6696)
+--- gazebo-svn/server/physics/HingeJoint.cc (revision 6715)
+++ gazebo-svn/server/physics/HingeJoint.cc (working copy)
@@ -38,6 +38,7 @@
: Joint()
@@ -500,7 +616,7 @@
Index: gazebo-svn/server/physics/SliderJoint.cc
===================================================================
---- gazebo-svn/server/physics/SliderJoint.cc (revision 6696)
+--- gazebo-svn/server/physics/SliderJoint.cc (revision 6715)
+++ gazebo-svn/server/physics/SliderJoint.cc (working copy)
@@ -35,6 +35,8 @@
: Joint()
@@ -511,9 +627,355 @@
}
+Index: gazebo-svn/server/sensors/Sensor.hh
+===================================================================
+--- gazebo-svn/server/sensors/Sensor.hh (revision 6715)
++++ gazebo-svn/server/sensors/Sensor.hh (working copy)
+@@ -63,6 +63,7 @@
+
+ /// \brief Set whether the sensor is active or not
+ public: void SetActive(bool value);
++ public: bool IsActive();
+
+ /// \brief Load the child sensor
+ protected: virtual void LoadChild(XMLConfigNode * /*node*/) {};
+@@ -88,8 +89,14 @@
+
+ /// \brief True if active
+ protected: bool active;
++
++ /// \brief Update period
++ protected: double updatePeriod;
++
++ /// \brief Last update time
++ protected: double lastUpdate;
++
+ };
+-
+ /// \}
+ }
+ #endif
+Index: gazebo-svn/server/sensors/camera/MonoCameraSensor.cc
+===================================================================
+--- gazebo-svn/server/sensors/camera/MonoCameraSensor.cc (revision 6715)
++++ gazebo-svn/server/sensors/camera/MonoCameraSensor.cc (working copy)
+@@ -49,6 +49,7 @@
+ MonoCameraSensor::MonoCameraSensor(Body *body)
+ : Sensor(body), OgreCamera("Mono")
+ {
++ this->active = false;
+ }
+
+
+@@ -120,46 +121,50 @@
+ // Update the drawing
+ void MonoCameraSensor::UpdateChild()
+ {
+- this->UpdateCam();
+
+- this->renderTarget->update();
+-
+- Ogre::HardwarePixelBufferSharedPtr mBuffer;
++ // Allocate buffer
+ size_t size;
+-
+- // Get access to the buffer and make an image and write it to file
+- mBuffer = this->renderTexture->getBuffer(0, 0);
+-
+ size = this->imageWidth * this->imageHeight * 3;
+-
+- // Allocate buffer
+ if (!this->saveFrameBuffer)
+ this->saveFrameBuffer = new unsigned char[size];
+
+- mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY);
++ if (this->active)
++ {
++ this->UpdateCam();
+
+- int top = (int)((mBuffer->getHeight() - this->imageHeight) / 2.0);
+- int left = (int)((mBuffer->getWidth() - this->imageWidth) / 2.0);
+- int right = left + this->imageWidth;
+- int bottom = top + this->imageHeight;
++ this->renderTarget->update();
+
+- // Get the center of the texture in RGB 24 bit format
+- mBuffer->blitToMemory(
+- Ogre::Box(left, top, right, bottom),
++ Ogre::HardwarePixelBufferSharedPtr mBuffer;
+
+- Ogre::PixelBox(
+- this->imageWidth,
+- this->imageHeight,
+- 1,
+- Ogre::PF_B8G8R8,
+- this->saveFrameBuffer)
+- );
++ // Get access to the buffer and make an image and write it to file
++ mBuffer = this->renderTexture->getBuffer(0, 0);
+
+- mBuffer->unlock();
+
++ mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY);
+
+- if (this->saveFrames)
+- this->SaveFrame();
++ int top = (int)((mBuffer->getHeight() - this->imageHeight) / 2.0);
++ int left = (int)((mBuffer->getWidth() - this->imageWidth) / 2.0);
++ int right = left + this->imageWidth;
++ int bottom = top + this->imageHeight;
++
++ // Get the center of the texture in RGB 24 bit format
++ mBuffer->blitToMemory(
++ Ogre::Box(left, top, right, bottom),
++
++ Ogre::PixelBox(
++ this->imageWidth,
++ this->imageHeight,
++ 1,
++ Ogre::PF_B8G8R8,
++ this->saveFrameBuffer)
++ );
++
++ mBuffer->unlock();
++
++
++ if (this->saveFrames)
++ this->SaveFrame();
++ }
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+Index: gazebo-svn/server/sensors/camera/StereoCameraSensor.cc
+===================================================================
+--- gazebo-svn/server/sensors/camera/StereoCameraSensor.cc (revision 6715)
++++ gazebo-svn/server/sensors/camera/StereoCameraSensor.cc (working copy)
+@@ -58,6 +58,7 @@
+ this->depthBuffer[1] = NULL;
+ this->rgbBuffer[0] = NULL;
+ this->rgbBuffer[1] = NULL;
++ this->active = false;
+ }
+
+
+@@ -195,87 +196,90 @@
+ Ogre::SceneNode *gridNode = NULL;
+ int i;
+
+- this->UpdateCam();
+-
+- try
++ if (this->active)
+ {
+- gridNode = sceneMgr->getSceneNode("__OGRE_GRID_NODE__");
+- }
+- catch (...)
+- {
+- gridNode = NULL;
+- }
++ this->UpdateCam();
+
+- sceneMgr->_suppressRenderStateChanges(true);
++ try
++ {
++ gridNode = sceneMgr->getSceneNode("__OGRE_GRID_NODE__");
++ }
++ catch (...)
++ {
++ gridNode = NULL;
++ }
+
+- //prev_pass = sceneMgr->getPass();
++ sceneMgr->_suppressRenderStateChanges(true);
+
+- // Get pointer to the material pass
+- pass = this->depthMaterial->getBestTechnique()->getPass(0);
++ //prev_pass = sceneMgr->getPass();
+
+- if (gridNode)
+- gridNode->setVisible(false);
++ // Get pointer to the material pass
++ pass = this->depthMaterial->getBestTechnique()->getPass(0);
+
+- // Render the depth texture
+- for (i=2; i<4; i++)
+- {
++ if (gridNode)
++ gridNode->setVisible(false);
+
+- // OgreSceneManager::_render function automatically sets farClip to 0.
+- // Which normally equates to infinite distance. We don't want this. So
+- // we have to set the distance every time.
+- this->GetOgreCamera()->setFarClipDistance( this->farClip );
++ // Render the depth texture
++ for (i=2; i<4; i++)
++ {
+
++ // OgreSceneManager::_render function automatically sets farClip to 0.
++ // Which normally equates to infinite distance. We don't want this. So
++ // we have to set the distance every time.
++ this->GetOgreCamera()->setFarClipDistance( this->farClip );
+
+- Ogre::AutoParamDataSource autoParamDataSource;
+
+- vp = this->renderTargets[i]->getViewport(0);
++ Ogre::AutoParamDataSource autoParamDataSource;
+
+- // Need this line to render the ground plane. No idea why it's necessary.
+- renderSys->_setViewport(vp);
+- sceneMgr->_setPass(pass, true, false);
+- autoParamDataSource.setCurrentPass(pass);
+- autoParamDataSource.setCurrentViewport(vp);
+- autoParamDataSource.setCurrentRenderTarget(this->renderTargets[i]);
+- autoParamDataSource.setCurrentSceneManager(sceneMgr);
+- autoParamDataSource.setCurrentCamera(this->GetOgreCamera());
+- pass->_updateAutoParamsNoLights(autoParamDataSource);
++ vp = this->renderTargets[i]->getViewport(0);
+
+- // These two lines don't seem to do anything useful
+- renderSys->_setProjectionMatrix(this->GetOgreCamera()->getProjectionMatrixRS());
+- renderSys->_setViewMatrix(this->GetOgreCamera()->getViewMatrix(true));
++ // Need this line to render the ground plane. No idea why it's necessary.
++ renderSys->_setViewport(vp);
++ sceneMgr->_setPass(pass, true, false);
++ autoParamDataSource.setCurrentPass(pass);
++ autoParamDataSource.setCurrentViewport(vp);
++ autoParamDataSource.setCurrentRenderTarget(this->renderTargets[i]);
++ autoParamDataSource.setCurrentSceneManager(sceneMgr);
++ autoParamDataSource.setCurrentCamera(this->GetOgreCamera());
++ pass->_updateAutoParamsNoLights(autoParamDataSource);
+
+- // NOTE: We MUST bind parameters AFTER updating the autos
+- if (pass->hasVertexProgram())
++ // These two lines don't seem to do anything useful
++ renderSys->_setProjectionMatrix(this->GetOgreCamera()->getProjectionMatrixRS());
++ renderSys->_setViewMatrix(this->GetOgreCamera()->getViewMatrix(true));
++
++ // NOTE: We MUST bind parameters AFTER updating the autos
++ if (pass->hasVertexProgram())
++ {
++ renderSys->bindGpuProgram( pass->getVertexProgram()->_getBindingDelegate() );
++ renderSys->bindGpuProgramParameters(Ogre::GPT_VERTEX_PROGRAM,
++ pass->getVertexProgramParameters());
++ }
++
++ if (pass->hasFragmentProgram())
++ {
++ renderSys->bindGpuProgram( pass->getFragmentProgram()->_getBindingDelegate() );
++ renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM,
++ pass->getFragmentProgramParameters());
++ }
++ this->renderTargets[i]->update();
++ }
++
++ sceneMgr->_suppressRenderStateChanges(false);
++
++ // Render the image texture
++ for (i=0; i<2; i++)
+ {
+- renderSys->bindGpuProgram( pass->getVertexProgram()->_getBindingDelegate() );
+- renderSys->bindGpuProgramParameters(Ogre::GPT_VERTEX_PROGRAM,
+- pass->getVertexProgramParameters());
++ this->renderTargets[i]->update();
+ }
+
+- if (pass->hasFragmentProgram())
+- {
+- renderSys->bindGpuProgram( pass->getFragmentProgram()->_getBindingDelegate() );
+- renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM,
+- pass->getFragmentProgramParameters());
+- }
+- this->renderTargets[i]->update();
+- }
++ if (gridNode)
++ gridNode->setVisible(true);
+
+- sceneMgr->_suppressRenderStateChanges(false);
++ this->FillBuffers();
+
+- // Render the image texture
+- for (i=0; i<2; i++)
+- {
+- this->renderTargets[i]->update();
++ if (this->saveFrames)
++ this->SaveFrame();
+ }
+-
+- if (gridNode)
+- gridNode->setVisible(true);
+-
+- this->FillBuffers();
+-
+- if (this->saveFrames)
+- this->SaveFrame();
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+Index: gazebo-svn/server/sensors/ray/RaySensor.cc
+===================================================================
+--- gazebo-svn/server/sensors/ray/RaySensor.cc (revision 6715)
++++ gazebo-svn/server/sensors/ray/RaySensor.cc (working copy)
+@@ -237,7 +237,7 @@
+ // Update the sensor information
+ void RaySensor::UpdateChild()
+ {
+-// if (this->active)
++ if (this->active)
+ {
+ std::vector<RayGeom*>::iterator iter;
+ Pose3d poseDelta;
+Index: gazebo-svn/server/sensors/Sensor.cc
+===================================================================
+--- gazebo-svn/server/sensors/Sensor.cc (revision 6715)
++++ gazebo-svn/server/sensors/Sensor.cc (working copy)
+@@ -32,6 +32,7 @@
+ #include "World.hh"
+ #include "ControllerFactory.hh"
+ #include "Sensor.hh"
++#include "Simulator.hh"
+
+ using namespace gazebo;
+
+@@ -58,6 +59,10 @@
+ this->SetName(node->GetString("name","",1));
+ this->LoadController( node->GetChildByNSPrefix("controller") );
+ this->LoadChild(node);
++
++ this->updatePeriod = 1.0 / (node->GetDouble("updateRate", 10) + 1e-6);
++ this->lastUpdate = -1e6;
++
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -71,7 +76,12 @@
+ /// Update the sensor
+ void Sensor::Update()
+ {
+- this->UpdateChild();
++ if (this->lastUpdate + updatePeriod <= Simulator::Instance()->GetSimTime())
++ {
++ this->UpdateChild();
++ this->lastUpdate = Simulator::Instance()->GetSimTime();
++ }
++ // controller has its own updatRate control
+ if (this->controller)
+ this->controller->Update();
+ }
+@@ -147,4 +157,11 @@
+ this->active = value;
+ }
+
++////////////////////////////////////////////////////////////////////////////////
++/// \brief Set whether the sensor is active or not
++bool Sensor::IsActive()
++{
++ return this->active;
++}
+
++
Index: gazebo-svn/server/GazeboConfig.cc
===================================================================
---- gazebo-svn/server/GazeboConfig.cc (revision 6696)
+--- gazebo-svn/server/GazeboConfig.cc (revision 6715)
+++ gazebo-svn/server/GazeboConfig.cc (working copy)
@@ -56,6 +56,17 @@
@@ -535,7 +997,7 @@
XMLConfig rc;
Index: gazebo-svn/server/gui/GLWindow.cc
===================================================================
---- gazebo-svn/server/gui/GLWindow.cc (revision 6696)
+--- gazebo-svn/server/gui/GLWindow.cc (revision 6715)
+++ gazebo-svn/server/gui/GLWindow.cc (working copy)
@@ -66,7 +66,11 @@
this->directionVec.x = 0;
@@ -602,7 +1064,7 @@
this->moveAmount *= 2;
Index: gazebo-svn/server/World.hh
===================================================================
---- gazebo-svn/server/World.hh (revision 6696)
+--- gazebo-svn/server/World.hh (revision 6715)
+++ gazebo-svn/server/World.hh (working copy)
@@ -91,6 +91,26 @@
/// \return Pointer to the physics engine
@@ -641,9 +1103,166 @@
private: friend class DestroyerT<World>;
private: friend class SingletonT<World>;
};
+Index: gazebo-svn/server/controllers/camera/stereo/Stereo_Camera.cc
+===================================================================
+--- gazebo-svn/server/controllers/camera/stereo/Stereo_Camera.cc (revision 6715)
++++ gazebo-svn/server/controllers/camera/stereo/Stereo_Camera.cc (working copy)
+@@ -87,26 +87,59 @@
+ // Update the controller
+ void Stereo_Camera::UpdateChild()
+ {
+- if (this->cameraIface)
++ bool cameraOpen = false;
++ bool stereoOpen = false;
++
++
++ // do this first so there's chance for sensor to run 1 frame after activate
++ if (this->myParent->IsActive())
+ {
+- this->cameraIface->Lock(1);
+- if (this->cameraIface->data->head.openCount > 0)
+- this->PutCameraData();
+- this->cameraIface->Unlock();
++ if (this->cameraIface)
++ {
++ this->cameraIface->Lock(1);
++ if (this->cameraIface->data->head.openCount > 0)
++ this->PutCameraData();
++ this->cameraIface->Unlock();
+
+- // New data is available
+- this->cameraIface->Post();
++ // New data is available
++ this->cameraIface->Post();
++ }
++
++ if (this->stereoIface)
++ {
++ this->stereoIface->Lock(1);
++ if (this->stereoIface->data->head.openCount > 0)
++ this->PutStereoData();
++ this->stereoIface->Unlock();
++
++ this->stereoIface->Post();
++ }
+ }
+
+- if (this->stereoIface)
++
++ // activate if iface open
++ if (this->cameraIface->Lock(1))
+ {
+- this->stereoIface->Lock(1);
+- if (this->stereoIface->data->head.openCount > 0)
+- this->PutStereoData();
++ cameraOpen = (this->cameraIface->GetOpenCount() > 0);
++ //std::cout << " stereo camera open count " << this->cameraIface->GetOpenCount() << std::endl;
++ this->cameraIface->Unlock();
++ }
++
++ if (this->stereoIface->Lock(1))
++ {
++ stereoOpen = (this->stereoIface->GetOpenCount() > 0);
++ //std::cout << " stereo depth open count " << this->stereoIface->GetOpenCount() << std::endl;
+ this->stereoIface->Unlock();
++ }
+
+- this->stereoIface->Post();
+- }
++ if (cameraOpen || stereoOpen)
++ this->myParent->SetActive(true);
++ else
++ this->myParent->SetActive(false);
++
++ //std::cout << " stereo active " << this->myParent->IsActive() << std::endl;
++
++
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+Index: gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc
+===================================================================
+--- gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc (revision 6715)
++++ gazebo-svn/server/controllers/camera/generic/Generic_Camera.cc (working copy)
+@@ -79,7 +79,24 @@
+ // Update the controller
+ void Generic_Camera::UpdateChild()
+ {
+- this->PutCameraData();
++
++ // do this first so there's chance for sensor to run 1 frame after activate
++ if (this->myParent->IsActive())
++ this->PutCameraData();
++
++ // activate if iface open
++ if (this->cameraIface->Lock(1))
++ {
++ if (this->cameraIface->GetOpenCount() > 0)
++ this->myParent->SetActive(true);
++ else
++ this->myParent->SetActive(false);
++
++ //std::cout << " camera open count " << this->cameraIface->GetOpenCount() << std::endl;
++ this->cameraIface->Unlock();
++ }
++ //std::cout << " camera active " << this->myParent->IsActive() << std::endl;
++
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+Index: gazebo-svn/server/controllers/ControllerFactory.cc
+===================================================================
+--- gazebo-svn/server/controllers/ControllerFactory.cc (revision 6715)
++++ gazebo-svn/server/controllers/ControllerFactory.cc (working copy)
+@@ -42,6 +42,7 @@
+ // Register a controller class. Use by dynamically loaded modules
+ void ControllerFactory::RegisterController(std::string type, std::string classname, ControllerFactoryFn factoryfn)
+ {
++ std::cout << " controllers " << classname << " registered " << std::endl;
+ controllers[classname] = factoryfn;
+ }
+
+Index: gazebo-svn/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
+===================================================================
+--- gazebo-svn/server/controllers/laser/sicklms200/SickLMS200_Laser.cc (revision 6715)
++++ gazebo-svn/server/controllers/laser/sicklms200/SickLMS200_Laser.cc (working copy)
+@@ -98,6 +98,7 @@
+ if (this->laserIface->Lock(1))
+ {
+ laserOpened = this->laserIface->GetOpenCount() > 0;
++ //std::cout << " laser open count " << this->laserIface->GetOpenCount() << std::endl;
+ this->laserIface->Unlock();
+ }
+
+@@ -123,6 +124,8 @@
+ {
+ this->myParent->SetActive(false);
+ }
++ //std::cout << " active " << this->myParent->IsActive() << std::endl;
++
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+Index: gazebo-svn/server/controllers/Controller.cc
+===================================================================
+--- gazebo-svn/server/controllers/Controller.cc (revision 6715)
++++ gazebo-svn/server/controllers/Controller.cc (working copy)
+@@ -141,10 +141,10 @@
+ /// Update the controller. Called every cycle.
+ void Controller::Update()
+ {
+- if (lastUpdate + updatePeriod <= Simulator::Instance()->GetSimTime())
++ if (this->lastUpdate + updatePeriod <= Simulator::Instance()->GetSimTime())
+ {
+ this->UpdateChild();
+- lastUpdate = Simulator::Instance()->GetSimTime();
++ this->lastUpdate = Simulator::Instance()->GetSimTime();
+ }
+ }
+
Index: gazebo-svn/server/World.cc
===================================================================
---- gazebo-svn/server/World.cc (revision 6696)
+--- gazebo-svn/server/World.cc (revision 6715)
+++ gazebo-svn/server/World.cc (working copy)
@@ -27,6 +27,7 @@
#include <assert.h>
@@ -735,7 +1354,7 @@
int World::LoadEntities(XMLConfigNode *node, Model *parent)
Index: gazebo-svn/SConstruct
===================================================================
---- gazebo-svn/SConstruct (revision 6696)
+--- gazebo-svn/SConstruct (revision 6715)
+++ gazebo-svn/SConstruct (working copy)
@@ -24,6 +24,8 @@
parseConfigs=['pkg-config --cflags --libs OGRE',
@@ -746,3 +1365,20 @@
'fltk-config --cflags --libs --ldflags --use-gl --use-images',
'pkg-config --cflags --libs xft'
]
+Index: gazebo-svn/worlds/stereocamera.world
+===================================================================
+--- gazebo-svn/worlds/stereocamera.world (revision 6715)
++++ gazebo-svn/worlds/stereocamera.world (working copy)
+@@ -105,10 +105,10 @@
+ <saveFramePath>frames</saveFramePath>
+ <baseline>0.2</baseline>
+
+- <controller:stereo_camera name="stereo_camera_controller">
++ <controller:stereocamera name="stereo_camera_controller">
+ <interface:stereocamera name="stereo_iface_0" />
+ <interface:camera name="camera_iface_0" />
+- </controller:stereo_camera>
++ </controller:stereocamera>
+ </sensor:stereocamera>
+ </body:empty>
+ </model:physical>
Modified: pkg/trunk/demos/2dnav-gazebo/world/pr2.model
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-07-01 17:50:06 UTC (rev 1139)
+++ pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-07-01 18:06:44 UTC (rev 1140)
@@ -131,6 +131,7 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
+ <updateRate>10.0</updateRate>
<controller:sicklms200_laser name="base_laser_controller_1">
<interface:laser name="base_laser_iface_1"/>
<updateRate>10</updateRate>
@@ -312,6 +313,7 @@
<hfov>60</hfov>
<nearClip>0.1</nearClip>
<farClip>100</farClip>
+ <updateRate>15.0</updateRate>
<controller:generic_camera name="head_cam_left_controller">
<updateRate>15.0</updateRate>
<interface:camera name="head_cam_left_iface_0" />
@@ -356,6 +358,7 @@
<hfov>60</hfov>
<nearClip>0.1</nearClip>
<farClip>100</farClip>
+ <updateRate>15.0</updateRate>
<controller:generic_camera name="head_cam_right_controller">
<updateRate>15.0</updateRate>
<interface:camera name="head_cam_right_iface_0" />
@@ -430,13 +433,12 @@
<saveFrames>false</saveFrames>
<saveFramePath>frames</saveFramePath>
<baseline>0.2</baseline>
-
+ <updateRate>15.0</updateRate>
<controller:stereocamera name="stereo_camera_controller">
<updateRate>15.0</updateRate>
<interface:stereocamera name="stereo_iface_0" />
<interface:camera name="camera_iface_0" />
</controller:stereocamera>
-
</sensor:stereocamera>
<geom:box name="stereo_camera_geom">
<xyz> 0.00 0.00 0.00</xyz>
@@ -1417,6 +1419,7 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
+ <updateRate>10</updateRate>
<controller:sicklms200_laser name="laser_controller_1">
<interface:laser name="laser_iface_1"/>
<updateRate>10</updateRate>
Modified: pkg/trunk/demos/2dnav-gazebo/world/robot.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-07-01 17:50:06 UTC (rev 1139)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-07-01 18:06:44 UTC (rev 1140)
@@ -47,7 +47,7 @@
</sky>
<gazeboPath>media</gazeboPath>
<grid>off</grid>
- <desiredFPS>10</desiredFPS>
+ <maxUpdateRate>100</maxUpdateRate>
</rendering:ogre>
@@ -95,6 +95,7 @@
<farClip>20</farClip>
<saveFrames>false</saveFrames>
<saveFramePath>frames</saveFramePath>
+ <updateRate>10.0</updateRate>
<controller:generic_camera name="cam1_controller">
<updateRate>10.0</updateRate>
<interface:camera name="cam1_iface_0" />
Modified: pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-07-01 17:50:06 UTC (rev 1139)
+++ pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-07-01 18:06:44 UTC (rev 1140)
@@ -310,44 +310,44 @@
cmdSpeed = this->myIface->data->actuators[count].cmdSpeed;
switch(this->myIface->data->actuators[count].controlMode)
{
- case PR2::TORQUE_CONTROL:
- printf("Hinge Torque Control\n");
- hjoint->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce);
- //std::cout << count << " " << this->myIface->data->actuators[count].controlMode << std::endl;
- break;
- // case PR2::PD_CONTROL1 :
- // // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
- // positionError = ModNPi2Pi(cmdPosition - hjoint->GetAngle());
- // speedError = cmdSpeed - hjoint->GetAngleRate();
- // currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
- // std::cout << "hinge err:" << positionError << " cmd: " << currentCmd << std::endl;
- // // limit torque
- // currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
- // currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
- // hjoint->SetTorque(currentCmd);
- // break;
- case PR2::PD_CONTROL:
...
[truncated message content] |
Thanks for helping keep SourceForge clean.
X