[gazebo] 01/01: Imported Upstream version 6.1.0
Jose Luis Rivero
jrivero-guest at moszumanska.debian.org
Mon Aug 31 12:22:51 UTC 2015
This is an automated email from the git hooks/post-receive script.
jrivero-guest pushed a commit to annotated tag upstream/6.1.0
in repository gazebo.
commit 18733e3d136fff2657a215c258ef6f926a6dde0d
Author: Jose Luis Rivero <jrivero at osrfoundation.org>
Date: Mon Aug 24 18:19:07 2015 +0200
Imported Upstream version 6.1.0
---
.hg_archival.txt | 4 +-
CMakeLists.txt | 2 +-
Changelog.md | 15 ++
cmake/SearchForStuff.cmake | 2 +-
.../plugins/animate_joints/animate_joints.world | 1 -
examples/plugins/animate_pose/animate_pose.cc | 4 +-
gazebo/Server.cc | 3 +
gazebo/gazebo_main.cc | 3 +
gazebo/gui/GuiIface.cc | 3 +
gazebo/msgs/CMakeLists.txt | 2 +
gazebo/msgs/logical_camera_image.proto | 28 +++
gazebo/msgs/logical_camera_sensor.proto | 20 +++
gazebo/msgs/sensor.proto | 31 ++--
gazebo/physics/CMakeLists.txt | 1 +
gazebo/physics/bullet/BulletBoxShape.hh | 3 +
gazebo/physics/bullet/BulletCylinderShape.hh | 1 +
gazebo/physics/bullet/BulletPlaneShape.hh | 1 +
gazebo/physics/bullet/BulletRaySensor.cc | 149 ----------------
gazebo/physics/bullet/BulletRaySensor.hh | 81 ---------
gazebo/physics/bullet/BulletSphereShape.hh | 2 +
gazebo/physics/bullet/CMakeLists.txt | 1 -
gazebo/rendering/CMakeLists.txt | 2 +
gazebo/rendering/Heightmap.cc | 1 +
gazebo/rendering/LogicalCameraVisual.cc | 126 ++++++++++++++
gazebo/rendering/LogicalCameraVisual.hh | 62 +++++++
gazebo/rendering/RTShaderSystem.cc | 1 +
gazebo/rendering/RenderTypes.hh | 5 +
gazebo/rendering/Scene.cc | 23 +++
.../rendering/selection_buffer/MaterialSwitcher.cc | 1 +
.../rendering/selection_buffer/SelectionBuffer.cc | 1 +
gazebo/sensors/CMakeLists.txt | 2 +
gazebo/sensors/LogicalCameraSensor.cc | 192 +++++++++++++++++++++
gazebo/sensors/LogicalCameraSensor.hh | 104 +++++++++++
gazebo/sensors/LogicalCameraSensorPrivate.hh | 54 ++++++
gazebo/sensors/Sensor.cc | 12 +-
gazebo/sensors/SensorFactory.cc | 16 +-
gazebo/sensors/SensorTypes.hh | 5 +
plugins/CMakeLists.txt | 1 +
plugins/RandomVelocityPlugin.cc | 181 +++++++++++++++++++
plugins/RandomVelocityPlugin.hh | 88 ++++++++++
plugins/RandomVelocityPluginPrivate.hh | 69 ++++++++
test/integration/CMakeLists.txt | 1 +
test/integration/logical_camera_sensor.cc | 108 ++++++++++++
test/integration/physics_friction.cc | 144 +++++-----------
tools/gz_TEST.cc | 2 +-
worlds/CMakeLists.txt | 1 +
worlds/logical_camera.world | 47 +++++
worlds/random_velocity.world | 65 +++++++
48 files changed, 1309 insertions(+), 362 deletions(-)
diff --git a/.hg_archival.txt b/.hg_archival.txt
index 576ba0d..e76edd5 100644
--- a/.hg_archival.txt
+++ b/.hg_archival.txt
@@ -1,5 +1,5 @@
repo: e803bb5fe03c1370cbb9f434912cd73fd1e7942d
-node: 8b634ebbd50868f4009824b89cabde57f3015ce9
+node: 6f51fd02c306714d862254456de294b3f41c843c
branch: gazebo6
latesttag: gazebo6_6.0.0
-latesttagdistance: 1
+latesttagdistance: 30
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9104a60..06502fd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
set (GAZEBO_MAJOR_VERSION 6)
-set (GAZEBO_MINOR_VERSION 0)
+set (GAZEBO_MINOR_VERSION 1)
# The patch version may have been bumped for prerelease purposes; be sure to
# check gazebo-release/ubuntu/debian/changelog at default to determine what the
# next patch version should be for a regular release.
diff --git a/Changelog.md b/Changelog.md
index cd3aaa2..3925a11 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -1,5 +1,11 @@
## Gazebo 6.0
+1. Added logical_camera sensor.
+ * [Pull request #1845](https://bitbucket.org/osrf/gazebo/pull-request/1845)
+
+1. Added RandomVelocityPlugin, which applies a random velocity to a model's link.
+ * [Pull request #1839](https://bitbucket.org/osrf/gazebo/pull-request/1839)
+
1. Added magnetometer sensor. A contribution from Andrew Symington.
* [Pull request #1788](https://bitbucket.org/osrf/gazebo/pull-request/1788)
@@ -358,6 +364,15 @@ compilation on Windows.
### Gazebo 5.x.x
+1. Initialize sigact struct fields that valgrind said were being used uninitialized
+ * [Pull request #1809](https://bitbucket.org/osrf/gazebo/pull-request/1809)
+
+1. Add missing ogre includes to ensure macros are properly defined
+ * [Pull request #1813](https://bitbucket.org/osrf/gazebo/pull-request/1813)
+
+1. Use ToSDF functions to simplify physics_friction test
+ * [Pull request #1808](https://bitbucket.org/osrf/gazebo/pull-request/1808)
+
1. Added lines to laser sensor visualization
* [Pull request #1742](https://bitbucket.org/osrf/gazebo/pull-request/1742)
* [Issue #935](https://bitbucket.org/osrf/gazebo/issue/935)
diff --git a/cmake/SearchForStuff.cmake b/cmake/SearchForStuff.cmake
index 0bfe805..d94bf4b 100644
--- a/cmake/SearchForStuff.cmake
+++ b/cmake/SearchForStuff.cmake
@@ -476,7 +476,7 @@ endif ()
########################################
# Find SDFormat
-set (SDFormat_MIN_VERSION 3.0.0)
+set (SDFormat_MIN_VERSION 3.1.0)
find_package(SDFormat ${SDFormat_MIN_VERSION})
if (NOT SDFormat_FOUND)
diff --git a/examples/plugins/animate_joints/animate_joints.world b/examples/plugins/animate_joints/animate_joints.world
index 12e9e31..6a847a8 100644
--- a/examples/plugins/animate_joints/animate_joints.world
+++ b/examples/plugins/animate_joints/animate_joints.world
@@ -85,7 +85,6 @@
</axis>
</joint>
<plugin name="animate_joints" filename="libanimate_joints.so" />
- <static>true</static>
</model>
<light type="directional" name="sun">
<pose>0 0 10 0 0 0</pose>
diff --git a/examples/plugins/animate_pose/animate_pose.cc b/examples/plugins/animate_pose/animate_pose.cc
index 3dc5a84..79e7188 100644
--- a/examples/plugins/animate_pose/animate_pose.cc
+++ b/examples/plugins/animate_pose/animate_pose.cc
@@ -30,7 +30,7 @@ namespace gazebo
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
gazebo::common::PoseAnimationPtr anim(
- new gazebo::common::PoseAnimation("test", 1000.0, true));
+ new gazebo::common::PoseAnimation("test", 10.0, true));
gazebo::common::PoseKeyFrame *key;
@@ -38,7 +38,7 @@ namespace gazebo
key->Translation(ignition::math::Vector3d(0, 0, 0));
key->Rotation(ignition::math::Quaterniond(0, 0, 0));
- key = anim->CreateKeyFrame(1000.0);
+ key = anim->CreateKeyFrame(10.0);
key->Translation(ignition::math::Vector3d(5, 0, 0));
key->Rotation(ignition::math::Quaterniond(0, 0, 1.5707));
diff --git a/gazebo/Server.cc b/gazebo/Server.cc
index 2348f6d..d88e508 100644
--- a/gazebo/Server.cc
+++ b/gazebo/Server.cc
@@ -467,7 +467,10 @@ void Server::Run()
// Now that we're about to run, install a signal handler to allow for
// graceful shutdown on Ctrl-C.
struct sigaction sigact;
+ sigact.sa_flags = 0;
sigact.sa_handler = Server::SigInt;
+ if (sigemptyset(&sigact.sa_mask) != 0)
+ std::cerr << "sigemptyset failed while setting up for SIGINT" << std::endl;
if (sigaction(SIGINT, &sigact, NULL))
std::cerr << "sigaction(2) failed while setting up for SIGINT" << std::endl;
#endif
diff --git a/gazebo/gazebo_main.cc b/gazebo/gazebo_main.cc
index c6b46ad..80337ca 100644
--- a/gazebo/gazebo_main.cc
+++ b/gazebo/gazebo_main.cc
@@ -117,7 +117,10 @@ int main(int _argc, char **_argv)
}
struct sigaction sigact;
+ sigact.sa_flags = 0;
sigact.sa_handler = sig_handler;
+ if (sigemptyset(&sigact.sa_mask) != 0)
+ std::cerr << "sigemptyset failed while setting up for SIGINT" << std::endl;
if (sigaction(SIGINT, &sigact, NULL))
{
gzerr << "Stopping. Unable to catch SIGINT.\n"
diff --git a/gazebo/gui/GuiIface.cc b/gazebo/gui/GuiIface.cc
index 9a8755e..4a7a5e0 100644
--- a/gazebo/gui/GuiIface.cc
+++ b/gazebo/gui/GuiIface.cc
@@ -301,7 +301,10 @@ bool gui::run(int _argc, char **_argv)
// Now that we're about to run, install a signal handler to allow for
// graceful shutdown on Ctrl-C.
struct sigaction sigact;
+ sigact.sa_flags = 0;
sigact.sa_handler = signal_handler;
+ if (sigemptyset(&sigact.sa_mask) != 0)
+ std::cerr << "sigemptyset failed while setting up for SIGINT" << std::endl;
if (sigaction(SIGINT, &sigact, NULL))
{
std::cerr << "signal(2) failed while setting up for SIGINT" << std::endl;
diff --git a/gazebo/msgs/CMakeLists.txt b/gazebo/msgs/CMakeLists.txt
index 00a79ee..013233c 100644
--- a/gazebo/msgs/CMakeLists.txt
+++ b/gazebo/msgs/CMakeLists.txt
@@ -51,6 +51,8 @@ set (msgs
log_playback_control.proto
log_playback_stats.proto
log_status.proto
+ logical_camera_image.proto
+ logical_camera_sensor.proto
magnetometer.proto
material.proto
meshgeom.proto
diff --git a/gazebo/msgs/logical_camera_image.proto b/gazebo/msgs/logical_camera_image.proto
new file mode 100644
index 0000000..4c3d629
--- /dev/null
+++ b/gazebo/msgs/logical_camera_image.proto
@@ -0,0 +1,28 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface LogicalCameraImage
+/// \brief Information about models seen by a LogicalCameraSensor
+
+import "pose.proto";
+
+message LogicalCameraImage
+{
+ /// \brief Information about a model that is reported by a
+ /// LogicalCameraSensor
+ message Model
+ {
+ /// \brief Name of the detected model
+ required string name = 1;
+
+ /// \brief Pose of the detected model. The pose is relative to the
+ /// logical camera's pose.
+ required Pose pose = 2;
+ }
+
+ /// \brief Pose of the logical camera.
+ required Pose pose = 1;
+
+ /// \brief All the models seen by the LogicalCamera
+ repeated Model model = 2;
+}
diff --git a/gazebo/msgs/logical_camera_sensor.proto b/gazebo/msgs/logical_camera_sensor.proto
new file mode 100644
index 0000000..1320265
--- /dev/null
+++ b/gazebo/msgs/logical_camera_sensor.proto
@@ -0,0 +1,20 @@
+package gazebo.msgs;
+
+/// \ingroup gazebo_msgs
+/// \interface LogicalCameraSensor
+/// \brief Information about a logical camera sensor element
+
+message LogicalCameraSensor
+{
+ /// \brief Near clipping plane of the view frustum in meters.
+ required double near = 1;
+
+ /// \brief Far clipping plane of the view frustum in meters.
+ required double far = 2;
+
+ /// \brief Horizontal field of view in radians.
+ required double horizontal_fov = 3;
+
+ /// \brief Near and far clipping plane aspect ratio (width/height).
+ required double aspect_ratio = 4;
+}
diff --git a/gazebo/msgs/sensor.proto b/gazebo/msgs/sensor.proto
index e892b21..aa5e04c 100644
--- a/gazebo/msgs/sensor.proto
+++ b/gazebo/msgs/sensor.proto
@@ -4,25 +4,28 @@ package gazebo.msgs;
/// \interface Sensor
/// \brief Information about a sensor element
-
import "pose.proto";
import "camerasensor.proto";
import "raysensor.proto";
import "contactsensor.proto";
+import "logical_camera_sensor.proto";
message Sensor
{
- required string name = 1;
- optional uint32 id = 2;
- required string parent = 3;
- required uint32 parent_id = 4;
- required string type = 5;
- optional bool always_on = 6;
- optional double update_rate = 7;
- optional Pose pose = 8;
- optional CameraSensor camera = 9;
- optional RaySensor ray = 10;
- optional ContactSensor contact = 11;
- optional bool visualize = 12;
- optional string topic = 13;
+ required string name = 1;
+ optional uint32 id = 2;
+ required string parent = 3;
+ required uint32 parent_id = 4;
+ required string type = 5;
+ optional bool always_on = 6;
+ optional double update_rate = 7;
+ optional Pose pose = 8;
+ optional CameraSensor camera = 9;
+ optional RaySensor ray = 10;
+ optional ContactSensor contact = 11;
+ optional bool visualize = 12;
+ optional string topic = 13;
+
+ /// \brief Description of a logical camera sensor
+ optional LogicalCameraSensor logical_camera = 14;
}
diff --git a/gazebo/physics/CMakeLists.txt b/gazebo/physics/CMakeLists.txt
index ef79380..4f0c759 100644
--- a/gazebo/physics/CMakeLists.txt
+++ b/gazebo/physics/CMakeLists.txt
@@ -77,6 +77,7 @@ set (headers
ContactManager.hh
CylinderShape.hh
Entity.hh
+ FixedJoint.hh
HeightmapShape.hh
Hinge2Joint.hh
HingeJoint.hh
diff --git a/gazebo/physics/bullet/BulletBoxShape.hh b/gazebo/physics/bullet/BulletBoxShape.hh
index eb46c69..27a36bf 100644
--- a/gazebo/physics/bullet/BulletBoxShape.hh
+++ b/gazebo/physics/bullet/BulletBoxShape.hh
@@ -23,6 +23,9 @@
#define _BULLETBOXSHAPE_HH_
#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletTypes.hh"
+#include "gazebo/physics/bullet/BulletCollision.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/physics/BoxShape.hh"
#include "gazebo/util/system.hh"
diff --git a/gazebo/physics/bullet/BulletCylinderShape.hh b/gazebo/physics/bullet/BulletCylinderShape.hh
index e013ebc..d0fa999 100644
--- a/gazebo/physics/bullet/BulletCylinderShape.hh
+++ b/gazebo/physics/bullet/BulletCylinderShape.hh
@@ -23,6 +23,7 @@
#define _BULLETCYLINDERSHAPE_HH_
#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
#include "gazebo/physics/CylinderShape.hh"
#include "gazebo/util/system.hh"
diff --git a/gazebo/physics/bullet/BulletPlaneShape.hh b/gazebo/physics/bullet/BulletPlaneShape.hh
index 309c1b4..767c20e 100644
--- a/gazebo/physics/bullet/BulletPlaneShape.hh
+++ b/gazebo/physics/bullet/BulletPlaneShape.hh
@@ -25,6 +25,7 @@
#include <iostream>
#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletCollision.hh"
#include "gazebo/physics/PlaneShape.hh"
#include "gazebo/util/system.hh"
diff --git a/gazebo/physics/bullet/BulletRaySensor.cc b/gazebo/physics/bullet/BulletRaySensor.cc
deleted file mode 100644
index 130d4be..0000000
--- a/gazebo/physics/bullet/BulletRaySensor.cc
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
- * Copyright (C) 2012-2015 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-/* Desc: Bullet ray sensor
- * Author: Nate Koenig
- * Date: 21 May 2009
- */
-/*
-#include "World.hh"
-
-#include "BulletPhysics.hh"
-#include "BulletRaySensor.hh"
-*/
-
-using namespace gazebo;
-using namespace physics;
-
-
-//////////////////////////////////////////////////
-BulletRaySensor::BulletRaySensor(Link *_body)
- : PhysicsRaySensor(_body)
-{
- this->body = dynamic_cast<BulletLink*>(_body);
-
- if (this->body == NULL)
- gzthrow("BulletRaySensor requires an BulletLink");
-}
-
-//////////////////////////////////////////////////
-BulletRaySensor::~BulletRaySensor()
-{
- std::vector<BulletRayCollision*>::iterator iter;
-
- for (iter = this->rays.begin(); iter != this->rays.end(); ++iter)
- {
- delete (*iter);
- }
- this->rays.clear();
-}
-
-//////////////////////////////////////////////////
-void BulletRaySensor::AddRay(math::Vector3 start, math::Vector3 end,
- double minRange, double maxRange, bool display)
-{
- BulletRayCollision *rayCollision;
-
- rayCollision = static_cast<BulletRayCollision*>(
- this->GetWorld()->CreateCollision("ray", this->body));
- rayCollision->SetDisplayRays(display);
- rayCollision->SetMinLength(minRange);
- rayCollision->SetMaxLength(maxRange);
-
- rayCollision->SetPoints(start, end);
- this->rays.push_back(rayCollision);
-}
-
-//////////////////////////////////////////////////
-int BulletRaySensor::GetCount() const
-{
- return this->rays.size();
-}
-
-//////////////////////////////////////////////////
-void BulletRaySensor::GetRelativePoints(int _index,
- math::Vector3 &_a, math::Vector3 &_b)
-{
- if (_index <0 || _index >= static_cast<int>(this->rays.size()))
- {
- std::ostringstream stream;
- stream << "_index[" << _index << "] is out of range[0-"
- << this->GetCount() << "]";
- gzthrow(stream.str());
- }
-
- this->rays[_index]->GetRelativePoints(_a, _b);
-}
-
-//////////////////////////////////////////////////
-double BulletRaySensor::GetRange(int _index) const
-{
- if (_index <0 || _index >= static_cast<int>(this->rays.size()))
- {
- std::ostringstream stream;
- stream << "_index[" << _index << "] is out of range[0-"
- << this->GetCount() << "]";
- gzthrow(stream.str());
- }
-
- return this->rays[_index]->GetLength();
-}
-
-//////////////////////////////////////////////////
-double BulletRaySensor::GetRetro(int _index) const
-{
- if (_index <0 || _index >= static_cast<int>(this->rays.size()))
- {
- std::ostringstream stream;
- stream << "_index[" << _index << "] is out of range[0-"
- << this->GetCount() << "]";
- gzthrow(stream.str());
- }
-
- return this->rays[_index]->GetRetro();
-}
-
-//////////////////////////////////////////////////
-double BulletRaySensor::GetFiducial(int _index) const
-{
- if (_index <0 || _index >= static_cast<int>(this->rays.size()))
- {
- std::ostringstream stream;
- stream << "_index[" << _index << "] is out of range[0-"
- << this->GetCount() << "]";
- gzthrow(stream.str());
- }
-
- return this->rays[_index]->GetFiducial();
-}
-
-//////////////////////////////////////////////////
-void BulletRaySensor::Update()
-{
- std::vector<BulletRayCollision*>::iterator iter;
-
- for (iter = this->rays.begin(); iter != this->rays.end(); ++iter)
- {
- (*iter)->SetLength((*iter)->GetMaxLength());
- (*iter)->SetRetro(0.0);
- (*iter)->SetFiducial(-1);
-
- // Get the global points of the line
- (*iter)->Update();
- }
-}
-
-
diff --git a/gazebo/physics/bullet/BulletRaySensor.hh b/gazebo/physics/bullet/BulletRaySensor.hh
deleted file mode 100644
index 24ddcde..0000000
--- a/gazebo/physics/bullet/BulletRaySensor.hh
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- * Copyright (C) 2012-2015 Open Source Robotics Foundation
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *
-*/
-/* Desc: Bullet ray sensor
- * Author: Nate Koenig
- * Date: 21 May 2009
- */
-
-#ifndef BULLETRAYSENSOR_HH
-#define BULLETRAYSENSOR_HH
-
-#include <vector>
-#include "gazebo/util/system.hh"
-
-namespace gazebo
-{
- namespace physics
- {
- class BulletRayCollision;
- class BulletLink;
-
- /// \ingroup gazebo_physics
- /// \addtogroup gazebo_physics_bullet Bullet Physics
- /// \{
-
- /// \brief An Bullet Ray sensor
- class GZ_PHYSICS_VISIBLE BulletRaySensor : public PhysicsRaySensor
- {
- /// \brief Constructor
- public: BulletRaySensor(Link *body);
-
- /// \brief Destructor
- public: virtual ~BulletRaySensor();
-
- /// \brief Add a ray to the sensor
- public: void AddRay(math::Vector3 start, math::Vector3 end,
- double minRange, double maxRange, bool display);
-
- /// \brief Get the number of rays
- public: int GetCount() const;
-
- /// \brief Get the relative starting and ending points of a ray
- public: void GetRelativePoints(int index, math::Vector3 &a,
- math::Vector3 &b);
-
- /// \brief Get the range of a ray
- public: double GetRange(int index) const;
-
- /// \brief Get the retro reflectance value of a ray
- public: double GetRetro(int index) const;
-
- /// \brief Get the fiducial value of a ray
- public: double GetFiducial(int index) const;
-
- /// \brief Update the ray sensor
- public: virtual void Update();
-
- /// All the rays
- private: std::vector<BulletRayCollision*> rays;
-
- private: BulletLink *body;
- };
- /// \}
- }
-}
-#endif
-
-
diff --git a/gazebo/physics/bullet/BulletSphereShape.hh b/gazebo/physics/bullet/BulletSphereShape.hh
index 1742fe5..d9c9d35 100644
--- a/gazebo/physics/bullet/BulletSphereShape.hh
+++ b/gazebo/physics/bullet/BulletSphereShape.hh
@@ -23,6 +23,8 @@
#define _BULLETSPHERESHAPE_HH_
#include "gazebo/physics/bullet/BulletPhysics.hh"
+#include "gazebo/physics/bullet/BulletCollision.hh"
+#include "gazebo/physics/bullet/BulletLink.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/physics/SphereShape.hh"
#include "gazebo/util/system.hh"
diff --git a/gazebo/physics/bullet/CMakeLists.txt b/gazebo/physics/bullet/CMakeLists.txt
index 8a330c2..bc93b11 100644
--- a/gazebo/physics/bullet/CMakeLists.txt
+++ b/gazebo/physics/bullet/CMakeLists.txt
@@ -49,7 +49,6 @@ set (headers
BulletPhysics.hh
BulletPlaneShape.hh
BulletPolylineShape.hh
- BulletRaySensor.hh
BulletRayShape.hh
BulletScrewJoint.hh
BulletSliderJoint.hh
diff --git a/gazebo/rendering/CMakeLists.txt b/gazebo/rendering/CMakeLists.txt
index c55a383..0427a34 100644
--- a/gazebo/rendering/CMakeLists.txt
+++ b/gazebo/rendering/CMakeLists.txt
@@ -44,6 +44,7 @@ set (sources
LinkFrameVisual.cc
SonarVisual.cc
Light.cc
+ LogicalCameraVisual.cc
Material.cc
MovableText.cc
OrbitViewController.cc
@@ -100,6 +101,7 @@ set (headers
JointVisual.hh
LaserVisual.hh
LinkFrameVisual.hh
+ LogicalCameraVisual.hh
SonarVisual.hh
Light.hh
Material.hh
diff --git a/gazebo/rendering/Heightmap.cc b/gazebo/rendering/Heightmap.cc
index c11d130..d429f8c 100644
--- a/gazebo/rendering/Heightmap.cc
+++ b/gazebo/rendering/Heightmap.cc
@@ -30,6 +30,7 @@
#include "gazebo/common/SystemPaths.hh"
#include "gazebo/math/Helpers.hh"
#include "gazebo/transport/TransportIface.hh"
+#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/RTShaderSystem.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/Light.hh"
diff --git a/gazebo/rendering/LogicalCameraVisual.cc b/gazebo/rendering/LogicalCameraVisual.cc
new file mode 100644
index 0000000..05172a0
--- /dev/null
+++ b/gazebo/rendering/LogicalCameraVisual.cc
@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+#endif
+
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/rendering/ogre_gazebo.h"
+#include "gazebo/rendering/DynamicLines.hh"
+#include "gazebo/rendering/VisualPrivate.hh"
+#include "gazebo/rendering/LogicalCameraVisual.hh"
+
+using namespace gazebo;
+using namespace rendering;
+
+/////////////////////////////////////////////////
+LogicalCameraVisual::LogicalCameraVisual(const std::string &_name,
+ VisualPtr _vis)
+: Visual(_name, _vis)
+{
+ this->dataPtr->type = VT_SENSOR;
+}
+
+/////////////////////////////////////////////////
+LogicalCameraVisual::~LogicalCameraVisual()
+{
+ this->Fini();
+}
+
+/////////////////////////////////////////////////
+void LogicalCameraVisual::Load(const msgs::LogicalCameraSensor &_msg)
+{
+ double nearWidth = (tan(_msg.horizontal_fov()*0.5) * _msg.near());
+ double nearHeight = nearWidth / _msg.aspect_ratio();
+
+ double farWidth = (tan(_msg.horizontal_fov()*0.5) * _msg.far());
+ double farHeight = farWidth / _msg.aspect_ratio();
+
+ DynamicLines *line = this->CreateDynamicLine(RENDERING_LINE_LIST);
+
+ // Draw the near clipping plane as a white rectangle
+ line->AddPoint(math::Vector3(_msg.near(), nearWidth, nearHeight));
+ line->AddPoint(math::Vector3(_msg.near(), nearWidth, -nearHeight));
+
+ line->AddPoint(math::Vector3(_msg.near(), nearWidth, -nearHeight));
+ line->AddPoint(math::Vector3(_msg.near(), -nearWidth, -nearHeight));
+
+ line->AddPoint(math::Vector3(_msg.near(), -nearWidth, -nearHeight));
+ line->AddPoint(math::Vector3(_msg.near(), -nearWidth, nearHeight));
+
+ line->AddPoint(math::Vector3(_msg.near(), -nearWidth, nearHeight));
+ line->AddPoint(math::Vector3(_msg.near(), nearWidth, nearHeight));
+
+ // Draw the far clipping plane as a white rectangle
+ line->AddPoint(math::Vector3(_msg.far(), farWidth, farHeight));
+ line->AddPoint(math::Vector3(_msg.far(), farWidth, -farHeight));
+
+ line->AddPoint(math::Vector3(_msg.far(), farWidth, -farHeight));
+ line->AddPoint(math::Vector3(_msg.far(), -farWidth, -farHeight));
+
+ line->AddPoint(math::Vector3(_msg.far(), -farWidth, -farHeight));
+ line->AddPoint(math::Vector3(_msg.far(), -farWidth, farHeight));
+
+ line->AddPoint(math::Vector3(_msg.far(), -farWidth, farHeight));
+ line->AddPoint(math::Vector3(_msg.far(), farWidth, farHeight));
+
+ // Connect the near and far clipping planes with lines
+ line->AddPoint(math::Vector3(_msg.near(), nearWidth, nearHeight));
+ line->AddPoint(math::Vector3(_msg.far(), farWidth, farHeight));
+
+ line->AddPoint(math::Vector3(_msg.near(), -nearWidth, nearHeight));
+ line->AddPoint(math::Vector3(_msg.far(), -farWidth, farHeight));
+
+ line->AddPoint(math::Vector3(_msg.near(), -nearWidth, -nearHeight));
+ line->AddPoint(math::Vector3(_msg.far(), -farWidth, -farHeight));
+
+ line->AddPoint(math::Vector3(_msg.near(), nearWidth, -nearHeight));
+ line->AddPoint(math::Vector3(_msg.far(), farWidth, -farHeight));
+
+ line->setMaterial("Gazebo/WhiteGlow");
+ line->setVisibilityFlags(GZ_VISIBILITY_GUI);
+
+ // Draw green lines from the near clipping plane to the origin
+ DynamicLines *sourceLine = this->CreateDynamicLine(RENDERING_LINE_LIST);
+ sourceLine->AddPoint(math::Vector3::Zero);
+ sourceLine->AddPoint(math::Vector3(_msg.near(), nearWidth, nearHeight));
+
+ sourceLine->AddPoint(math::Vector3::Zero);
+ sourceLine->AddPoint(math::Vector3(_msg.near(), -nearWidth, nearHeight));
+
+ sourceLine->AddPoint(math::Vector3::Zero);
+ sourceLine->AddPoint(math::Vector3(_msg.near(), -nearWidth, -nearHeight));
+
+ sourceLine->AddPoint(math::Vector3::Zero);
+ sourceLine->AddPoint(math::Vector3(_msg.near(), nearWidth, -nearHeight));
+
+ sourceLine->setMaterial("Gazebo/PurpleGlow");
+ sourceLine->setVisibilityFlags(GZ_VISIBILITY_GUI);
+
+ this->SetVisibilityFlags(GZ_VISIBILITY_GUI);
+
+ if (this->dataPtr->parent)
+ this->dataPtr->parent->AttachVisual(shared_from_this());
+}
+
+/////////////////////////////////////////////////
+void LogicalCameraVisual::Fini()
+{
+ this->DetachObjects();
+}
diff --git a/gazebo/rendering/LogicalCameraVisual.hh b/gazebo/rendering/LogicalCameraVisual.hh
new file mode 100644
index 0000000..9a295dc
--- /dev/null
+++ b/gazebo/rendering/LogicalCameraVisual.hh
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef _GAZEBO_LOGICAL_CAMERAVISUAL_HH_
+#define _GAZEBO_LOGICAL_CAMERAVISUAL_HH_
+
+#include <string>
+
+#include "gazebo/msgs/MessageTypes.hh"
+#include "gazebo/rendering/Visual.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace rendering
+ {
+ /// \addtogroup gazebo_rendering Rendering
+ /// \{
+
+ /// \brief Logical camera visualization
+ ///
+ /// This class is used to visualize a logical camera generated from
+ /// a LogicalCameraSensor. The sensor's frustum is drawn in the 3D
+ /// environment.
+ class GZ_RENDERING_VISIBLE LogicalCameraVisual : public Visual
+ {
+ /// \brief Constructor
+ /// \param[in] _name Name of the Visual
+ /// \param[in] _vis Pointer to the parent Visual
+ public: LogicalCameraVisual(const std::string &_name, VisualPtr _vis);
+
+ /// \brief Destructor
+ public: virtual ~LogicalCameraVisual();
+
+ /// \brief Load the Visual
+ /// \param[in] _msg Message describing the camera sensor.
+ public: void Load(const msgs::LogicalCameraSensor &_msg);
+ using Visual::Load;
+
+ // Documentation inherited
+ protected: virtual void Fini();
+
+ /// \brief Update the visual
+ private: void Update();
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/rendering/RTShaderSystem.cc b/gazebo/rendering/RTShaderSystem.cc
index b7d812f..9506bc3 100644
--- a/gazebo/rendering/RTShaderSystem.cc
+++ b/gazebo/rendering/RTShaderSystem.cc
@@ -27,6 +27,7 @@
#include "gazebo/common/Console.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/SystemPaths.hh"
+#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/RenderEngine.hh"
#include "gazebo/rendering/Scene.hh"
#include "gazebo/rendering/Visual.hh"
diff --git a/gazebo/rendering/RenderTypes.hh b/gazebo/rendering/RenderTypes.hh
index a5194e1..a94eaf0 100644
--- a/gazebo/rendering/RenderTypes.hh
+++ b/gazebo/rendering/RenderTypes.hh
@@ -50,6 +50,7 @@ namespace gazebo
class DynamicLines;
class Visual;
class LaserVisual;
+ class LogicalCameraVisual;
class SonarVisual;
class WrenchVisual;
class CameraVisual;
@@ -121,6 +122,10 @@ namespace gazebo
/// \brief Shared pointer to CameraVisual
typedef boost::shared_ptr<CameraVisual> CameraVisualPtr;
+ /// \def LogicalCameraVisualPtr
+ /// \brief Shared pointer to LogicalCameraVisual
+ typedef boost::shared_ptr<LogicalCameraVisual> LogicalCameraVisualPtr;
+
/// \def JointVisualPtr
/// \brief Shared pointer to JointVisual
typedef boost::shared_ptr<JointVisual> JointVisualPtr;
diff --git a/gazebo/rendering/Scene.cc b/gazebo/rendering/Scene.cc
index 571d754..09d8b95 100644
--- a/gazebo/rendering/Scene.cc
+++ b/gazebo/rendering/Scene.cc
@@ -32,6 +32,7 @@
#include "gazebo/rendering/SonarVisual.hh"
#include "gazebo/rendering/WrenchVisual.hh"
#include "gazebo/rendering/CameraVisual.hh"
+#include "gazebo/rendering/LogicalCameraVisual.hh"
#include "gazebo/rendering/JointVisual.hh"
#include "gazebo/rendering/COMVisual.hh"
#include "gazebo/rendering/InertiaVisual.hh"
@@ -2082,6 +2083,28 @@ bool Scene::ProcessSensorMsg(ConstSensorPtr &_msg)
}
}
}
+ else if (_msg->type() == "logical_camera" && _msg->visualize())
+ {
+ VisualPtr parentVis = this->GetVisual(_msg->parent_id());
+ if (!parentVis)
+ return false;
+
+ Visual_M::iterator iter = this->dataPtr->visuals.find(_msg->id());
+ if (iter == this->dataPtr->visuals.end())
+ {
+ LogicalCameraVisualPtr cameraVis(new LogicalCameraVisual(
+ _msg->name()+"_GUIONLY_logical_camera_vis", parentVis));
+
+ // need to call AttachVisual in order for cameraVis to be added to
+ // parentVis' children list so that it can be properly deleted.
+ parentVis->AttachVisual(cameraVis);
+
+ cameraVis->SetPose(msgs::ConvertIgn(_msg->pose()));
+ cameraVis->SetId(_msg->id());
+ cameraVis->Load(_msg->logical_camera());
+ this->dataPtr->visuals[cameraVis->GetId()] = cameraVis;
+ }
+ }
else if (_msg->type() == "contact" && _msg->visualize() &&
!_msg->topic().empty())
{
diff --git a/gazebo/rendering/selection_buffer/MaterialSwitcher.cc b/gazebo/rendering/selection_buffer/MaterialSwitcher.cc
index 0faf806..7222932 100644
--- a/gazebo/rendering/selection_buffer/MaterialSwitcher.cc
+++ b/gazebo/rendering/selection_buffer/MaterialSwitcher.cc
@@ -16,6 +16,7 @@
*/
#include "gazebo/common/Console.hh"
+#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/selection_buffer/MaterialSwitcher.hh"
#include "gazebo/rendering/RenderTypes.hh"
diff --git a/gazebo/rendering/selection_buffer/SelectionBuffer.cc b/gazebo/rendering/selection_buffer/SelectionBuffer.cc
index 7939a0e..46a6489 100644
--- a/gazebo/rendering/selection_buffer/SelectionBuffer.cc
+++ b/gazebo/rendering/selection_buffer/SelectionBuffer.cc
@@ -16,6 +16,7 @@
*/
#include "gazebo/common/Console.hh"
+#include "gazebo/rendering/ogre_gazebo.h"
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/rendering/selection_buffer/SelectionRenderListener.hh"
#include "gazebo/rendering/selection_buffer/MaterialSwitcher.hh"
diff --git a/gazebo/sensors/CMakeLists.txt b/gazebo/sensors/CMakeLists.txt
index fb72ac3..6d929ce 100644
--- a/gazebo/sensors/CMakeLists.txt
+++ b/gazebo/sensors/CMakeLists.txt
@@ -14,6 +14,7 @@ set (sources
GpsSensor.cc
GpuRaySensor.cc
ImuSensor.cc
+ LogicalCameraSensor.cc
MagnetometerSensor.cc
MultiCameraSensor.cc
Noise.cc
@@ -40,6 +41,7 @@ set (headers
GpsSensor.hh
GpuRaySensor.hh
ImuSensor.hh
+ LogicalCameraSensor.hh
MagnetometerSensor.hh
MultiCameraSensor.hh
Noise.hh
diff --git a/gazebo/sensors/LogicalCameraSensor.cc b/gazebo/sensors/LogicalCameraSensor.cc
new file mode 100644
index 0000000..d1e5f3b
--- /dev/null
+++ b/gazebo/sensors/LogicalCameraSensor.cc
@@ -0,0 +1,192 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifdef _WIN32
+ // Ensure that Winsock2.h is included before Windows.h, which can get
+ // pulled in by anybody (e.g., Boost).
+ #include <Winsock2.h>
+#endif
+
+#include "gazebo/transport/transport.hh"
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/physics/World.hh"
+#include "gazebo/physics/Model.hh"
+
+#include "gazebo/sensors/SensorFactory.hh"
+#include "gazebo/sensors/LogicalCameraSensorPrivate.hh"
+#include "gazebo/sensors/LogicalCameraSensor.hh"
+
+using namespace gazebo;
+using namespace sensors;
+
+GZ_REGISTER_STATIC_SENSOR("logical_camera", LogicalCameraSensor)
+
+//////////////////////////////////////////////////
+LogicalCameraSensor::LogicalCameraSensor()
+ : Sensor(sensors::OTHER), dataPtr(new LogicalCameraSensorPrivate())
+{
+}
+
+//////////////////////////////////////////////////
+LogicalCameraSensor::~LogicalCameraSensor()
+{
+ delete this->dataPtr;
+}
+
+//////////////////////////////////////////////////
+void LogicalCameraSensor::Load(const std::string &_worldName,
+ sdf::ElementPtr _sdf)
+{
+ Sensor::Load(_worldName, _sdf);
+
+ // Get a pointer to the parent link. This will be used to adjust the
+ // orientation of the logical camera.
+ physics::EntityPtr parentEntity = this->world->GetEntity(this->parentName);
+ this->dataPtr->parentLink =
+ boost::dynamic_pointer_cast<physics::Link>(parentEntity);
+
+ // Store parent model's name for use in the UpdateImpl function.
+ this->dataPtr->modelName = this->dataPtr->parentLink->GetModel()->GetName();
+}
+
+//////////////////////////////////////////////////
+std::string LogicalCameraSensor::GetTopic() const
+{
+ std::string topicName = "~/" + this->parentName + "/" + this->GetName() +
+ "/models";
+ boost::replace_all(topicName, "::", "/");
+
+ return topicName;
+}
+
+//////////////////////////////////////////////////
+void LogicalCameraSensor::Load(const std::string &_worldName)
+{
+ Sensor::Load(_worldName);
+
+ // Create publisher of the logical camera images
+ this->dataPtr->pub = this->node->Advertise<msgs::LogicalCameraImage>(
+ this->GetTopic(), 50);
+}
+
+//////////////////////////////////////////////////
+void LogicalCameraSensor::Init()
+{
+ // Read configuration values
+ if (this->sdf->HasElement("logical_camera"))
+ {
+ sdf::ElementPtr cameraSdf = this->sdf->GetElement("logical_camera");
+
+ // These values are required in SDF, so no need to check for their
+ // existence.
+ this->dataPtr->frustum.SetNear(cameraSdf->Get<double>("near"));
+ this->dataPtr->frustum.SetFar(cameraSdf->Get<double>("far"));
+ this->dataPtr->frustum.SetFOV(cameraSdf->Get<double>("horizontal_fov"));
+ this->dataPtr->frustum.SetAspectRatio(
+ cameraSdf->Get<double>("aspect_ratio"));
+ }
+
+ Sensor::Init();
+}
+
+//////////////////////////////////////////////////
+void LogicalCameraSensor::Fini()
+{
+ Sensor::Fini();
+}
+
+//////////////////////////////////////////////////
+bool LogicalCameraSensor::UpdateImpl(bool _force)
+{
+ // Only compute if active, or the update is forced
+ if (_force || this->IsActive())
+ {
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+ this->dataPtr->msg.clear_model();
+
+ // Get the pose of the camera's parent.
+ ignition::math::Pose3d myPose = this->pose +
+ this->dataPtr->parentLink->GetWorldPose().Ign();
+
+ // Update the pose of the frustum.
+ this->dataPtr->frustum.SetPose(myPose);
+
+ // Set the camera's pose in the message.
+ msgs::Set(this->dataPtr->msg.mutable_pose(), myPose);
+
+ // Check all models for inclusion in the frustum.
+ for (auto const &model : this->world->GetModels())
+ {
+ // Add the the model to the output if it is in the frustum, and
+ // we are not detecting ourselves.
+ if (this->dataPtr->modelName != model->GetName() &&
+ this->dataPtr->frustum.Contains(model->GetBoundingBox().Ign()))
+ {
+ // Add new model msg
+ msgs::LogicalCameraImage::Model *modelMsg =
+ this->dataPtr->msg.add_model();
+
+ // Set the name and pose reported by the sensor.
+ modelMsg->set_name(model->GetScopedName());
+ msgs::Set(modelMsg->mutable_pose(),
+ model->GetWorldPose().Ign() - myPose);
+ }
+ }
+
+ // Send the message.
+ this->dataPtr->pub->Publish(this->dataPtr->msg);
+ }
+
+ return true;
+}
+
+//////////////////////////////////////////////////
+bool LogicalCameraSensor::IsActive()
+{
+ return Sensor::IsActive() ||
+ (this->dataPtr->pub && this->dataPtr->pub->HasConnections());
+}
+
+//////////////////////////////////////////////////
+double LogicalCameraSensor::Near() const
+{
+ return this->dataPtr->frustum.Near();
+}
+
+//////////////////////////////////////////////////
+double LogicalCameraSensor::Far() const
+{
+ return this->dataPtr->frustum.Far();
+}
+
+//////////////////////////////////////////////////
+ignition::math::Angle LogicalCameraSensor::HorizontalFOV() const
+{
+ return this->dataPtr->frustum.FOV();
+}
+
+//////////////////////////////////////////////////
+double LogicalCameraSensor::AspectRatio() const
+{
+ return this->dataPtr->frustum.AspectRatio();
+}
+
+//////////////////////////////////////////////////
+msgs::LogicalCameraImage LogicalCameraSensor::Image() const
+{
+ std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
+ return this->dataPtr->msg;
+}
diff --git a/gazebo/sensors/LogicalCameraSensor.hh b/gazebo/sensors/LogicalCameraSensor.hh
new file mode 100644
index 0000000..cc47371
--- /dev/null
+++ b/gazebo/sensors/LogicalCameraSensor.hh
@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef _GAZEBO_LOGICAL_CAMERASENSOR_HH_
+#define _GAZEBO_LOGICAL_CAMERASENSOR_HH_
+
+#include <string>
+#include <sdf/sdf.hh>
+
+#include "gazebo/sensors/Sensor.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+ namespace sensors
+ {
+ // Forward declare private data class
+ class LogicalCameraSensorPrivate;
+
+ /// \addtogroup gazebo_sensors Sensors
+ /// \{
+
+ /// \class LogicalCameraSensor LogicalCameraSensor.hh sensors/sensors.hh
+ /// \brief A camera sensor that reports locations of objects instead
+ /// of rendering a scene. This camera finds models in the sensor's
+ /// vicinity, and transmits information about the models on the sensor's
+ /// topic.
+ class GAZEBO_VISIBLE LogicalCameraSensor : public Sensor
+ {
+ /// \brief Constructor
+ public: LogicalCameraSensor();
+
+ /// \brief Destructor
+ public: virtual ~LogicalCameraSensor();
+
+ // Documentation inherited
+ public: virtual void Load(const std::string &_worldName,
+ sdf::ElementPtr _sdf);
+
+ // Documentation inherited
+ public: virtual void Load(const std::string &_worldName);
+
+ // Documentation inherited
+ public: virtual void Init();
+
+ // Documentation inherited
+ public: virtual std::string GetTopic() const;
+
+ /// \brief Get the near distance. This is the distance from the
+ /// frustum's vertex to the closest plane.
+ /// \return Near distance.
+ public: double Near() const;
+
+ /// \brief Get the far distance. This is the distance from the
+ /// frustum's vertex to the farthest plane.
+ /// \return Far distance.
+ public: double Far() const;
+
+ /// \brief Get the horizontal field of view. The field of view is the
+ /// angle between the frustum's vertex and the edges of the near or far
+ /// plane. This value represents the horizontal angle.
+ /// \return The field of view.
+ public: ignition::math::Angle HorizontalFOV() const;
+
+ /// \brief Get the aspect ratio, which is the width divided by height
+ /// of the near or far planes.
+ /// \return The frustum's aspect ratio.
+ public: double AspectRatio() const;
+
+ /// \brief Get the latest image. An image is an instance of
+ /// msgs::LogicalCameraImage, which contains a list of detected models.
+ /// \return List of detected models.
+ public: msgs::LogicalCameraImage Image() const;
+
+ // Documentation inherited
+ protected: virtual bool UpdateImpl(bool _force);
+
+ // \brief Finalize the logical camera
+ protected: virtual void Fini();
+
+ // Documentation inherited
+ public: virtual bool IsActive();
+
+ // \internal
+ // \brief Private data pointer
+ private: LogicalCameraSensorPrivate *dataPtr;
+ };
+ /// \}
+ }
+}
+#endif
diff --git a/gazebo/sensors/LogicalCameraSensorPrivate.hh b/gazebo/sensors/LogicalCameraSensorPrivate.hh
new file mode 100644
index 0000000..dfd42ce
--- /dev/null
+++ b/gazebo/sensors/LogicalCameraSensorPrivate.hh
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef _GAZEBO_LOGICAL_CAMERASENSOR_PRIVATE_HH_
+#define _GAZEBO_LOGICAL_CAMERASENSOR_PRIVATE_HH_
+
+#include <string>
+#include <ignition/math/Frustum.hh>
+#include "gazebo/transport/TransportTypes.hh"
+#include "gazebo/msgs/msgs.hh"
+#include "gazebo/physics/Link.hh"
+
+namespace gazebo
+{
+ namespace sensors
+ {
+ /// \internal
+ /// \brief Logical camera sensor private data.
+ class LogicalCameraSensorPrivate
+ {
+ /// \brief Publisher of msgs::LogicalCameraImage messages.
+ public: transport::PublisherPtr pub;
+
+ /// \brief Camera frustum.
+ public: ignition::math::Frustum frustum;
+
+ /// \brief Pointer to the parent link.
+ public: physics::LinkPtr parentLink;
+
+ /// \brief Used to store and report the detected models.
+ public: msgs::LogicalCameraImage msg;
+
+ /// \brief Mutex to protect the msg.
+ public: std::mutex mutex;
+
+ /// \brief Name of the parent model.
+ public: std::string modelName;
+ };
+ }
+}
+#endif
diff --git a/gazebo/sensors/Sensor.cc b/gazebo/sensors/Sensor.cc
index 758f67d..49fc8fc 100644
--- a/gazebo/sensors/Sensor.cc
+++ b/gazebo/sensors/Sensor.cc
@@ -38,6 +38,7 @@
#include "gazebo/rendering/Scene.hh"
#include "gazebo/sensors/CameraSensor.hh"
+#include "gazebo/sensors/LogicalCameraSensor.hh"
#include "gazebo/sensors/Noise.hh"
#include "gazebo/sensors/Sensor.hh"
#include "gazebo/sensors/SensorManager.hh"
@@ -362,7 +363,16 @@ void Sensor::FillMsg(msgs::Sensor &_msg)
_msg.set_visualize(this->GetVisualize());
_msg.set_topic(this->GetTopic());
- if (this->GetType() == "camera")
+ if (this->GetType() == "logical_camera")
+ {
+ LogicalCameraSensor *camSensor = static_cast<LogicalCameraSensor*>(this);
+ msgs::LogicalCameraSensor *camMsg = _msg.mutable_logical_camera();
+ camMsg->set_near(camSensor->Near());
+ camMsg->set_far(camSensor->Far());
+ camMsg->set_horizontal_fov(camSensor->HorizontalFOV().Radian());
+ camMsg->set_aspect_ratio(camSensor->AspectRatio());
+ }
+ else if (this->GetType() == "camera")
{
CameraSensor *camSensor = static_cast<CameraSensor*>(this);
msgs::CameraSensor *camMsg = _msg.mutable_camera();
diff --git a/gazebo/sensors/SensorFactory.cc b/gazebo/sensors/SensorFactory.cc
index ba160b4..500736f 100644
--- a/gazebo/sensors/SensorFactory.cc
+++ b/gazebo/sensors/SensorFactory.cc
@@ -34,18 +34,19 @@ void RegisterAltimeterSensor();
void RegisterCameraSensor();
void RegisterContactSensor();
void RegisterDepthCameraSensor();
-void RegisterMultiCameraSensor();
+void RegisterForceTorqueSensor();
void RegisterGpsSensor();
void RegisterGpuRaySensor();
void RegisterImuSensor();
+void RegisterLogicalCameraSensor();
void RegisterMagnetometerSensor();
+void RegisterMultiCameraSensor();
void RegisterRaySensor();
void RegisterRFIDSensor();
void RegisterRFIDTag();
void RegisterSonarSensor();
-void RegisterForceTorqueSensor();
-void RegisterWirelessTransmitter();
void RegisterWirelessReceiver();
+void RegisterWirelessTransmitter();
using namespace gazebo;
using namespace sensors;
@@ -59,18 +60,19 @@ void SensorFactory::RegisterAll()
RegisterCameraSensor();
RegisterContactSensor();
RegisterDepthCameraSensor();
- RegisterMultiCameraSensor();
+ RegisterForceTorqueSensor();
+ RegisterImuSensor();
RegisterGpsSensor();
RegisterGpuRaySensor();
- RegisterImuSensor();
+ RegisterLogicalCameraSensor();
+ RegisterMultiCameraSensor();
RegisterMagnetometerSensor();
RegisterRaySensor();
RegisterRFIDSensor();
RegisterRFIDTag();
RegisterSonarSensor();
- RegisterForceTorqueSensor();
- RegisterWirelessTransmitter();
RegisterWirelessReceiver();
+ RegisterWirelessTransmitter();
}
/////////////////////////////////////////////////
diff --git a/gazebo/sensors/SensorTypes.hh b/gazebo/sensors/SensorTypes.hh
index dd83564..bb6ae7b 100644
--- a/gazebo/sensors/SensorTypes.hh
+++ b/gazebo/sensors/SensorTypes.hh
@@ -34,6 +34,7 @@ namespace gazebo
class Sensor;
class RaySensor;
class CameraSensor;
+ class LogicalCameraSensor;
class MagnetometerSensor;
class MultiCameraSensor;
class DepthCameraSensor;
@@ -192,6 +193,10 @@ namespace gazebo
/// \brief Vector of WirelessReceiver
typedef std::vector<WirelessReceiver> WirelessReceiver_V;
+ /// \def LogicalCameraSensorPtr
+ /// \brief Shared pointer to LogicalCameraSensor
+ typedef boost::shared_ptr<LogicalCameraSensor> LogicalCameraSensorPtr;
+
/// \def SensorNoiseType
/// \brief Eumeration of all sensor noise types
enum SensorNoiseType
diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt
index 2a869c8..9e544b6 100644
--- a/plugins/CMakeLists.txt
+++ b/plugins/CMakeLists.txt
@@ -57,6 +57,7 @@ set (plugins
ModelPropShop
MudPlugin
PressurePlugin
+ RandomVelocityPlugin
RayPlugin
RaySensorNoisePlugin
RubblePlugin
diff --git a/plugins/RandomVelocityPlugin.cc b/plugins/RandomVelocityPlugin.cc
new file mode 100644
index 0000000..c340b89
--- /dev/null
+++ b/plugins/RandomVelocityPlugin.cc
@@ -0,0 +1,181 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#include <ignition/math/Rand.hh>
+#include <gazebo/common/Events.hh>
+#include <gazebo/common/Assert.hh>
+#include <gazebo/physics/Model.hh>
+#include <gazebo/physics/Link.hh>
+#include "RandomVelocityPluginPrivate.hh"
+#include "RandomVelocityPlugin.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_MODEL_PLUGIN(RandomVelocityPlugin)
+
+/////////////////////////////////////////////////
+RandomVelocityPlugin::RandomVelocityPlugin()
+ : dataPtr(new RandomVelocityPluginPrivate)
+{
+}
+
+/////////////////////////////////////////////////
+RandomVelocityPlugin::~RandomVelocityPlugin()
+{
+ delete this->dataPtr;
+}
+
+/////////////////////////////////////////////////
+void RandomVelocityPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+{
+ GZ_ASSERT(_model, "Model pointer is null");
+
+ // Make sure the link has been specified
+ if (!_sdf->HasElement("link"))
+ {
+ gzerr << "<link> element missing from RandomVelocity plugin. "
+ << "The plugin will not function.\n";
+ return;
+ }
+
+ // Get the link;
+ this->dataPtr->link = _model->GetLink(_sdf->Get<std::string>("link"));
+ if (!this->dataPtr->link)
+ {
+ gzerr << "Unable to find link[" << _sdf->Get<std::string>("link") << "] "
+ << "in model[" << _model->GetName() << "]. The RandomVelocity plugin "
+ << "will not function.\n";
+ return;
+ }
+
+ // Get x clamping values
+ if (_sdf->HasElement("min_x"))
+ this->dataPtr->xRange.X(_sdf->Get<double>("min_x"));
+ if (_sdf->HasElement("max_x"))
+ this->dataPtr->xRange.Y(_sdf->Get<double>("max_x"));
+
+ // Make sure min <= max
+ //
+ // Analysis:
+ // min_x max_x | result_X result_Y
+ // ----------------------------------
+ // 0: 0 0 | 0 0
+ // 1: 1 0 | 0 1
+ // 2: 0 1 | 0 1
+ //
+ // Line 1 above is the error case, and can be handled different ways:
+ // a: Throw an exception
+ // b: Set both min and max values to the same value (either min_x or
+ // max_x)
+ // c: Fix it by swapping the values
+ // d: Do nothing, which would:
+ // i. First clamp the value to <= max_x
+ // ii. Second clamp the value to >= min_x
+ // iii. The result would always be the value of min_x
+ //
+ // We've opted for option c, which seems the most resonable.
+ ignition::math::Vector2d tmp = this->dataPtr->xRange;
+ this->dataPtr->xRange.X(std::min(tmp.X(), tmp.Y()));
+ this->dataPtr->xRange.Y(std::max(tmp.X(), tmp.Y()));
+
+ // Get y clamping values
+ if (_sdf->HasElement("min_y"))
+ this->dataPtr->yRange.X(_sdf->Get<double>("min_y"));
+ if (_sdf->HasElement("max_y"))
+ this->dataPtr->yRange.Y(_sdf->Get<double>("max_y"));
+
+ // Make sure min <= max
+ tmp = this->dataPtr->yRange;
+ this->dataPtr->yRange.X(std::min(tmp.X(), tmp.Y()));
+ this->dataPtr->yRange.Y(std::max(tmp.X(), tmp.Y()));
+
+ // Get z clamping values
+ if (_sdf->HasElement("min_z"))
+ this->dataPtr->zRange.X(_sdf->Get<double>("min_z"));
+ if (_sdf->HasElement("max_z"))
+ this->dataPtr->zRange.Y(_sdf->Get<double>("max_z"));
+
+ // Make sure min <= max
+ tmp = this->dataPtr->zRange;
+ this->dataPtr->zRange.X(std::min(tmp.X(), tmp.Y()));
+ this->dataPtr->zRange.Y(std::max(tmp.X(), tmp.Y()));
+
+ // Set the initial velocity, if present
+ if (_sdf->HasElement("initial_velocity"))
+ {
+ this->dataPtr->velocity =
+ _sdf->Get<ignition::math::Vector3d>("initial_velocity");
+ }
+
+ // Set the velocity factor
+ if (_sdf->HasElement("velocity_factor"))
+ this->dataPtr->velocityFactor = _sdf->Get<double>("velocity_factor");
+
+ // Set the update period
+ if (_sdf->HasElement("update_period"))
+ this->dataPtr->updatePeriod = _sdf->Get<double>("update_period");
+
+ // Connect to the world update signal
+ this->dataPtr->updateConnection = event::Events::ConnectWorldUpdateBegin(
+ std::bind(&RandomVelocityPlugin::Update, this, std::placeholders::_1));
+}
+
+/////////////////////////////////////////////////
+void RandomVelocityPlugin::Reset()
+{
+ this->dataPtr->prevUpdate.Set(0, 0);
+}
+
+/////////////////////////////////////////////////
+void RandomVelocityPlugin::Update(const common::UpdateInfo &_info)
+{
+ GZ_ASSERT(this->dataPtr->link, "<link> in RandomVelocity plugin is null");
+
+ // Short-circuit in case the link is invalid.
+ if (!this->dataPtr->link)
+ return;
+
+ // Change direction when enough time has elapsed
+ if (_info.simTime - this->dataPtr->prevUpdate > this->dataPtr->updatePeriod)
+ {
+ // Get a random velocity value.
+ this->dataPtr->velocity.Set(
+ ignition::math::Rand::DblUniform(-1, 1),
+ ignition::math::Rand::DblUniform(-1, 1),
+ ignition::math::Rand::DblUniform(-1, 1));
+
+ // Apply scaling factor
+ this->dataPtr->velocity.Normalize();
+ this->dataPtr->velocity *= this->dataPtr->velocityFactor;
+
+ // Clamp X value
+ this->dataPtr->velocity.X(ignition::math::clamp(this->dataPtr->velocity.X(),
+ this->dataPtr->xRange.X(), this->dataPtr->xRange.Y()));
+
+ // Clamp Y value
+ this->dataPtr->velocity.Y(ignition::math::clamp(this->dataPtr->velocity.Y(),
+ this->dataPtr->yRange.X(), this->dataPtr->yRange.Y()));
+
+ // Clamp Z value
+ this->dataPtr->velocity.Z(ignition::math::clamp(this->dataPtr->velocity.Z(),
+ this->dataPtr->zRange.X(), this->dataPtr->zRange.Y()));
+
+ this->dataPtr->prevUpdate = _info.simTime;
+ }
+
+ // Apply velocity
+ this->dataPtr->link->SetLinearVel(this->dataPtr->velocity);
+}
diff --git a/plugins/RandomVelocityPlugin.hh b/plugins/RandomVelocityPlugin.hh
new file mode 100644
index 0000000..2a0f6b9
--- /dev/null
+++ b/plugins/RandomVelocityPlugin.hh
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef _GAZEBO_RANDOMVELOCITY_PLUGIN_HH_
+#define _GAZEBO_RANDOMVELOCITY_PLUGIN_HH_
+
+#include <sdf/sdf.hh>
+#include <gazebo/common/UpdateInfo.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/util/system.hh>
+
+namespace gazebo
+{
+ // Forward declare private data class.
+ class RandomVelocityPrivate;
+
+ /// \brief Plugin that applies a random velocity to a linke periodically.
+ ///
+ /// \verbatim
+ /// <!-- Apply a random velocity to the specified link-->
+ /// <!-- In this example, we move a box around. A key property of the link
+ /// is the frictionless surface -->
+ /// <plugin name="random" filename="libRandomVelocityPlugin.so">
+ ///
+ /// <!-- Name of the link in this model that receives the velocity -->
+ /// <link>link</link>
+ ///
+ /// <!-- Initial velocity that is applied to the link -->
+ /// <initial_velocity>0 0.5 0</initial_velocity>
+ ///
+ /// <!-- Scaling factor that is used to compute a new velocity -->
+ /// <velocity_factor>0.5</velocity_factor>
+ ///
+ /// <!-- Time, in seconds, between new velocities -->
+ /// <update_period>5</update_period>
+ ///
+ /// <!-- Clamp the X velocity value to between -1 and 1.-->
+ /// <min_x>-1</min_x>
+ /// <max_x>1</max_x>
+ ///
+ /// <!-- Clamp the Y velocity value to between 0 and 1.-->
+ /// <min_y>0</min_y>
+ /// <max_y>1</max_y>
+ ///
+ /// <!-- Clamp the Z velocity value to zero.-->
+ /// <min_z>0</min_z>
+ /// <max_z>0</max_z>
+ /// </plugin>
+ /// \endverbatim
+ ///
+ /// See worlds/random_velocity.world for a complete example.
+ class GAZEBO_VISIBLE RandomVelocityPlugin : public ModelPlugin
+ {
+ /// \brief Constructor.
+ public: RandomVelocityPlugin();
+
+ /// \brief Destructor.
+ public: ~RandomVelocityPlugin();
+
+ // Documentation inherited
+ public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+ // Documentation inherited
+ public: virtual void Reset();
+
+ /// \brief Update the plugin once every iteration of simulation.
+ /// \param[in] _info World update information.
+ private: void Update(const common::UpdateInfo &_info);
+
+ /// \internal
+ /// \brief Private data pointer
+ private: RandomVelocityPluginPrivate *dataPtr;
+ };
+}
+#endif
diff --git a/plugins/RandomVelocityPluginPrivate.hh b/plugins/RandomVelocityPluginPrivate.hh
new file mode 100644
index 0000000..991e765
--- /dev/null
+++ b/plugins/RandomVelocityPluginPrivate.hh
@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef _GAZEBO_RANDOMVELOCITY_PLUGIN_PRIVATE_HH_
+#define _GAZEBO_RANDOMVELOCITY_PLUGIN_PRIVATE_HH_
+
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/Vector2.hh>
+
+#include <gazebo/physics/Link.hh>
+#include <gazebo/common/Time.hh>
+
+namespace gazebo
+{
+ /// \internal
+ /// \brief Private data for the RandomVelocityPlugin.
+ class RandomVelocityPluginPrivate
+ {
+ public: RandomVelocityPluginPrivate()
+ : velocityFactor(1.0),
+ updatePeriod(10, 0),
+ xRange(-IGN_DBL_MAX, IGN_DBL_MAX),
+ yRange(-IGN_DBL_MAX, IGN_DBL_MAX),
+ zRange(-IGN_DBL_MAX, IGN_DBL_MAX)
+ {
+ }
+
+ /// \brief Velocity scaling factor.
+ public: double velocityFactor;
+
+ /// \brief Time between recomputing a new velocity vector
+ public: common::Time updatePeriod;
+
+ /// \brief Time the of the last update.
+ public: common::Time prevUpdate;
+
+ /// \brief Velocity to apply.
+ public: ignition::math::Vector3d velocity;
+
+ /// \brief Connects to world update event.
+ public: event::ConnectionPtr updateConnection;
+
+ /// \brief X velocity clamping values
+ public: ignition::math::Vector2d xRange;
+
+ /// \brief Y velocity clamping values
+ public: ignition::math::Vector2d yRange;
+
+ /// \brief Z velocity clamping values
+ public: ignition::math::Vector2d zRange;
+
+ /// \brief Pointer to the link that will receive the velocity.
+ public: physics::LinkPtr link;
+ };
+}
+#endif
diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 53353d7..44b4e22 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -55,6 +55,7 @@ set(tests
joint_test.cc
joint_universal.cc
laser.cc
+ logical_camera_sensor.cc
model.cc
model_database.cc
noise.cc
diff --git a/test/integration/logical_camera_sensor.cc b/test/integration/logical_camera_sensor.cc
new file mode 100644
index 0000000..8c597aa
--- /dev/null
+++ b/test/integration/logical_camera_sensor.cc
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2015 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include "gazebo/sensors/sensors.hh"
+#include "gazebo/sensors/LogicalCameraSensor.hh"
+
+#include "gazebo/test/ServerFixture.hh"
+
+using namespace gazebo;
+class LogicalCameraSensor : public ServerFixture
+{
+};
+
+/////////////////////////////////////////////////
+TEST_F(LogicalCameraSensor, GroundPlane)
+{
+ Load("worlds/logical_camera.world");
+
+ /// \brief Wait until the sensors have been initialized
+ while (!sensors::SensorManager::Instance()->SensorsInitialized())
+ common::Time::MSleep(1000);
+
+ sensors::LogicalCameraSensorPtr cam = boost::dynamic_pointer_cast<
+ sensors::LogicalCameraSensor>(sensors::get_sensor("logical_camera"));
+ ASSERT_TRUE(cam != NULL);
+
+ // Check that the camera parameters are correct
+ EXPECT_NEAR(cam->Near(), 0.55, 1e-4);
+ EXPECT_NEAR(cam->Far(), 5.0, 1e-4);
+ EXPECT_NEAR(cam->HorizontalFOV().Radian(), 1.04719755, 1e-4);
+ EXPECT_NEAR(cam->AspectRatio(), 1.778, 1e-4);
+ EXPECT_TRUE(cam->IsActive());
+
+ // Update, but do not force the update
+ cam->Update(false);
+ EXPECT_EQ(cam->Image().model_size(), 0);
+
+ // Force the update
+ cam->Update(true);
+
+ // We should now detect the ground plane
+ ASSERT_EQ(cam->Image().model_size(), 1);
+ EXPECT_EQ(cam->Image().model(0).name(), "ground_plane");
+}
+
+/////////////////////////////////////////////////
+TEST_F(LogicalCameraSensor, Box)
+{
+ Load("worlds/logical_camera.world");
+
+ /// \brief Wait until the sensors have been initialized
+ while (!sensors::SensorManager::Instance()->SensorsInitialized())
+ common::Time::MSleep(1000);
+
+ // Get the world
+ physics::WorldPtr world = physics::get_world("default");
+ ASSERT_TRUE(world != NULL);
+
+ // Get the model that has the logical camera
+ physics::ModelPtr cameraModel = world->GetModel("box");
+ ASSERT_TRUE(cameraModel != NULL);
+
+ // Get the logical camera sensor
+ sensors::LogicalCameraSensorPtr cam = boost::dynamic_pointer_cast<
+ sensors::LogicalCameraSensor>(sensors::get_sensor("logical_camera"));
+ ASSERT_TRUE(cam != NULL);
+
+ // Force the update
+ cam->Update(true);
+
+ // We should now detect the ground plane
+ ASSERT_EQ(cam->Image().model_size(), 1);
+ EXPECT_EQ(cam->Image().model(0).name(), "ground_plane");
+
+ // Insert box
+ SpawnBox("spawn_box", math::Vector3(1, 1, 1), math::Vector3(2, 0, 0.5),
+ math::Vector3::Zero);
+ cam->Update(true);
+
+ ASSERT_EQ(cam->Image().model_size(), 2);
+ EXPECT_EQ(cam->Image().model(1).name(), "spawn_box");
+
+ // Rotate the model, which should move "spawn_box" out of the frustum
+ cameraModel->SetWorldPose(math::Pose(0, 0, 0, 0, 0, 1.5707));
+ cam->Update(true);
+ ASSERT_EQ(cam->Image().model_size(), 1);
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char **argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/integration/physics_friction.cc b/test/integration/physics_friction.cc
index 878fcc2..8c722df 100644
--- a/test/integration/physics_friction.cc
+++ b/test/integration/physics_friction.cc
@@ -30,7 +30,7 @@ const double g_friction_tolerance = 1e-3;
class PhysicsFrictionTest : public ServerFixture,
public testing::WithParamInterface<const char*>
{
- protected: PhysicsFrictionTest() : ServerFixture(), spawnCount(0)
+ protected: PhysicsFrictionTest() : ServerFixture()
{
}
@@ -78,22 +78,22 @@ class PhysicsFrictionTest : public ServerFixture,
}
/// \brief Size of box to spawn.
- public: math::Vector3 size;
+ public: ignition::math::Vector3d size;
/// \brief Mass of box to spawn (inertia computed automatically).
public: double mass;
/// \brief Model pose.
- public: math::Pose modelPose;
+ public: ignition::math::Pose3d modelPose;
/// \brief Link pose.
- public: math::Pose linkPose;
+ public: ignition::math::Pose3d linkPose;
/// \brief Inertial pose.
- public: math::Pose inertialPose;
+ public: ignition::math::Pose3d inertialPose;
/// \brief Collision pose.
- public: math::Pose collisionPose;
+ public: ignition::math::Pose3d collisionPose;
/// \brief Friction coefficient in primary direction.
public: double friction1;
@@ -102,93 +102,37 @@ class PhysicsFrictionTest : public ServerFixture,
public: double friction2;
/// \brief Primary friction direction.
- public: math::Vector3 direction1;
+ public: ignition::math::Vector3d direction1;
};
/// \brief Spawn a box with friction coefficients and direction.
/// \param[in] _opt Options for friction box.
public: physics::ModelPtr SpawnBox(const SpawnFrictionBoxOptions &_opt)
{
- msgs::Factory msg;
- std::ostringstream modelStr;
- std::ostringstream modelName;
- modelName << "box_model" << this->spawnCount++;
-
- double dx = _opt.size.x;
- double dy = _opt.size.y;
- double dz = _opt.size.z;
- double ixx = _opt.mass/12.0 * (dy*dy + dz*dz);
- double iyy = _opt.mass/12.0 * (dz*dz + dx*dx);
- double izz = _opt.mass/12.0 * (dx*dx + dy*dy);
-
- modelStr
- << "<sdf version='" << SDF_VERSION << "'>"
- << "<model name ='" << modelName.str() << "'>"
- << " <pose>" << _opt.modelPose << "</pose>"
- << " <link name='link'>"
- << " <pose>" << _opt.linkPose << "</pose>"
- << " <inertial>"
- << " <pose>" << _opt.inertialPose << "</pose>"
- << " <mass>" << _opt.mass << "</mass>"
- << " <inertia>"
- << " <ixx>" << ixx << "</ixx>"
- << " <iyy>" << iyy << "</iyy>"
- << " <izz>" << izz << "</izz>"
- << " <ixy>" << 0.0 << "</ixy>"
- << " <ixz>" << 0.0 << "</ixz>"
- << " <iyz>" << 0.0 << "</iyz>"
- << " </inertia>"
- << " </inertial>"
- << " <collision name='collision'>"
- << " <pose>" << _opt.collisionPose << "</pose>"
- << " <geometry>"
- << " <box><size>" << _opt.size << "</size></box>"
- << " </geometry>"
- << " <surface>"
- << " <friction>"
- << " <ode>"
- << " <mu>" << _opt.friction1 << "</mu>"
- << " <mu2>" << _opt.friction2 << "</mu2>"
- << " <fdir1>" << _opt.direction1 << "</fdir1>"
- << " </ode>"
- << " </friction>"
- << " </surface>"
- << " </collision>"
- << " <visual name='visual'>"
- << " <pose>" << _opt.collisionPose << "</pose>"
- << " <geometry>"
- << " <box><size>" << _opt.size << "</size></box>"
- << " </geometry>"
- << " </visual>"
- << " </link>"
- << "</model>"
- << "</sdf>";
-
- physics::WorldPtr world = physics::get_world("default");
- world->InsertModelString(modelStr.str());
-
- physics::ModelPtr model;
- common::Time wait(100, 0);
-
- common::Time wallStart = common::Time::GetWallTime();
- unsigned int waitCount = 0;
- while (wait > (common::Time::GetWallTime() - wallStart) &&
- !this->HasEntity(modelName.str()))
+ std::string modelName = this->GetUniqueString("box_model");
+
+ msgs::Model model;
+ model.set_name(modelName);
+ msgs::Set(model.mutable_pose(), _opt.modelPose);
+
+ msgs::AddBoxLink(model, _opt.mass, _opt.size);
+ auto link = model.mutable_link(0);
+ msgs::Set(link->mutable_pose(), _opt.linkPose);
+
{
- common::Time::MSleep(10);
- if (++waitCount % 100 == 0)
- {
- gzwarn << "Waiting " << waitCount / 100 << " seconds for "
- << "box to spawn." << std::endl;
- }
+ auto inertial = link->mutable_inertial();
+ msgs::Set(inertial->mutable_pose(), _opt.inertialPose);
}
- if (this->HasEntity(modelName.str()) && waitCount >= 100)
- gzwarn << "box has spawned." << std::endl;
- if (world != NULL)
- model = world->GetModel(modelName.str());
+ auto collision = link->mutable_collision(0);
+ msgs::Set(collision->mutable_pose(), _opt.collisionPose);
+
+ auto friction = collision->mutable_surface()->mutable_friction();
+ friction->set_mu(_opt.friction1);
+ friction->set_mu2(_opt.friction2);
+ msgs::Set(friction->mutable_fdir1(), _opt.direction1);
- return model;
+ return ServerFixture::SpawnModel(model);
}
/// \brief Use the friction_demo world.
@@ -211,9 +155,6 @@ class PhysicsFrictionTest : public ServerFixture,
/// no NaN's are generated.
/// \param[in] _physicsEngine Physics engine to use.
public: void DirectionNaN(const std::string &_physicsEngine);
-
- /// \brief Count of spawned models, used to ensure unique model names.
- private: unsigned int spawnCount;
};
class WorldStepFrictionTest : public PhysicsFrictionTest
@@ -369,28 +310,29 @@ void PhysicsFrictionTest::MaximumDissipation(const std::string &_physicsEngine)
// Compute angle for each box
double radius = 9.0 + ring;
double angle = 2*M_PI*static_cast<double>(i) / static_cast<double>(boxes);
- opt.modelPose.pos.Set(radius*cos(angle), radius*sin(angle), dz/2);
+ opt.modelPose.Pos().Set(radius*cos(angle), radius*sin(angle), dz/2);
if (ring == 0)
- opt.direction1 = math::Vector3(-sin(angle), cos(angle), 0);
+ opt.direction1 = ignition::math::Vector3d(-sin(angle), cos(angle), 0);
else if (ring < 4)
- opt.direction1 = math::Vector3(0.0, 1.0, 0.0);
+ opt.direction1 = ignition::math::Vector3d(0.0, 1.0, 0.0);
if (ring == 1)
- opt.collisionPose.rot.SetFromEuler(0.0, 0.0, angle);
+ opt.collisionPose.Rot().Euler(0.0, 0.0, angle);
if (ring == 2)
- opt.linkPose.rot.SetFromEuler(0.0, 0.0, angle);
+ opt.linkPose.Rot().Euler(0.0, 0.0, angle);
if (ring == 3)
- opt.modelPose.rot.SetFromEuler(0.0, 0.0, angle);
+ opt.modelPose.Rot().Euler(0.0, 0.0, angle);
physics::ModelPtr model = SpawnBox(opt);
ASSERT_TRUE(model != NULL);
modelAngles[model] = angle;
// Set velocity, larger for outer rings.
- model->SetLinearVel(radius * math::Vector3(cos(angle), sin(angle), 0));
+ model->SetLinearVel(
+ radius * ignition::math::Vector3d(cos(angle), sin(angle), 0));
}
}
@@ -472,21 +414,21 @@ void PhysicsFrictionTest::BoxDirectionRing(const std::string &_physicsEngine)
// Compute angle for each box
double radius = 5.0 + ring;
double angle = M_PI*static_cast<double>(i) / static_cast<double>(boxes);
- opt.modelPose.pos.Set(radius*cos(angle), radius*sin(angle), dz/2);
+ opt.modelPose.Pos().Set(radius*cos(angle), radius*sin(angle), dz/2);
if (ring == 0)
- opt.direction1 = math::Vector3(-sin(angle), cos(angle), 0);
+ opt.direction1 = ignition::math::Vector3d(-sin(angle), cos(angle), 0);
else
- opt.direction1 = math::Vector3(0.0, 1.0, 0.0);
+ opt.direction1 = ignition::math::Vector3d(0.0, 1.0, 0.0);
if (ring == 1)
- opt.collisionPose.rot.SetFromEuler(0.0, 0.0, angle);
+ opt.collisionPose.Rot().Euler(0.0, 0.0, angle);
if (ring == 2)
- opt.linkPose.rot.SetFromEuler(0.0, 0.0, angle);
+ opt.linkPose.Rot().Euler(0.0, 0.0, angle);
if (ring == 3)
- opt.modelPose.rot.SetFromEuler(0.0, 0.0, angle);
+ opt.modelPose.Rot().Euler(0.0, 0.0, angle);
physics::ModelPtr model = SpawnBox(opt);
ASSERT_TRUE(model != NULL);
@@ -561,8 +503,8 @@ void PhysicsFrictionTest::DirectionNaN(const std::string &_physicsEngine)
// Set box size and anisotropic friction
SpawnFrictionBoxOptions opt;
opt.size.Set(dx, dy, dz);
- opt.direction1 = math::Vector3(0.0, 0.0, 1.0);
- opt.modelPose.pos.z = dz/2;
+ opt.direction1 = ignition::math::Vector3d(0.0, 0.0, 1.0);
+ opt.modelPose.Pos().Z(dz/2);
physics::ModelPtr model = SpawnBox(opt);
ASSERT_TRUE(model != NULL);
diff --git a/tools/gz_TEST.cc b/tools/gz_TEST.cc
index df0a82b..10fae60 100644
--- a/tools/gz_TEST.cc
+++ b/tools/gz_TEST.cc
@@ -641,7 +641,7 @@ TEST_F(gzTest, SDF)
docSums["1.2"] = "27f9d91080ce8aa18eac27c9d899fde2d4b78785";
docSums["1.3"] = "ad80986d42eae97baf277118f52d7e8b951d8ea1";
docSums["1.4"] = "153ddd6ba6797c37c7fcddb2be5362c9969d97a1";
- docSums["1.5"] = "1ccc4861895a2eb331de76a2aa92da5a98b45273";
+ docSums["1.5"] = "73b73f2735debfb86ca8361009ca32e3e0712ed4";
// Test each docSum
for (std::map<std::string, std::string>::iterator iter = docSums.begin();
diff --git a/worlds/CMakeLists.txt b/worlds/CMakeLists.txt
index 32d93c4..88ed275 100644
--- a/worlds/CMakeLists.txt
+++ b/worlds/CMakeLists.txt
@@ -44,6 +44,7 @@ set (files
projector.world
quad_rotor_demo.world
quad_rotor_demo_2.world
+ random_velocity.world
ray_cpu.world
ray_noise_plugin.world
road.world
diff --git a/worlds/logical_camera.world b/worlds/logical_camera.world
new file mode 100644
index 0000000..186fbc2
--- /dev/null
+++ b/worlds/logical_camera.world
@@ -0,0 +1,47 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <model name="box">
+ <pose>0 0 0.5 0 0 0</pose>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <sensor name="logical_camera" type="logical_camera">
+
+ <logical_camera>
+ <near>0.55</near>
+ <far>5</far>
+ <horizontal_fov>1.04719755</horizontal_fov>
+ <aspect_ratio>1.778</aspect_ratio>
+ </logical_camera>
+
+ <visualize>true</visualize>
+ <always_on>true</always_on>
+ <update_rate>10</update_rate>
+ </sensor>
+ </link>
+ </model>
+
+ </world>
+</sdf>
diff --git a/worlds/random_velocity.world b/worlds/random_velocity.world
new file mode 100644
index 0000000..8b6bd2b
--- /dev/null
+++ b/worlds/random_velocity.world
@@ -0,0 +1,65 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <world name="default">
+ <!-- A global light source -->
+ <include>
+ <uri>model://sun</uri>
+ </include>
+ <!-- A ground plane -->
+ <include>
+ <uri>model://ground_plane</uri>
+ </include>
+
+ <model name="box">
+ <pose>0 0 0.5 0 0 0</pose>
+ <link name="link">
+ <kinematic>true</kinematic>
+ <collision name="collision">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>0.0</mu>
+ <mu2>0.0</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <box>
+ <size>1 1 1</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <!-- Apply a random velocity to the specified link-->
+ <!-- In this example, we move a box around. A key property of the link
+ is the frictionless surface -->
+ <plugin name="random" filename="libRandomVelocityPlugin.so">
+
+ <!-- Name of the link in this model that receives the velocity -->
+ <link>link</link>
+
+ <!-- Initial velocity that is applied to the link -->
+ <initial_velocity>0 0.5 0</initial_velocity>
+
+ <!-- Scaling factor that is used to compute a new velocity -->
+ <velocity_factor>0.5</velocity_factor>
+
+ <!-- Time, in seconds, between new velocities -->
+ <update_period>5</update_period>
+
+ <!-- Clamp the Z velocity value to zero. You can also clamp x and
+ y values -->
+ <min_z>0</min_z>
+ <max_z>0</max_z>
+ </plugin>
+ </model>
+ </world>
+</sdf>
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/gazebo.git
More information about the debian-science-commits
mailing list