Revision: 7846
http://playerstage.svn.sourceforge.net/playerstage/?rev=7846&view=rev
Author: rtv
Date: 2009-06-12 22:30:34 +0000 (Fri, 12 Jun 2009)
Log Message:
-----------
separated out the model propeties into its own file
Modified Paths:
--------------
code/gazebo/trunk/webgazebo/CMakeLists.txt
code/gazebo/trunk/webgazebo/WebGazebo.cc
Added Paths:
-----------
code/gazebo/trunk/webgazebo/Properties.cc
Modified: code/gazebo/trunk/webgazebo/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/webgazebo/CMakeLists.txt 2009-06-12 22:16:33 UTC (rev
7845)
+++ code/gazebo/trunk/webgazebo/CMakeLists.txt 2009-06-12 22:30:34 UTC (rev
7846)
@@ -1,12 +1,10 @@
-SET (sources WebGazebo.cc
+SET (sources WebGazebo.cc
+ Properties.cc
../server/Quatern.cc
../server/Vector3.cc
../server/Angle.cc
)
-MESSAGE( STATUS "websim include dirs ${WEBSIM_INCLUDE_DIRS}" )
-MESSAGE( STATUS "websim library dirs ${WEBSIM_LIBRARY_DIRS}" )
-
INCLUDE_DIRECTORIES(
${CMAKE_SOURCE_DIR}/server
${CMAKE_SOURCE_DIR}/libgazebo
Added: code/gazebo/trunk/webgazebo/Properties.cc
===================================================================
--- code/gazebo/trunk/webgazebo/Properties.cc (rev 0)
+++ code/gazebo/trunk/webgazebo/Properties.cc 2009-06-12 22:30:34 UTC (rev
7846)
@@ -0,0 +1,151 @@
+/*
+ * 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: HTTP portal to libgazebo
+ * Author: Brian Gerkey, Richard Vaughan
+ * Date: 9 March 2009
+ * SVN: $Id: gazebo.h 7398 2009-03-09 07:21:49Z natepak $
+ */
+
+#include "WebGazebo.hh"
+
+#include <fstream>
+
+#include <stdio.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <time.h>
+#include <string.h>
+
+#include "Quatern.hh"
+
+bool
+WebGazebo::GetModelPVA(const std::string& name,
+ websim::Time &t,
+ websim::Pose& p,
+ websim::Velocity& v,
+ websim::Acceleration& a,
+ std::string& response)
+{
+ // Discard any leftover responses
+ this->simIface->data->responseCount = 0;
+
+ // Ask Gazebo
+ this->simIface->GetPose3d(name.c_str());
+
+ // Wait for the response
+ double timeout = 3.0;
+ struct timeval t0, t1;
+ gettimeofday(&t0, NULL);
+ struct timespec sleeptime = {0, 1000000};
+ while(this->simIface->data->responseCount == 0)
+ {
+ gettimeofday(&t1, NULL);
+ if(((t1.tv_sec + t1.tv_usec/1e6) - (t0.tv_sec + t0.tv_usec/1e6))
+ > timeout)
+ {
+ response= "Timeout";
+ return false;
+ }
+ nanosleep(&sleeptime, NULL);
+ }
+
+ assert(this->simIface->data->responseCount == 1);
+ p.x = this->simIface->data->responses[0].modelPose.pos.x;
+ p.y = this->simIface->data->responses[0].modelPose.pos.y;
+ p.z = this->simIface->data->responses[0].modelPose.pos.z;
+ p.r = this->simIface->data->responses[0].modelPose.roll;
+ p.p = this->simIface->data->responses[0].modelPose.pitch;
+ p.a = this->simIface->data->responses[0].modelPose.yaw;
+
+ return true;
+}
+
+
+bool
+WebGazebo::SetModelPVA(const std::string& name,
+ const websim::Pose& p,
+ const websim::Velocity& v,
+ const websim::Acceleration& a,
+ std::string& response)
+{
+ // TODO: get pose, check for too-large jump, and complain about it
+ // note: CheckTolerances() is a first cut at doing this
+
+ // Tell Gazebo the new pose
+ gazebo::Pose pose;
+ gazebo::Vec3 lv, av, la, aa;
+ pose.pos.x = p.x;
+ pose.pos.y = p.y;
+ pose.pos.z = p.z;
+ pose.roll = p.r;
+ pose.pitch = p.p;
+ pose.yaw = p.a;
+
+ lv.x = v.x;
+ lv.y = v.y;
+ lv.z = v.z;
+ av.x = v.r;
+ av.y = v.p;
+ av.z = v.a;
+
+ la.x = a.x;
+ la.y = a.y;
+ la.z = a.z;
+ aa.x = a.r;
+ aa.y = a.p;
+ aa.z = a.a;
+
+ this->simIface->SetState(name.c_str(),
+ pose, lv, av, la, aa);
+
+ return true;
+}
+
+bool
+WebGazebo::CheckTolerances(gazebo::Pose p, gazebo::Pose q)
+{
+ // Check position
+ double sq_dd = ((p.pos.x - q.pos.x)*(p.pos.x - q.pos.x) +
+ (p.pos.y - q.pos.y)*(p.pos.y - q.pos.y) +
+ (p.pos.z - q.pos.z)*(p.pos.z - q.pos.z));
+ if((sq_dist_tol > 0) && (sq_dd > sq_dist_tol))
+ return false;
+
+ // Check orientation
+ gazebo::Quatern p_quat, q_quat;
+ gazebo::Vector3 pv(p.roll, p.pitch, p.yaw);
+ gazebo::Vector3 qv(q.roll, q.pitch, q.yaw);
+ p_quat.SetFromEuler(pv);
+ q_quat.SetFromEuler(qv);
+ gazebo::Quatern da = p_quat - q_quat;
+ //da.Normalize();
+
+ double sq_da = ((da.u*da.u) +
+ (da.x*da.x) +
+ (da.y*da.y) +
+ (da.z*da.z));
+ if((sq_ang_tol > 0) && (sq_da > sq_ang_tol))
+ return false;
+
+ return true;
+}
+
Modified: code/gazebo/trunk/webgazebo/WebGazebo.cc
===================================================================
--- code/gazebo/trunk/webgazebo/WebGazebo.cc 2009-06-12 22:16:33 UTC (rev
7845)
+++ code/gazebo/trunk/webgazebo/WebGazebo.cc 2009-06-12 22:30:34 UTC (rev
7846)
@@ -20,7 +20,7 @@
*/
/* Desc: HTTP portal to libgazebo
- * Author: Brian Gerkey
+ * Author: Brian Gerkey, Richard Vaughan
* Date: 9 March 2009
* SVN: $Id: gazebo.h 7398 2009-03-09 07:21:49Z natepak $
*/
@@ -37,9 +37,6 @@
#include "Quatern.hh"
-// TODO:
-// - make ghost models not collide, fall, etc.
-
WebGazebo::WebGazebo(const std::string& fedfile,
const std::string& host, unsigned short port,
double dtol, double atol) :
@@ -70,48 +67,6 @@
delete this->client;
}
-bool
-WebGazebo::GetModelPVA(const std::string& name,
- websim::Time &t,
- websim::Pose& p,
- websim::Velocity& v,
- websim::Acceleration& a,
- std::string& response)
-{
- // Discard any leftover responses
- this->simIface->data->responseCount = 0;
-
- // Ask Gazebo
- this->simIface->GetPose3d(name.c_str());
-
- // Wait for the response
- double timeout = 3.0;
- struct timeval t0, t1;
- gettimeofday(&t0, NULL);
- struct timespec sleeptime = {0, 1000000};
- while(this->simIface->data->responseCount == 0)
- {
- gettimeofday(&t1, NULL);
- if(((t1.tv_sec + t1.tv_usec/1e6) - (t0.tv_sec + t0.tv_usec/1e6))
- > timeout)
- {
- response= "Timeout";
- return false;
- }
- nanosleep(&sleeptime, NULL);
- }
-
- assert(this->simIface->data->responseCount == 1);
- p.x = this->simIface->data->responses[0].modelPose.pos.x;
- p.y = this->simIface->data->responses[0].modelPose.pos.y;
- p.z = this->simIface->data->responses[0].modelPose.pos.z;
- p.r = this->simIface->data->responses[0].modelPose.roll;
- p.p = this->simIface->data->responses[0].modelPose.pitch;
- p.a = this->simIface->data->responses[0].modelPose.yaw;
-
- return true;
-}
-
/** Get the current simulation time */
websim::Time
WebGazebo::GetTime()
@@ -126,74 +81,6 @@
bool
-WebGazebo::SetModelPVA(const std::string& name,
- const websim::Pose& p,
- const websim::Velocity& v,
- const websim::Acceleration& a,
- std::string& response)
-{
- // TODO: get pose, check for too-large jump, and complain about it
-
- // Tell Gazebo the new pose
- gazebo::Pose pose;
- gazebo::Vec3 lv, av, la, aa;
- pose.pos.x = p.x;
- pose.pos.y = p.y;
- pose.pos.z = p.z;
- pose.roll = p.r;
- pose.pitch = p.p;
- pose.yaw = p.a;
-
- lv.x = v.x;
- lv.y = v.y;
- lv.z = v.z;
- av.x = v.r;
- av.y = v.p;
- av.z = v.a;
-
- la.x = a.x;
- la.y = a.y;
- la.z = a.z;
- aa.x = a.r;
- aa.y = a.p;
- aa.z = a.a;
-
- this->simIface->SetState(name.c_str(),
- pose, lv, av, la, aa);
-
- return true;
-}
-
-bool
-WebGazebo::CheckTolerances(gazebo::Pose p, gazebo::Pose q)
-{
- // Check position
- double sq_dd = ((p.pos.x - q.pos.x)*(p.pos.x - q.pos.x) +
- (p.pos.y - q.pos.y)*(p.pos.y - q.pos.y) +
- (p.pos.z - q.pos.z)*(p.pos.z - q.pos.z));
- if((sq_dist_tol > 0) && (sq_dd > sq_dist_tol))
- return false;
-
- // Check orientation
- gazebo::Quatern p_quat, q_quat;
- gazebo::Vector3 pv(p.roll, p.pitch, p.yaw);
- gazebo::Vector3 qv(q.roll, q.pitch, q.yaw);
- p_quat.SetFromEuler(pv);
- q_quat.SetFromEuler(qv);
- gazebo::Quatern da = p_quat - q_quat;
- //da.Normalize();
-
- double sq_da = ((da.u*da.u) +
- (da.x*da.x) +
- (da.y*da.y) +
- (da.z*da.z));
- if((sq_ang_tol > 0) && (sq_da > sq_ang_tol))
- return false;
-
- return true;
-}
-
-bool
WebGazebo::GetModel(const std::string& name,
const std::string& type,
std::string& xmldata,
@@ -246,7 +133,7 @@
if(!GetModel(name,type,xmldata,response))
return false;
- struct timespec sleeptime = {0, 100000000};
+ struct timespec sleeptime = {0, 1e08};
for(;;)
{
this->factoryIface->Lock(1);
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Crystal Reports - New Free Runtime and 30 Day Trial
Check out the new simplified licensing option that enables unlimited
royalty-free distribution of the report engine for externally facing
server and web deployment.
http://p.sf.net/sfu/businessobjects
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
