Commit c77ced0d authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

Added support for Gazebo >8

parent a5500d64
......@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
std_msgs
gazebo_version_helpers
)
find_package(gazebo REQUIRED)
......@@ -30,8 +31,8 @@ find_package(gazebo REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES gazebo_grasp_fix
CATKIN_DEPENDS gazebo_ros geometry_msgs roscpp std_msgs
DEPENDS gazebo_ros
CATKIN_DEPENDS gazebo_ros geometry_msgs roscpp std_msgs gazebo_version_helpers
DEPENDS gazebo
)
###########
......@@ -59,8 +60,8 @@ include_directories(
)
link_directories(
${GAZEBO_LIBRARY_DIRS}
${catkin_LIBRARY_DIRS}
${GAZEBO_LIBRARY_DIRS}
)
## Declare a cpp library
......
......@@ -8,7 +8,6 @@
#include <gazebo/transport/TransportTypes.hh>
#include <stdio.h>
#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
// #include <gazebo_grasp_plugin/CollidingPoint.h>
namespace gazebo
{
......@@ -114,8 +113,8 @@ class GazeboGraspFix : public ModelPlugin
/**
* Collects for each object all forces which are currently applied on it.
* Then, for each object, checks whether of all the forces applied,
* there are opposing forces. This is done by calling CheckGrip() with the
* list of all forces applied.
* there are opposing forces.
* This is done by calling CheckGrip() with the list of all forces applied.
* If CheckGrip() returns true, the number of "grip counts"
* is increased for the holding arm (but grip counts will never exceed \e max_grip_count).
* If the number of grip counts for this object exceeds \e grip_count_threshold,
......@@ -152,13 +151,8 @@ class GazeboGraspFix : public ModelPlugin
*/
void OnContact(const ConstContactsPtr &ptr);
/**
* Checks whether any two vectors in the set have an angle greater
* than minAngleDiff (in rad), and one is at least
* lengthRatio (0..1) of the other in it's length.
*/
bool CheckGrip(const std::vector<math::Vector3> &forces, float minAngleDiff,
float lengthRatio);
// bool CheckGrip(const std::vector<GzVector3> &forces, float minAngleDiff,
// float lengthRatio);
bool IsGripperLink(const std::string &linkName, std::string &gripperName) const;
......
......@@ -34,12 +34,13 @@
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>gazebo_version_helpers</build_depend>
<run_depend>gazebo_ros</run_depend>
<run_depend>gazebo</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>gazebo_version_helpers</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
......
......@@ -7,9 +7,10 @@
#include <stdio.h>
#include <gazebo_grasp_plugin/GazeboGraspFix.h>
#include <gazebo_version_helpers/GazeboVersionHelpers.h>
using gazebo::GazeboGraspFix;
using gazebo::GzVector3;
#define DEFAULT_FORCES_ANGLE_TOLERANCE 120
#define DEFAULT_UPDATE_RATE 5
......@@ -255,10 +256,10 @@ void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
// ++++++++++++ start up things +++++++++++++++
physics::PhysicsEnginePtr physics = this->world->GetPhysicsEngine();
this->node->Init(this->world->GetName());
physics::PhysicsEnginePtr physics = GetPhysics(this->world);
this->node->Init(gazebo::GetName(*(this->world)));
physics::ContactManager *contactManager = physics->GetContactManager();
contactManager->PublishContacts(); //XXX not sure I need this?
contactManager->PublishContacts(); // TODO not sure this is required?
std::string topic = contactManager->CreateFilter(model->GetScopedName(),
collisionNames);
......@@ -280,7 +281,7 @@ class GazeboGraspFix::ObjectContactInfo
public:
// all forces effecting on the object
std::vector<gazebo::math::Vector3> appliedForces;
std::vector<GzVector3> appliedForces;
// all grippers involved in the process, along with
// a number counting the number of contact points with the
......@@ -404,15 +405,15 @@ class GazeboGraspFix::CollidingPoint
physics::CollisionPtr collLink, collObj;
// average force vector of the colliding point
gazebo::math::Vector3 force;
GzVector3 force;
// position (relative to reference frame of gripper
// collision surface) where the contact happens on collision surface
gazebo::math::Vector3 pos;
GzVector3 pos;
// position (relative to reference frame of *gripper* collision surface)
// where the object center is located during collision.
gazebo::math::Vector3 objPos;
GzVector3 objPos;
// sum of force and pose (they are actually summed
// up from several contact points).
......@@ -420,6 +421,71 @@ class GazeboGraspFix::CollidingPoint
int sum;
};
////////////////////////////////////////////////////////////////////////////////
double AngularDistance(const GzVector3 &_v1,
const GzVector3 &_v2)
{
GzVector3 v1 = _v1;
GzVector3 v2 = _v2;
v1.Normalize();
v2.Normalize();
return acos(v1.Dot(v2));
}
////////////////////////////////////////////////////////////////////////////////
// Checks whether any two vectors in the set have an angle greater
// than minAngleDiff (in rad), and one is at least
// lengthRatio (0..1) of the other in it's length.
bool CheckGrip(const std::vector<GzVector3> &forces,
float minAngleDiff, float lengthRatio)
{
if (((lengthRatio > 1) || (lengthRatio < 0)) && (lengthRatio > 1e-04
&& (fabs(lengthRatio - 1) > 1e-04)))
{
std::cerr << "ERROR: CheckGrip: always specify a lengthRatio of [0..1]" <<
std::endl;
return false;
}
if (minAngleDiff < M_PI_2)
{
std::cerr << "ERROR: CheckGrip: min angle must be at least 90 degrees (PI/2)" <<
std::endl;
return false;
}
std::vector<GzVector3>::const_iterator it1, it2;
for (it1 = forces.begin(); it1 != forces.end(); ++it1)
{
GzVector3 v1 = *it1;
for (it2 = it1 + 1; it2 != forces.end(); ++it2)
{
GzVector3 v2 = *it2;
float l1 = gazebo::GetLength(v1);
float l2 = gazebo::GetLength(v2);
if ((l1 < 1e-04) || (l2 < 1e-04)) continue;
/*GzVector3 _v1=v1;
GzVector3 _v2=v2;
_v1/=l1;
_v2/=l2;
float angle=acos(_v1.Dot(_v2));*/
float angle = AngularDistance(v1, v2);
// gzmsg<<"Angular distance between v1.len="<<v1.GetLength()<<" and v2.len="<<v2.GetLength()<<": "<<angle*180/M_PI<<std::endl;
if (angle > minAngleDiff)
{
float ratio;
if (l1 > l2) ratio = l2 / l1;
else ratio = l1 / l2;
// gzmsg<<"Got angle "<<angle<<", ratio "<<ratio<<std::endl;
if (ratio >= lengthRatio)
{
// gzmsg<<"CheckGrip() is true"<<std::endl;
return true;
}
}
}
}
return false;
}
////////////////////////////////////////////////////////////////////////////////
void GazeboGraspFix::OnUpdate()
{
......@@ -455,7 +521,7 @@ void GazeboGraspFix::OnUpdate()
{
std::string linkName = lIt->first;
CollidingPoint &collP = lIt->second;
gazebo::math::Vector3 avgForce = collP.force / collP.sum;
GzVector3 avgForce = collP.force / collP.sum;
// gzmsg << "Found collision with "<<linkName<<": "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" (avg over "<<collP.sum<<")"<<std::endl;
objContInfo.appliedForces.push_back(avgForce);
// insert the gripper (if it doesn't exist yet) and increase contact counter
......@@ -657,46 +723,45 @@ void GazeboGraspFix::OnUpdate()
{
CollidingPoint &cpInfo = pointIt->second;
// initial distance from link to contact point (relative to link)
gazebo::math::Vector3 relContactPos = cpInfo.pos / cpInfo.sum;
// initial distance from link to object (relative to link)
gazebo::math::Vector3 relObjPos = cpInfo.objPos / cpInfo.sum;
GzVector3 relContactPos = cpInfo.pos / cpInfo.sum;
// Initial distance from link to object (relative to link)
GzVector3 relObjPos = cpInfo.objPos / cpInfo.sum;
// get current world pose of object
gazebo::math::Pose currObjWorldPose = cpInfo.collObj->GetLink()->GetWorldPose();
// Get current world pose of object
GzPose3 currObjWorldPose =
gazebo::GetWorldPose(cpInfo.collObj->GetLink());
// get world pose of link
gazebo::math::Pose currLinkWorldPose =
cpInfo.collLink->GetLink()->GetWorldPose();
// Get world pose of link
GzPose3 currLinkWorldPose =
gazebo::GetWorldPose(cpInfo.collLink->GetLink());
// Get transform for currLinkWorldPose as matrix
gazebo::math::Matrix4 worldToLink = currLinkWorldPose.rot.GetAsMatrix4();
worldToLink.SetTranslate(currLinkWorldPose.pos);
GzMatrix4 worldToLink = gazebo::GetMatrix(currLinkWorldPose);
// Get the transform from collision link to contact point
gazebo::math::Matrix4 linkToContact = gazebo::math::Matrix4::IDENTITY;
linkToContact.SetTranslate(relContactPos);
GzMatrix4 linkToContact = gazebo::GetMatrix(relContactPos);
// the current world position of the contact point right now is:
gazebo::math::Matrix4 _currContactWorldPose = worldToLink * linkToContact;
gazebo::math::Vector3 currContactWorldPose =
_currContactWorldPose.GetTranslation();
// The current world position of the contact point right now is:
GzMatrix4 _currContactWorldPose = worldToLink * linkToContact;
GzVector3 currContactWorldPose = gazebo::GetPos(_currContactWorldPose);
// the initial contact point location on the link should still correspond
// The initial contact point location on the link should still correspond
// to the initial contact point location on the object.
// initial vector from object center to contact point (relative to link,
// Initial vector from object center to contact point (relative to link,
// because relObjPos and relContactPos are from center of link)
gazebo::math::Vector3 oldObjDist = relContactPos - relObjPos;
// the same vector as \e oldObjDist, but calculated by the current world pose
GzVector3 oldObjDist = relContactPos - relObjPos;
// The same vector as \e oldObjDist, but calculated by the current world pose
// of object and the current location of the initial contact location on the link.
gazebo::math::Vector3 newObjDist = currContactWorldPose -
currObjWorldPose.pos; // new distance from contact to object
// This is the new distance from contact to object.
GzVector3 newObjDist = currContactWorldPose - gazebo::GetPos(currObjWorldPose);
//gzmsg<<"Obj Trans "<<cpInfo.collLink->GetName()<<": "<<relObjPos.x<<", "<<relObjPos.y<<", "<<relObjPos.z<<std::endl;
//gzmsg<<"Cont Trans "<<cpInfo.collLink->GetName()<<": "<<relContactPos.x<<", "<<relContactPos.y<<", "<<relContactPos.z<<std::endl;
// the difference between these vectors should not be too large...
float diff = fabs(oldObjDist.GetLength() - newObjDist.GetLength());
float diff =
fabs(gazebo::GetLength(oldObjDist) - gazebo::GetLength(newObjDist));
//gzmsg<<"Diff for link "<<cpInfo.collLink->GetName()<<": "<<diff<<std::endl;
if (diff > releaseTolerance)
......@@ -731,68 +796,6 @@ void GazeboGraspFix::OnUpdate()
this->prevUpdateTime = common::Time::GetWallTime();
}
////////////////////////////////////////////////////////////////////////////////
double angularDistance(const gazebo::math::Vector3 &_v1,
const gazebo::math::Vector3 &_v2)
{
gazebo::math::Vector3 v1 = _v1;
gazebo::math::Vector3 v2 = _v2;
v1.Normalize();
v2.Normalize();
return acos(v1.Dot(v2));
}
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::CheckGrip(const std::vector<gazebo::math::Vector3> &forces,
float minAngleDiff, float lengthRatio)
{
if (((lengthRatio > 1) || (lengthRatio < 0)) && (lengthRatio > 1e-04
&& (fabs(lengthRatio - 1) > 1e-04)))
{
std::cerr << "ERROR: CheckGrip: always specify a lengthRatio of [0..1]" <<
std::endl;
return false;
}
if (minAngleDiff < M_PI_2)
{
std::cerr << "ERROR: CheckGrip: min angle must be at least 90 degrees (PI/2)" <<
std::endl;
return false;
}
std::vector<gazebo::math::Vector3>::const_iterator it1, it2;
for (it1 = forces.begin(); it1 != forces.end(); ++it1)
{
gazebo::math::Vector3 v1 = *it1;
for (it2 = it1 + 1; it2 != forces.end(); ++it2)
{
gazebo::math::Vector3 v2 = *it2;
float l1 = v1.GetLength();
float l2 = v2.GetLength();
if ((l1 < 1e-04) || (l2 < 1e-04)) continue;
/*gazebo::math::Vector3 _v1=v1;
gazebo::math::Vector3 _v2=v2;
_v1/=l1;
_v2/=l2;
float angle=acos(_v1.Dot(_v2));*/
float angle = angularDistance(v1, v2);
// gzmsg<<"Angular distance between v1.len="<<v1.GetLength()<<" and v2.len="<<v2.GetLength()<<": "<<angle*180/M_PI<<std::endl;
if (angle > minAngleDiff)
{
float ratio;
if (l1 > l2) ratio = l2 / l1;
else ratio = l1 / l2;
// gzmsg<<"Got angle "<<angle<<", ratio "<<ratio<<std::endl;
if (ratio >= lengthRatio)
{
// gzmsg<<"CheckGrip() is true"<<std::endl;
return true;
}
}
}
}
return false;
}
////////////////////////////////////////////////////////////////////////////////
void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
{
......@@ -802,10 +805,10 @@ void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
{
physics::CollisionPtr collision1 =
boost::dynamic_pointer_cast<physics::Collision>(
this->world->GetEntity(_msg->contact(i).collision1()));
gazebo::GetEntityByName(this->world, _msg->contact(i).collision1()));
physics::CollisionPtr collision2 =
boost::dynamic_pointer_cast<physics::Collision>(
this->world->GetEntity(_msg->contact(i).collision2()));
gazebo::GetEntityByName(this->world, _msg->contact(i).collision2()));
if ((collision1 && !collision1->IsStatic()) && (collision2
&& !collision2->IsStatic()))
......@@ -841,7 +844,7 @@ void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
}
// all force vectors which are part of this contact
std::vector<gazebo::math::Vector3> force;
std::vector<GzVector3> force;
// find out which part of the colliding entities is the object, *not* the gripper,
// and copy all the forces applied to it into the vector 'force'.
......@@ -871,7 +874,7 @@ void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
force.push_back(contact.wrench[k].body2Force);
}
gazebo::math::Vector3 avgForce;
GzVector3 avgForce;
// compute average/sum of the forces applied on the object
for (int k = 0; k < force.size(); ++k)
{
......@@ -879,53 +882,51 @@ void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
}
avgForce /= force.size();
gazebo::math::Vector3 avgPos;
GzVector3 avgPos;
// compute center point (average pose) of all the origin positions of the forces appied
for (int k = 0; k < contact.count; ++k) avgPos += contact.positions[k];
avgPos /= contact.count;
// now, get average pose relative to the colliding link
gazebo::math::Pose linkWorldPose = linkCollision->GetLink()->GetWorldPose();
GzPose3 linkWorldPose = gazebo::GetWorldPose(linkCollision->GetLink());
// To find out the collision point relative to the Link's local coordinate system, first get the Poses as 4x4 matrices
gazebo::math::Matrix4 worldToLink = linkWorldPose.rot.GetAsMatrix4();
worldToLink.SetTranslate(linkWorldPose.pos);
GzMatrix4 worldToLink = gazebo::GetMatrix(linkWorldPose);
gazebo::math::Matrix4 worldToContact = gazebo::math::Matrix4::IDENTITY;
//we can assume that the contact has identity rotation because we don't care about its orientation.
//We could always set another rotation here too.
worldToContact.SetTranslate(avgPos);
// We can assume that the contact has identity rotation because we don't care about its orientation.
// We could always set another rotation here too.
GzMatrix4 worldToContact = gazebo::GetMatrix(avgPos);
// now, worldToLink * contactInLocal = worldToContact
// hence, contactInLocal = worldToLink.Inv * worldToContact
gazebo::math::Matrix4 worldToLinkInv = worldToLink.Inverse();
gazebo::math::Matrix4 contactInLocal = worldToLinkInv * worldToContact;
gazebo::math::Vector3 contactPosInLocal = contactInLocal.GetTranslation();
GzMatrix4 worldToLinkInv = worldToLink.Inverse();
GzMatrix4 contactInLocal = worldToLinkInv * worldToContact;
GzVector3 contactPosInLocal = gazebo::GetPos(contactInLocal);
//gzmsg<<"---------"<<std::endl;
//gzmsg<<"CNT in loc: "<<contactPosInLocal.x<<","<<contactPosInLocal.y<<","<<contactPosInLocal.z<<std::endl;
/*gazebo::math::Vector3 sDiff=avgPos-linkWorldPose.pos;
/*GzVector3 sDiff=avgPos-linkWorldPose.pos;
gzmsg<<"SIMPLE trans: "<<sDiff.x<<","<<sDiff.y<<","<<sDiff.z<<std::endl;
gzmsg<<"coll world pose: "<<linkWorldPose.pos.x<<", "<<linkWorldPose.pos.y<<", "<<linkWorldPose.pos.z<<std::endl;
gzmsg<<"contact avg pose: "<<avgPos.x<<", "<<avgPos.y<<", "<<avgPos.z<<std::endl;
gazebo::math::Vector3 lX=linkWorldPose.rot.GetXAxis();
gazebo::math::Vector3 lY=linkWorldPose.rot.GetYAxis();
gazebo::math::Vector3 lZ=linkWorldPose.rot.GetZAxis();
GzVector3 lX=linkWorldPose.rot.GetXAxis();
GzVector3 lY=linkWorldPose.rot.GetYAxis();
GzVector3 lZ=linkWorldPose.rot.GetZAxis();
gzmsg<<"World ori: "<<linkWorldPose.rot.x<<","<<linkWorldPose.rot.y<<","<<linkWorldPose.rot.z<<","<<linkWorldPose.rot.w<<std::endl;
gzmsg<<"x axis: "<<lX.x<<","<<lX.y<<","<<lX.z<<std::endl;
gzmsg<<"y axis: "<<lY.x<<","<<lY.y<<","<<lY.z<<std::endl;
gzmsg<<"z axis: "<<lZ.x<<","<<lZ.y<<","<<lZ.z<<std::endl;*/
// now, get the pose of the object and compute it's relative position to the collision surface.
gazebo::math::Pose objWorldPose = objCollision->GetLink()->GetWorldPose();
gazebo::math::Matrix4 worldToObj = objWorldPose.rot.GetAsMatrix4();
worldToObj.SetTranslate(objWorldPose.pos);
// Get the pose of the object and compute it's relative position to
// the collision surface.
GzPose3 objWorldPose = gazebo::GetWorldPose(objCollision->GetLink());
GzMatrix4 worldToObj = gazebo::GetMatrix(objWorldPose);
gazebo::math::Matrix4 objInLocal = worldToLinkInv * worldToObj;
gazebo::math::Vector3 objPosInLocal = objInLocal.GetTranslation();
GzMatrix4 objInLocal = worldToLinkInv * worldToObj;
GzVector3 objPosInLocal = gazebo::GetPos(objInLocal);
{
boost::mutex::scoped_lock lock(this->mutexContacts);
......
......@@ -7,6 +7,7 @@
#include <stdio.h>
#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
#include <gazebo_version_helpers/GazeboVersionHelpers.h>
using gazebo::GazeboGraspGripper;
......@@ -53,7 +54,8 @@ bool GazeboGraspGripper::Init(physics::ModelPtr &_model,
this->attached = false;
this->disableCollisionsOnAttach = _disableCollisionsOnAttach;
this->model = _model;
physics::PhysicsEnginePtr physics = this->model->GetWorld()->GetPhysicsEngine();
physics::PhysicsEnginePtr physics =
gazebo::GetPhysics(this->model->GetWorld());
this->fixedJoint = physics->CreateJoint("revolute");
this->palmLink = this->model->GetLink(palmLinkName);
......@@ -166,20 +168,26 @@ bool GazeboGraspGripper::HandleAttach(const std::string &objName)
this->palmLink->GetWorldPose();
this->palmLink->AttachStaticModel(obj, diff);
#else
physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>
(world->GetEntity(objName));
physics::CollisionPtr obj =
boost::dynamic_pointer_cast<physics::Collision>(gazebo::GetEntityByName(world, objName));
if (!obj.get())
{
std::cerr << "ERROR: Object " << objName <<
" not found in world, can't attach it" << std::endl;
return false;
}
gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() -
this->palmLink->GetWorldPose();
gazebo::GzPose3 diff = gazebo::GetWorldPose(obj->GetLink()) -
gazebo::GetWorldPose(this->palmLink);
this->fixedJoint->Load(this->palmLink, obj->GetLink(), diff);
this->fixedJoint->Init();
#if GAZEBO_MAJOR_VERSION >= 8
this->fixedJoint->SetUpperLimit(0, 0);
this->fixedJoint->SetLowerLimit(0, 0);
#else
this->fixedJoint->SetHighStop(0, 0);
this->fixedJoint->SetLowStop(0, 0);
#endif
if (this->disableCollisionsOnAttach)
{
// we can disable collisions of the grasped object, because when the fingers keep colliding with
......@@ -212,7 +220,7 @@ void GazeboGraspGripper::HandleDetach(const std::string &objName)
this->palmLink->DetachStaticModel(objName);
#else
physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>
(world->GetEntity(objName));
(gazebo::GetEntityByName(world, objName));
if (!obj.get())
{
std::cerr << "ERROR: Object " << objName <<
......
......@@ -8,6 +8,7 @@ find_package(
catkin
REQUIRED COMPONENTS
gazebo_world_plugin_loader
gazebo_version_helpers
geometry_msgs
roscpp
shape_msgs
......
......@@ -29,6 +29,7 @@
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>gazebo_world_plugin_loader</build_depend>
<build_depend>gazebo_version_helpers</build_depend>
<build_depend>shape_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
......@@ -36,6 +37,7 @@
<build_depend>object_msgs_tools</build_depend>
<build_depend>eigen_conversions</build_depend>
<run_depend>gazebo_world_plugin_loader</run_depend>
<run_depend>gazebo_version_helpers</run_depend>
<run_depend>object_msgs</run_depend>
<run_depend>object_msgs_tools</run_depend>
<run_depend>eigen_conversions</run_depend>
......
#include <gazebo_state_plugins/GazeboMapPublisher.h>
#include <gazebo_version_helpers/GazeboVersionHelpers.h>
#include <gazebo/physics/physics.hh>
......@@ -33,6 +34,7 @@
#define MAX_MAP_VAL 100
using gazebo::GazeboMapPublisher;
using gazebo::GzVector3;
GZ_REGISTER_WORLD_PLUGIN(GazeboMapPublisher)
......@@ -187,11 +189,10 @@ bool GazeboMapPublisher::createMap(const CollisionMapRequest &msg, const std::st
double dist;
std::string entityName;
math::Vector3 start, end;
start.z = msg.height;
end.z = 0.001;
GzVector3 start = gazebo::GetVector(0, 0, msg.height);
GzVector3 end = gazebo::GetVector(0, 0, 0.001);
gazebo::physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
gazebo::physics::PhysicsEnginePtr engine = gazebo::GetPhysics(world);
engine->InitForThread();
//gazebo::physics::RayShapePtr ray = boost::shared_dynamic_cast<gazebo::physics::RayShape>(
gazebo::physics::RayShapePtr ray = boost::dynamic_pointer_cast<gazebo::physics::RayShape>(
......@@ -208,8 +209,10 @@ bool GazeboMapPublisher::createMap(const CollisionMapRequest &msg, const std::st
x += dX_horizontal;
y += dY_horizontal;
start.x = end.x= x;
start.y = end.y = y;
gazebo::SetX(start, x);
gazebo::SetX(end, x);
gazebo::SetY(start, y);
gazebo::SetY(end, y);
ray->SetPoints(start, end);
ray->GetIntersection(dist, entityName);
if (!entityName.empty()) {
......
#include <gazebo_state_plugins/GazeboObjectInfo.h>
#include <gazebo_version_helpers/GazeboVersionHelpers.h>
#include <object_msgs_tools/ObjectFunctions.h>
#include <gazebo/physics/Link.hh>
#include <gazebo/physics/BoxShape.hh>
......@@ -18,6 +19,7 @@
#define OBJECT_QUEUE_SIZE 100
using gazebo::GazeboObjectInfo;
using gazebo::GzVector3;
GZ_REGISTER_WORLD_PLUGIN(GazeboObjectInfo)
......@@ -56,8 +58,8 @@ void GazeboObjectInfo::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf){
bool GazeboObjectInfo::requestObject(object_msgs::ObjectInfo::Request &req, object_msgs::ObjectInfo::Response &res) {