Revision: 8270
http://playerstage.svn.sourceforge.net/playerstage/?rev=8270&view=rev
Author: natepak
Date: 2009-09-30 16:59:13 +0000 (Wed, 30 Sep 2009)
Log Message:
-----------
Fixed a problem with FreeImage in the build process. Added the beginnings of a
rovio controller
Modified Paths:
--------------
code/gazebo/trunk/CMakeLists.txt
code/gazebo/trunk/cmake/FindFreeimage.cmake
code/gazebo/trunk/server/CMakeLists.txt
code/gazebo/trunk/server/controllers/position2d/CMakeLists.txt
Added Paths:
-----------
code/gazebo/trunk/server/controllers/position2d/rovio/
code/gazebo/trunk/server/controllers/position2d/rovio/CMakeLists.txt
code/gazebo/trunk/server/controllers/position2d/rovio/Rovio_Position2d.cc
code/gazebo/trunk/server/controllers/position2d/rovio/Rovio_Position2d.hh
Removed Paths:
-------------
code/gazebo/trunk/libgazebo/SConscript
Modified: code/gazebo/trunk/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/CMakeLists.txt 2009-09-29 04:23:54 UTC (rev 8269)
+++ code/gazebo/trunk/CMakeLists.txt 2009-09-30 16:59:13 UTC (rev 8270)
@@ -31,7 +31,7 @@
SET (gazebocontroller_sources_desc "List of controller sources"
CACHE INTERNAL "Gazebo controller sources list description" FORCE)
-SET (OGRE_VERSION 1.6.1 CACHE INTERNAL "Ogre version requirement" FORCE)
+SET (OGRE_VERSION 1.6.4 CACHE INTERNAL "Ogre version requirement" FORCE)
SET (FREEIMAGE_MAJOR_VERSION 3 CACHE INTERNAL "FreeImage major version
requirement" FORCE)
SET (FREEIMAGE_MINOR_VERSION 10 CACHE INTERNAL "FreeImage minor version
requirement" FORCE)
Modified: code/gazebo/trunk/cmake/FindFreeimage.cmake
===================================================================
--- code/gazebo/trunk/cmake/FindFreeimage.cmake 2009-09-29 04:23:54 UTC (rev
8269)
+++ code/gazebo/trunk/cmake/FindFreeimage.cmake 2009-09-30 16:59:13 UTC (rev
8270)
@@ -58,4 +58,4 @@
APPEND_TO_CACHED_LIST(gazeboserver_link_libs
${gazeboserver_link_libs_desc}
${FI_LDFLAGS})
-ENDIF (NOT FI_FOUND)
\ No newline at end of file
+ENDIF (NOT FI_FOUND)
Deleted: code/gazebo/trunk/libgazebo/SConscript
===================================================================
--- code/gazebo/trunk/libgazebo/SConscript 2009-09-29 04:23:54 UTC (rev
8269)
+++ code/gazebo/trunk/libgazebo/SConscript 2009-09-30 16:59:13 UTC (rev
8270)
@@ -1,56 +0,0 @@
-#Import variable
-Import('*')
-
-env.Append(CPPPATH = '#libgazebo')
-
-#
-# Create the pkg-config file
-#
-def createPkgConfig(target,source, env):
- f = open(str(target[0]), 'wb')
- prefix = source[0].get_contents()
- f.write('prefix=' + prefix + '\n')
- f.write('Name: gazebo\n')
- f.write('Description: Simplified interface to Player\n')
- f.write('Version:' + version + '\n')
- f.write('Requires:\n')
- f.write('Libs: -L' + prefix + '/lib -lgazebo\n')
- f.write('Cflags: -I' + prefix + '/include\n')
-
-
-#
-# setup a special build environment for libgazebo. Do this so we can control
-# what libgazebo depends upon
-#
-# CC = '/opt/intel/cc/10.1.008/bin/icc',
-# CXX = '/opt/intel/cc/10.1.008/bin/icpc',
-# LD = '/opt/intel/cc/10.1.008/bin/xild',
-env = Environment (
- CC = 'g++',
- CCFLAGS = Split ('-pthread -pipe -W -Wall -O2'),
- LIBS=Split('boost_signals boost_thread'),
-
- CXXCOMSTR = 'Compiling $TARGET',
- CCCOMSTR = 'Compiling $TARGET',
-
- SHCXXCOMSTR = 'Compiling $TARGET',
- SHCCCOMSTR = 'Compiling $TARGET',
-
- SHLINKCOMSTR = 'Linking $TARGET',
- LINKCOMSTR = 'Linking $TARGET',
-
-)
-
-env['BUILDERS']['PkgConfig'] = Builder(action = createPkgConfig)
-pkgconfig = env.PkgConfig(target='libgazebo.pc', source=Value(install_prefix))
-env.Install(dir=install_prefix+'/lib/pkgconfig', source=pkgconfig)
-
-sources = Split('Server.cc Client.cc Iface.cc IfaceFactory.cc SimIface.cc
Graphics3dIface.cc')
-headers = Split('gazebo.h IfaceFactory.hh')
-
-sharedLib = myEnv.SharedLibrary('gazebo', sources)
-staticLib = myEnv.StaticLibrary('gazebo', sources)
-
-env.Install(install_prefix+'/lib', sharedLib)
-env.Install(install_prefix+'/lib', staticLib)
-env.Install(install_prefix+'/include/gazebo', headers)
Modified: code/gazebo/trunk/server/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/server/CMakeLists.txt 2009-09-29 04:23:54 UTC (rev
8269)
+++ code/gazebo/trunk/server/CMakeLists.txt 2009-09-30 16:59:13 UTC (rev
8270)
@@ -118,8 +118,8 @@
TARGET_LINK_LIBRARIES( gazebo-exec ${libtool_library}
${gazeboserver_link_libs}
${boost_libraries}
+ gazebo_server
${freeimage_library}
- gazebo_server
gazebo_av-shared
gazebo_gui-shared
gazebo
Modified: code/gazebo/trunk/server/controllers/position2d/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/server/controllers/position2d/CMakeLists.txt
2009-09-29 04:23:54 UTC (rev 8269)
+++ code/gazebo/trunk/server/controllers/position2d/CMakeLists.txt
2009-09-30 16:59:13 UTC (rev 8270)
@@ -1,3 +1,4 @@
ADD_SUBDIRECTORY(differential)
ADD_SUBDIRECTORY(holonome3sw)
ADD_SUBDIRECTORY(steering)
+ADD_SUBDIRECTORY(rovio)
Added: code/gazebo/trunk/server/controllers/position2d/rovio/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/server/controllers/position2d/rovio/CMakeLists.txt
(rev 0)
+++ code/gazebo/trunk/server/controllers/position2d/rovio/CMakeLists.txt
2009-09-30 16:59:13 UTC (rev 8270)
@@ -0,0 +1,7 @@
+include (${gazebo_cmake_dir}/GazeboUtils.cmake)
+
+SET (sources Rovio_Position2d.cc)
+SET (headers Rovio_Position2d.hh)
+
+APPEND_TO_SERVER_SOURCES(${sources})
+APPEND_TO_SERVER_HEADERS(${headers})
Added: code/gazebo/trunk/server/controllers/position2d/rovio/Rovio_Position2d.cc
===================================================================
--- code/gazebo/trunk/server/controllers/position2d/rovio/Rovio_Position2d.cc
(rev 0)
+++ code/gazebo/trunk/server/controllers/position2d/rovio/Rovio_Position2d.cc
2009-09-30 16:59:13 UTC (rev 8270)
@@ -0,0 +1,245 @@
+/*
+ * 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: Position2d controller for a Rovio drive.
+ * Author: Nathan Koenig
+ * Date: 01 Feb 2007
+ * SVN info: $Id: Rovio_Position2d.cc 8156 2009-07-30 02:33:46Z hsujohnhsu $
+ */
+
+#include "XMLConfig.hh"
+#include "Model.hh"
+#include "Global.hh"
+#include "HingeJoint.hh"
+#include "World.hh"
+#include "Simulator.hh"
+#include "gazebo.h"
+#include "GazeboError.hh"
+#include "PhysicsEngine.hh"
+#include "ControllerFactory.hh"
+#include "Rovio_Position2d.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_STATIC_CONTROLLER("rovio_position2d", Rovio_Position2d);
+
+enum {LEFT,RIGHT,BACK};
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Rovio_Position2d::Rovio_Position2d(Entity *parent )
+ : Controller(parent)
+{
+ this->myParent = dynamic_cast<Model*>(this->parent);
+
+ if (!this->myParent)
+ gzthrow("Rovio_Position2d controller requires a Model as its parent");
+
+ this->enableMotors = true;
+
+ this->wheelSpeed[RIGHT] = 0;
+ this->wheelSpeed[LEFT] = 0;
+
+ this->prevUpdateTime = Simulator::Instance()->GetSimTime();
+
+ Param::Begin(&this->parameters);
+ this->leftJointNameP = new ParamT<std::string>("leftJoint", "", 1);
+ this->rightJointNameP = new ParamT<std::string>("rightJoint", "", 1);
+ this->backJointNameP = new ParamT<std::string>("rightJoint", "", 1);
+ Param::End();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+Rovio_Position2d::~Rovio_Position2d()
+{
+ delete this->leftJointNameP;
+ delete this->rightJointNameP;
+ delete this->backJointNameP;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void Rovio_Position2d::LoadChild(XMLConfigNode *node)
+{
+ this->myIface = dynamic_cast<PositionIface*>(this->GetIface("position"));
+
+ this->leftJointNameP->Load(node);
+ this->rightJointNameP->Load(node);
+
+ this->joints[LEFT] =
dynamic_cast<HingeJoint*>(this->myParent->GetJoint(this->leftJointNameP->GetValue()));
+ this->joints[RIGHT] =
dynamic_cast<HingeJoint*>(this->myParent->GetJoint(this->rightJointNameP->GetValue()));
+ this->joints[BACK] =
dynamic_cast<HingeJoint*>(this->myParent->GetJoint(this->leftJointNameP->GetValue()));
+
+ if (!this->joints[LEFT])
+ gzthrow("The controller couldn't get left hinge joint");
+
+ if (!this->joints[RIGHT])
+ gzthrow("The controller couldn't get right hinge joint");
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Initialize the controller
+void Rovio_Position2d::InitChild()
+{
+ // Reset odometric pose
+ this->odomPose[0] = 0.0;
+ this->odomPose[1] = 0.0;
+ this->odomPose[2] = 0.0;
+
+ this->odomVel[0] = 0.0;
+ this->odomVel[1] = 0.0;
+ this->odomVel[2] = 0.0;
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void Rovio_Position2d::SaveChild(std::string &prefix, std::ostream &stream)
+{
+ stream << prefix << *(this->leftJointNameP) << "\n";
+ stream << prefix << *(this->rightJointNameP) << "\n";
+ stream << prefix << *(this->torqueP) << "\n";
+ stream << prefix << *(this->wheelDiamP) << "\n";
+ stream << prefix << *(this->wheelSepP) << "\n";
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Reset
+void Rovio_Position2d::ResetChild()
+{
+ // Reset odometric pose
+ this->odomPose[0] = 0.0;
+ this->odomPose[1] = 0.0;
+ this->odomPose[2] = 0.0;
+
+ this->odomVel[0] = 0.0;
+ this->odomVel[1] = 0.0;
+ this->odomVel[2] = 0.0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void Rovio_Position2d::UpdateChild()
+{
+ // TODO: Step should be in a parameter of this function
+ double wd, ws;
+ double d1, d2;
+ double dr, da;
+ double stepTime;
+
+ this->myIface->Lock(1);
+
+ this->GetPositionCmd();
+
+ wd = **(this->wheelDiamP);
+ ws = **(this->wheelSepP);
+
+
+ //stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime();
+ stepTime = Simulator::Instance()->GetSimTime() - this->prevUpdateTime;
+ this->prevUpdateTime = Simulator::Instance()->GetSimTime();
+
+ // Distance travelled by front wheels
+ d1 = stepTime * wd / 2 * this->joints[LEFT]->GetAngleRate();
+ d2 = stepTime * wd / 2 * this->joints[RIGHT]->GetAngleRate();
+
+ dr = (d1 + d2) / 2;
+ da = (d1 - d2) / ws;
+
+ // Compute odometric pose
+ this->odomPose[0] += dr * cos( this->odomPose[2] );
+ this->odomPose[1] += dr * sin( this->odomPose[2] );
+ this->odomPose[2] += da;
+
+ // Compute odometric instantaneous velocity
+ this->odomVel[0] = dr / stepTime;
+ this->odomVel[1] = 0.0;
+ this->odomVel[2] = da / stepTime;
+
+
+ //if (this->enableMotors)
+ {
+ this->joints[LEFT]->SetParam( dParamVel,
+ this->wheelSpeed[LEFT] /
(**(this->wheelDiamP) / 2.0) );
+
+ this->joints[RIGHT]->SetParam( dParamVel,
+ this->wheelSpeed[RIGHT] /
(**(this->wheelDiamP) / 2.0) );
+ this->joints[LEFT]->SetParam( dParamFMax, **(this->torqueP) );
+ this->joints[RIGHT]->SetParam( dParamFMax, **(this->torqueP) );
+
+ //printf("Set Speed[%f %f]\n",this->wheelSpeed[LEFT],
this->wheelSpeed[RIGHT]);
+ }
+ /*else
+ {
+ this->joints[LEFT]->SetParam( dParamVel, 0 );
+ this->joints[RIGHT]->SetParam( dParamVel, 0 );
+
+ this->joints[LEFT]->SetParam( dParamFMax, 0 );
+ this->joints[RIGHT]->SetParam( dParamFMax, 0 );
+ }*/
+
+ this->PutPositionData();
+
+ this->myIface->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Finalize the controller
+void Rovio_Position2d::FiniChild()
+{
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Get commands from the external interface
+void Rovio_Position2d::GetPositionCmd()
+{
+ double vr, va;
+
+ vr = this->myIface->data->cmdVelocity.pos.x;
+ va = this->myIface->data->cmdVelocity.yaw;
+
+ this->enableMotors = this->myIface->data->cmdEnableMotors > 0;
+
+ this->wheelSpeed[LEFT] = vr + va * **(this->wheelSepP) / 2;
+ this->wheelSpeed[RIGHT] = vr - va * **(this->wheelSepP) / 2;
+
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Update the data in the interface
+void Rovio_Position2d::PutPositionData()
+{
+ // TODO: Data timestamp
+ this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
+
+ this->myIface->data->pose.pos.x = this->odomPose[0];
+ this->myIface->data->pose.pos.y = this->odomPose[1];
+ this->myIface->data->pose.yaw = NORMALIZE(this->odomPose[2]);
+
+ this->myIface->data->velocity.pos.x = this->odomVel[0];
+ this->myIface->data->velocity.yaw = this->odomVel[2];
+
+ // TODO
+ this->myIface->data->stall = 0;
+}
Added: code/gazebo/trunk/server/controllers/position2d/rovio/Rovio_Position2d.hh
===================================================================
--- code/gazebo/trunk/server/controllers/position2d/rovio/Rovio_Position2d.hh
(rev 0)
+++ code/gazebo/trunk/server/controllers/position2d/rovio/Rovio_Position2d.hh
2009-09-30 16:59:13 UTC (rev 8270)
@@ -0,0 +1,138 @@
+/*
+ * 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: Position2d controller for a rovio robot
+ * Author: Nathan Koenig
+ * Date: 22 Sep 2009
+ * SVN: $Id$
+ */
+#ifndef ROVIO_POSITION2D_HH
+#define ROVIO_POSITION2D_HH
+
+#include <map>
+
+#include "Param.hh"
+#include "Controller.hh"
+
+namespace gazebo
+{
+ class HingeJoint;
+ class Entity;
+ class PositionIface;
+
+/// \addtogroup gazebo_controller
+/// \{
+/** \defgroup rovio_position2d rovio_position2d
+
+ \brief ROvio Position2D controller.
+
+ This is a controller that simulates a Rovio motion
+
+ \verbatim
+ <controller:rovio_position2d name="controller-name">
+ <leftJoint>left-joint-name</leftJoint>
+ <rightJoint>right-join-name</rightJoint>
+ <interface:position name="iface-name"/>
+ </controller:rovio_position2d>
+ \endverbatim
+
+ \{
+*/
+
+/// \brief Rovio Position2D controller.
+/// This is a controller that simulates a Rovio motion
+class Rovio_Position2d : public Controller
+{
+ /// Constructor
+ public: Rovio_Position2d(Entity *parent );
+
+ /// Destructor
+ public: virtual ~Rovio_Position2d();
+
+ /// Load the controller
+ /// \param node XML config node
+ protected: virtual void LoadChild(XMLConfigNode *node);
+
+ /// \brief Save the controller.
+ /// \stream Output stream
+ protected: void SaveChild(std::string &prefix, std::ostream &stream);
+
+ /// Init the controller
+ protected: virtual void InitChild();
+
+ /// \brief Reset the controller
+ protected: void ResetChild();
+
+ /// Update the controller
+ protected: virtual void UpdateChild();
+
+ /// Finalize the controller
+ protected: virtual void FiniChild();
+
+ /// Update the data in the interface
+ private: void PutPositionData();
+
+ /// Get the position command from libgazebo
+ private: void GetPositionCmd();
+
+ /// The Position interface
+ private: PositionIface *myIface;
+
+ /// The parent Model
+ private: Model *myParent;
+
+ /// Separation between the wheels
+ private: ParamT<float> *wheelSepP;
+
+ /// Diameter of the wheels
+ private: ParamT<float> *wheelDiamP;
+
+ ///Torque applied to the wheels
+ private: ParamT<float> *torqueP;
+
+ /// Speeds of the wheels
+ private: float wheelSpeed[2];
+
+ // Simulation time of the last update
+ private: double prevUpdateTime;
+
+ /// True = enable motors
+ private: bool enableMotors;
+
+ private: float odomPose[3];
+ private: float odomVel[3];
+
+ private: HingeJoint *joints[2];
+
+ private: PhysicsEngine *physicsEngine;
+
+ private: ParamT<std::string> *leftJointNameP;
+ private: ParamT<std::string> *rightJointNameP;
+ private: ParamT<std::string> *backJointNameP;
+};
+
+/** \} */
+/// \}
+
+}
+
+#endif
+
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Come build with us! The BlackBerry® Developer Conference in SF, CA
is the only developer event you need to attend this year. Jumpstart your
developing skills, take BlackBerry mobile applications to market and stay
ahead of the curve. Join us from November 9-12, 2009. Register now!
http://p.sf.net/sfu/devconf
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
