Skip to content

Commit

Permalink
Depth, Dvl, Ins and Gps plugins compile on gazebo9 and ros melodic
Browse files Browse the repository at this point in the history
  • Loading branch information
@stefanosuman
stefanosuman committed Apr 17, 2020
1 parent 32ecb86 commit 552dfe6
Show file tree
Hide file tree
Showing 5 changed files with 45 additions and 43 deletions.
30 changes: 15 additions & 15 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,29 +84,29 @@ set(SENSORS_SRCS

# dsros_sensors (passed at STARTUP as a system plugin)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
#add_library(dsros_sensors ${SENSORS_SRCS})
#target_link_libraries(dsros_sensors ds_sim_gazebo_msgs ${GAZEBO_LIBRARIES})
#add_dependencies(dsros_sensors ds_sim_gazebo_msgs)
add_library(dsros_sensors ${SENSORS_SRCS})
target_link_libraries(dsros_sensors ds_sim_gazebo_msgs ${GAZEBO_LIBRARIES})
add_dependencies(dsros_sensors ds_sim_gazebo_msgs)

# dsros_ros_depth
#add_library(dsros_ros_depth src/dsros_depth_plugin.cc src/dsros_depth_plugin.hh)
#target_link_libraries(dsros_ros_depth dsros_sensors ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
#add_dependencies(dsros_ros_depth ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_library(dsros_ros_depth src/dsros_depth_plugin.cc src/dsros_depth_plugin.hh)
target_link_libraries(dsros_ros_depth dsros_sensors ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
add_dependencies(dsros_ros_depth ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

# dsros_ros_ins
#add_library(dsros_ros_ins src/dsros_ins_plugin.cc src/dsros_ins_plugin.hh)
#target_link_libraries(dsros_ros_ins dsros_sensors ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
#add_dependencies(dsros_ros_ins ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_library(dsros_ros_ins src/dsros_ins_plugin.cc src/dsros_ins_plugin.hh)
target_link_libraries(dsros_ros_ins dsros_sensors ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
add_dependencies(dsros_ros_ins ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

# DVL plugin
#add_library(dsros_ros_dvl src/dsros_dvl_plugin.cc src/dsros_dvl_plugin.hh)
#target_link_libraries(dsros_ros_dvl dsros_sensors ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
#add_dependencies(dsros_ros_dvl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_library(dsros_ros_dvl src/dsros_dvl_plugin.cc src/dsros_dvl_plugin.hh)
target_link_libraries(dsros_ros_dvl dsros_sensors ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
add_dependencies(dsros_ros_dvl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

# GPS plugin
#add_library(dsros_ros_gps src/dsros_gps_plugin.cc src/dsros_gps_plugin.hh)
#target_link_libraries(dsros_ros_gps ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
#add_dependencies(dsros_ros_gps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_library(dsros_ros_gps src/dsros_gps_plugin.cc src/dsros_gps_plugin.hh)
target_link_libraries(dsros_ros_gps ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
add_dependencies(dsros_ros_gps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

# Reson plugin
add_library(dsros_ros_reson src/dsros_reson_plugin.cc src/dsros_reson_plugin.hh)
Expand Down
8 changes: 4 additions & 4 deletions gazebo_src/dsros_depth.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ void DsrosDepthSensor::Load(const std::string &_worldName) {
Sensor::Load(_worldName);

// Load the parent link
physics::EntityPtr parentEntity = this->world->GetEntity(
physics::EntityPtr parentEntity = this->world->EntityByName(
this->ParentName());
this->parentLink = boost::dynamic_pointer_cast<physics::Link>(parentEntity);
if (! this->parentLink) {
Expand All @@ -84,7 +84,7 @@ void DsrosDepthSensor::Load(const std::string &_worldName) {

void DsrosDepthSensor::Init() {
Sensor::Init();
this->sphericalCoordinates = this->world->GetSphericalCoordinates();
this->sphericalCoordinates = this->world->SphericalCoords();
}

void DsrosDepthSensor::Fini() {
Expand Down Expand Up @@ -135,7 +135,7 @@ bool DsrosDepthSensor::UpdateImpl(const bool _force) {

// Get the actual depth
double raw_depth;
ignition::math::Pose3d depthPose = this->pose + this->parentLink->GetWorldPose().Ign();
ignition::math::Pose3d depthPose = this->pose + this->parentLink->WorldPose();

raw_depth = -depthPose.Pos().Z();

Expand All @@ -160,7 +160,7 @@ bool DsrosDepthSensor::UpdateImpl(const bool _force) {
// <<" ERR: " <<depth - raw_depth <<" LAT: " <<lat <<" PRESS: " << press <<"\n";

// fill in the message
msgs::Set(this->msg.mutable_stamp(), this->world->GetSimTime());
msgs::Set(this->msg.mutable_stamp(), this->world->SimTime());
this->msg.set_depth(depth);
this->msg.set_pressure_dbar(press);
this->msg.set_latitude_deg(lat);
Expand Down
23 changes: 11 additions & 12 deletions gazebo_src/dsros_dvl.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,7 +136,7 @@ void DsrosDvlBeam::Update(const physics::WorldPtr& world,
// (possibly) update the contact cache
if (entityName != contactEntityName) {
contactEntityName = entityName;
contactEntityPtr = world->GetEntity(entityName);
contactEntityPtr = world->EntityByName(entityName);
}

// get velocities
Expand All @@ -145,9 +145,8 @@ void DsrosDvlBeam::Update(const physics::WorldPtr& world,
ignition::math::Vector3d intersection = inst2world.Rot().RotateVector(contactRange * beamUnitVector) + inst2world.Pos();

// Compute the velocity of the point of impact in world coordinates
ignition::math::Vector3d entityVel = contactEntityPtr->GetWorldLinearVel().Ign()
+ contactEntityPtr->GetWorldAngularVel().Ign().Cross(
contactEntityPtr->GetWorldPose().pos.Ign() - intersection);
ignition::math::Vector3d entityVel = contactEntityPtr->WorldLinearVel()
+ contactEntityPtr->WorldAngularVel().Cross(contactEntityPtr->WorldPose().Pos() - intersection);
//gzdbg <<"ENTITY: " <<entityVel.X() <<", " <<entityVel.Y() <<", " <<entityVel.Z() <<"\n";

//gzdbg <<"intersection: " <<intersection.X() <<"," <<intersection.Y() <<"," <<intersection.Z() <<"\n";
Expand Down Expand Up @@ -176,7 +175,7 @@ void DsrosDvlSensor::Load(const std::string &_worldName) {
Sensor::Load(_worldName);

// Load the parent link
physics::EntityPtr parentEntity = this->world->GetEntity(
physics::EntityPtr parentEntity = this->world->EntityByName(
this->ParentName());
this->parentLink = boost::dynamic_pointer_cast<physics::Link>(parentEntity);
if (! this->parentLink) {
Expand Down Expand Up @@ -228,7 +227,7 @@ void DsrosDvlSensor::Load(const std::string &_worldName) {
this->dvlPub = this->node->Advertise<ds_sim::msgs::Dvl>(this->topicName, 50);

// Setup physics!
physics::PhysicsEnginePtr physicsEngine = this->world->GetPhysicsEngine();
physics::PhysicsEnginePtr physicsEngine = this->world->Physics();
GZ_ASSERT(physicsEngine != NULL, "Unable to get pointer to physics engine");

// rotate
Expand Down Expand Up @@ -352,16 +351,16 @@ bool DsrosDvlSensor::UpdateImpl(const bool _force) {

// Acquire a mutex to avoid a race condition
boost::recursive_mutex::scoped_lock engine_lock(*(
this->world->GetPhysicsEngine()->GetPhysicsUpdateMutex()));
this->world->Physics()->GetPhysicsUpdateMutex()));

std::lock_guard<std::mutex> lock(this->mutex);

// update each beam
ignition::math::Pose3d vehPose = this->parentLink->GetWorldPose().Ign();
ignition::math::Vector3d bodyLinearVel = this->parentLink->GetWorldLinearVel().Ign();
ignition::math::Vector3d bodyAngularVel = this->parentLink->GetWorldAngularVel().Ign();
ignition::math::Pose3d vehPose = this->parentLink->WorldPose();
ignition::math::Vector3d bodyLinearVel = this->parentLink->WorldLinearVel();
ignition::math::Vector3d bodyAngularVel = this->parentLink->WorldAngularVel();
ignition::math::Vector3d sensorVel = bodyLinearVel + vehPose.Rot().RotateVector(bodyAngularVel.Cross(this->pose.Pos()));
ignition::math::Pose3d sensorPose = this->pose + this->parentLink->GetWorldPose().Ign();
ignition::math::Pose3d sensorPose = this->pose + this->parentLink->WorldPose();

// Compute a solution for all beams
int valid_beams = 0;
Expand Down Expand Up @@ -407,7 +406,7 @@ bool DsrosDvlSensor::UpdateImpl(const bool _force) {
}

// fill in the message
msgs::Set(this->msg.mutable_stamp(), this->world->GetSimTime());
msgs::Set(this->msg.mutable_stamp(), this->world->SimTime());
msgs::Set(this->msg.mutable_linear_velocity(), linear_velocity);
this->msg.set_num_beams(valid_beams);
for (size_t i=0; i<msg.ranges_size(); i++) {
Expand Down
21 changes: 12 additions & 9 deletions gazebo_src/dsros_ins.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ void DsrosInsSensor::Load(const std::string &_worldName) {
Sensor::Load(_worldName);

// Load the parent link
physics::EntityPtr parentEntity = this->world->GetEntity(
physics::EntityPtr parentEntity = this->world->EntityByName(
this->ParentName());
this->parentLink = boost::dynamic_pointer_cast<physics::Link>(parentEntity);
if (! this->parentLink) {
Expand All @@ -80,7 +80,7 @@ void DsrosInsSensor::Load(const std::string &_worldName) {

void DsrosInsSensor::Init() {
Sensor::Init();
this->sphericalCoordinates = this->world->GetSphericalCoordinates();
this->sphericalCoordinates = this->world->SphericalCoords();
}

void DsrosInsSensor::Fini() {
Expand Down Expand Up @@ -111,24 +111,27 @@ bool DsrosInsSensor::UpdateImpl(const bool _force) {
// Get the actual sensor values


ignition::math::Pose3d parentLinkPose = this->parentLink->GetWorldPose().Ign();
ignition::math::Pose3d parentLinkPose = this->parentLink->WorldPose();
ignition::math::Pose3d insPose = this->pose + parentLinkPose;

// angular velocity
// TODO This doesn't do what we want it to
ignition::math::Vector3<double> angular_velocity
= insPose.Rot().Inverse().RotateVector(this->parentLink->GetWorldAngularVel().Ign());
= insPose.Rot().Inverse().RotateVector(this->parentLink->WorldAngularVel());

// linear velocity
ignition::math::Vector3d linear_velocity = this->parentLink->
GetWorldLinearVel(this->pose.Pos(), this->pose.Rot()).Ign();
WorldLinearVel(this->pose.Pos(), this->pose.Rot());

// linear acceleration
ignition::math::Vector3d gravity = this->world->GetPhysicsEngine()->GetGravity().Ign();
//ignition::math::Vector3d gravity = this->world->GetPhysicsEngine()->GetGravity().Ign();
ignition::math::Vector3d gravity = this->world->Gravity();
//ignition::math::Vector3d linear_accel = this->parentLink->GetWorldLinearAccel().Ign();
//ignition::math::Vector3d angular_accel = this->parentLink->GetWorldAngularAccel().Ign();
ignition::math::Vector3d linear_accel = this->parentLink->GetRelativeLinearAccel().Ign();
ignition::math::Vector3d angular_accel = this->parentLink->GetRelativeAngularAccel().Ign();
//ignition::math::Vector3d linear_accel = this->parentLink->GetRelativeLinearAccel().Ign();
ignition::math::Vector3d linear_accel = this->parentLink->RelativeLinearAccel();
//ignition::math::Vector3d angular_accel = this->parentLink->GetRelativeAngularAccel().Ign();
ignition::math::Vector3d angular_accel = this->parentLink->RelativeAngularAccel();
// lever-arm offset
linear_accel += angular_accel.Cross(this->pose.Pos());
// TODO: There should probably be a centripital acceleration term or something?
Expand All @@ -145,7 +148,7 @@ bool DsrosInsSensor::UpdateImpl(const bool _force) {
lat = spherical.X();

// fill in the message
msgs::Set(this->msg.mutable_stamp(), this->world->GetSimTime());
msgs::Set(this->msg.mutable_stamp(), this->world->SimTime());
this->msg.set_entity_name(this->Name());
this->msg.set_roll_deg(insPose.Rot().Roll());
this->msg.set_pitch_deg(insPose.Rot().Pitch());
Expand Down
6 changes: 3 additions & 3 deletions src/dsros_gps_plugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,13 +61,13 @@ void dsrosRosGpsSensor::Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) {
// Gazebo's Spherical Coordinates conversion is b0rk3n. We have to do our own.
// Start by grabbing the origin
auto world = gazebo::physics::get_world(sensor->WorldName());
auto spherical = world->GetSphericalCoordinates();
auto spherical = world->SphericalCoords();
lat_origin_rad = spherical->LatitudeReference().Radian();
lat_origin = spherical->LatitudeReference().Degree();
lon_origin = spherical->LongitudeReference().Degree();
gzmsg <<"ORIGIN: " <<lat_origin <<", " <<lon_origin <<std::endl;

physics::EntityPtr parentEntity = world->GetEntity(sensor->ParentName());
physics::EntityPtr parentEntity = world->EntityByName(sensor->ParentName());
parent_link = boost::dynamic_pointer_cast<gazebo::physics::Link>(parentEntity);

if (!LoadParameters()) {
Expand Down Expand Up @@ -103,7 +103,7 @@ void dsrosRosGpsSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) {
data_time = sensor->LastMeasurementTime();
entity_name = frame_name;
// Gazebo is ENU. Unless you use the Spherical Coordinates module, then it's screwy
ignition::math::Pose3d gpsPose = sensor->Pose() + parent_link->GetWorldPose().Ign();
ignition::math::Pose3d gpsPose = sensor->Pose() + parent_link->WorldPose();
double easting = gpsPose.Pos().X();
double northing = gpsPose.Pos().Y();
altitude = sensor->Altitude();
Expand Down

0 comments on commit 552dfe6

Please sign in to comment.