365 lines
10 KiB
C++
365 lines
10 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2012 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: Gazebo Driver for Player
|
||
|
|
* Author: Nate Koenig
|
||
|
|
* Date: 2 March 2006
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <iostream>
|
||
|
|
|
||
|
|
#include "GazeboInterface.hh"
|
||
|
|
#include "SimulationInterface.hh"
|
||
|
|
#include "Position2dInterface.hh"
|
||
|
|
#include "LaserInterface.hh"
|
||
|
|
#include "CameraInterface.hh"
|
||
|
|
// #include "Graphics3dInterface.hh"
|
||
|
|
// #include "FiducialInterface.hh"
|
||
|
|
// #include "Position3dInterface.hh"
|
||
|
|
// #include "ActarrayInterface.hh"
|
||
|
|
// #include "OpaqueInterface.hh"
|
||
|
|
// #include "PTZInterface.hh"
|
||
|
|
// #include "BumperInterface.hh"
|
||
|
|
// #include "GripperInterface.hh"
|
||
|
|
// #include "IRInterface.hh"
|
||
|
|
// #include "ImuInterface.hh"
|
||
|
|
|
||
|
|
/*
|
||
|
|
#include "PowerInterface.hh"
|
||
|
|
#include "SonarInterface.hh"
|
||
|
|
#include "PtzInterface.hh"
|
||
|
|
#include "GripperInterface.hh"
|
||
|
|
#include "TruthInterface.hh"
|
||
|
|
#include "GpsInterface.hh"
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "GazeboDriver.hh"
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
/// A factory creation function, declared outside of the class so that it
|
||
|
|
/// can be invoked without any object context (alternatively, you can
|
||
|
|
/// declare it static in the class). In this function, we create and return
|
||
|
|
/// (as a generic Driver*) a pointer to a new instance of this driver.
|
||
|
|
Driver *GazeboDriver_Init(ConfigFile *_cf, int _section)
|
||
|
|
{
|
||
|
|
// Create and return a new instance of this driver
|
||
|
|
return (static_cast<Driver*>(new GazeboDriver(_cf, _section)));
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
/// A driver registration function, again declared outside of the class so
|
||
|
|
/// that it can be invoked without object context. In this function, we add
|
||
|
|
/// the driver into the given driver table, indicating which interface the
|
||
|
|
/// driver can support and how to create a driver instance.
|
||
|
|
void GazeboDriver_Register(DriverTable *_table)
|
||
|
|
{
|
||
|
|
// TODO: Fix the PACKAGE_VERSION
|
||
|
|
// printf("\n ** Gazebo plugin v%s **", PACKAGE_VERSION);
|
||
|
|
|
||
|
|
_table->AddDriver(const_cast<char*>("gazebo"), GazeboDriver_Init);
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
// need the extern to avoid C++ name-mangling
|
||
|
|
extern "C"
|
||
|
|
{
|
||
|
|
GAZEBO_VISIBLE
|
||
|
|
int player_driver_init(DriverTable *_table)
|
||
|
|
{
|
||
|
|
GazeboDriver_Register(_table);
|
||
|
|
return(0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
// Retrieve options from the configuration file and do any
|
||
|
|
// pre-Setup() setup.
|
||
|
|
GazeboDriver::GazeboDriver(ConfigFile *_cf, int _section)
|
||
|
|
: Driver(_cf, _section, false, 4096)
|
||
|
|
{
|
||
|
|
printf("GazeboDriver::GazeboDriver\n");
|
||
|
|
this->deviceMaxCount = 0;
|
||
|
|
|
||
|
|
if (this->LoadDevices(_cf, _section) < 0)
|
||
|
|
{
|
||
|
|
std::cout << "Error: Loading devices\n";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
GazeboDriver::~GazeboDriver()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
// Set up the device. Return 0 if things go well, and -1 otherwise.
|
||
|
|
int GazeboDriver::Setup()
|
||
|
|
{
|
||
|
|
return(0);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// Shutdown the device
|
||
|
|
int GazeboDriver::Shutdown()
|
||
|
|
{
|
||
|
|
return(0);
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
// Process all messages for this driver.
|
||
|
|
int GazeboDriver::ProcessMessage(QueuePointer &_respQueue,
|
||
|
|
player_msghdr *_hdr,
|
||
|
|
void *_data)
|
||
|
|
{
|
||
|
|
// find the right interface to handle this config
|
||
|
|
GazeboInterface *in = this->LookupDevice(_hdr->addr);
|
||
|
|
|
||
|
|
if (in)
|
||
|
|
{
|
||
|
|
return(in->ProcessMessage(_respQueue, _hdr, _data));
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
printf("can't find interface for device %d.%d.%d",
|
||
|
|
this->device_addr.robot,
|
||
|
|
this->device_addr.interf,
|
||
|
|
this->device_addr.index);
|
||
|
|
return(-1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
// Subscribe an device to this driver
|
||
|
|
int GazeboDriver::Subscribe(player_devaddr_t addr)
|
||
|
|
{
|
||
|
|
GazeboInterface *device = this->LookupDevice(addr);
|
||
|
|
|
||
|
|
if (device)
|
||
|
|
{
|
||
|
|
device->Subscribe();
|
||
|
|
return Driver::Subscribe(addr);
|
||
|
|
}
|
||
|
|
|
||
|
|
puts("failed to find a device.");
|
||
|
|
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
// Remove a device from this driver
|
||
|
|
int GazeboDriver::Unsubscribe(player_devaddr_t addr)
|
||
|
|
{
|
||
|
|
GazeboInterface *device = this->LookupDevice(addr);
|
||
|
|
|
||
|
|
if (device)
|
||
|
|
{
|
||
|
|
device->Unsubscribe();
|
||
|
|
return Driver::Unsubscribe(addr);
|
||
|
|
}
|
||
|
|
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
// Main function for device thread
|
||
|
|
void GazeboDriver::Update()
|
||
|
|
{
|
||
|
|
Driver::ProcessMessages();
|
||
|
|
|
||
|
|
for (auto device: this->devices)
|
||
|
|
{
|
||
|
|
device->Update();
|
||
|
|
}
|
||
|
|
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
// Helper function to load all devices on startup
|
||
|
|
int GazeboDriver::LoadDevices(ConfigFile *_cf, int _section)
|
||
|
|
{
|
||
|
|
// Get the device count
|
||
|
|
this->deviceMaxCount = _cf->GetTupleCount(_section, "provides");
|
||
|
|
|
||
|
|
if (!player_quiet_startup)
|
||
|
|
{
|
||
|
|
printf(" Gazebo Plugin driver creating %d %s\n",
|
||
|
|
this->deviceMaxCount,
|
||
|
|
this->deviceMaxCount == 1 ? "device" : "devices");
|
||
|
|
}
|
||
|
|
|
||
|
|
// Load all the devices
|
||
|
|
for (int d = 0; d < this->deviceMaxCount; d++)
|
||
|
|
{
|
||
|
|
player_devaddr_t playerAddr;
|
||
|
|
|
||
|
|
// Read in the Device address
|
||
|
|
if (_cf->ReadDeviceAddr(&playerAddr, _section,
|
||
|
|
"provides", 0, d, NULL) != 0)
|
||
|
|
{
|
||
|
|
this->SetError(-1);
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!player_quiet_startup)
|
||
|
|
{
|
||
|
|
printf(" %d.%d.%d is ",
|
||
|
|
playerAddr.robot, playerAddr.interf, playerAddr.index);
|
||
|
|
fflush(stdout);
|
||
|
|
}
|
||
|
|
|
||
|
|
GazeboInterface *ifsrc = NULL;
|
||
|
|
|
||
|
|
switch (playerAddr.interf)
|
||
|
|
{
|
||
|
|
case PLAYER_SIMULATION_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a simulation interface.\n");
|
||
|
|
ifsrc = new SimulationInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_POSITION2D_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a position2d interface.\n");
|
||
|
|
ifsrc = new Position2dInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_LASER_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a laser interface.\n");
|
||
|
|
ifsrc = new LaserInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_CAMERA_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a camera interface.\n");
|
||
|
|
ifsrc = new CameraInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
/*case PLAYER_GRAPHICS3D_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a graphics3d interface.\n");
|
||
|
|
ifsrc = new Graphics3dInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
|
||
|
|
case PLAYER_FIDUCIAL_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a fiducial interface.\n");
|
||
|
|
ifsrc = new FiducialInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_POSITION3D_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a position3d interface.\n");
|
||
|
|
ifsrc = new Position3dInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_ACTARRAY_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" an actarray interface.\n");
|
||
|
|
ifsrc = new ActarrayInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_OPAQUE_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" an opaque interface.\n");
|
||
|
|
ifsrc = new OpaqueInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_PTZ_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a ptz interface.\n");
|
||
|
|
ifsrc = new PTZInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_GRIPPER_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a gripper interface.\n");
|
||
|
|
ifsrc = new GripperInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_BUMPER_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a bumper interface.\n");
|
||
|
|
ifsrc = new BumperInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_IR_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" an ir interface.\n");
|
||
|
|
ifsrc = new IRInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_IMU_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" an imu interface.\n");
|
||
|
|
ifsrc = new ImuInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
*/
|
||
|
|
/* case PLAYER_POWER_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a power interface.\n");
|
||
|
|
ifsrc = new PowerInterface(playerAddr, this, _cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_SONAR_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a sonar interface.\n");
|
||
|
|
ifsrc = new SonarInterface(playerAddr, this, cf, _section);
|
||
|
|
break;
|
||
|
|
|
||
|
|
case PLAYER_GPS_CODE:
|
||
|
|
if (!player_quiet_startup) printf(" a gps interface.\n");
|
||
|
|
ifsrc = new GpsInterface(playerAddr, this, cf, _section);
|
||
|
|
break;
|
||
|
|
*/
|
||
|
|
default:
|
||
|
|
printf("error: Gazebo driver doesn't support interface type %d\n",
|
||
|
|
playerAddr.interf);
|
||
|
|
this->SetError(-1);
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (ifsrc)
|
||
|
|
{
|
||
|
|
// attempt to add this interface and we're done
|
||
|
|
if (this->AddInterface(ifsrc->device_addr))
|
||
|
|
{
|
||
|
|
printf("Gazebo driver error: AddInterface() failed\n");
|
||
|
|
this->SetError(-2);
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
// store the Interaface in our device list
|
||
|
|
this->devices.push_back(ifsrc);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
printf("No Gazebo source found for interface %d:%d:%d",
|
||
|
|
playerAddr.robot,
|
||
|
|
playerAddr.interf,
|
||
|
|
playerAddr.index);
|
||
|
|
|
||
|
|
this->SetError(-3);
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////
|
||
|
|
// Find a device according to a player_devaddr
|
||
|
|
GazeboInterface *GazeboDriver::LookupDevice(player_devaddr_t _addr)
|
||
|
|
{
|
||
|
|
for (auto iface: this->devices)
|
||
|
|
{
|
||
|
|
if (iface->device_addr.robot == _addr.robot &&
|
||
|
|
iface->device_addr.interf == _addr.interf &&
|
||
|
|
iface->device_addr.index == _addr.index)
|
||
|
|
return iface;
|
||
|
|
}
|
||
|
|
|
||
|
|
return NULL;
|
||
|
|
}
|