Hi all,
I wrote an [urdf](https://github.com/schizzz8/lucrezio_simulation_environments/blob/master/urdf/lucrezio_with_logical.urdf.xacro) for a diff-drive robot with a [logical camera](http://gazebosim.org/tutorials?tut=logical_camera_sensor&cat=sensors) sensor.
To publish its messages over ROS I wrote a simple plugin:
#include "logical_camera_plugin.h"
using namespace gazebo;
using namespace std;
using namespace ros;
GZ_REGISTER_SENSOR_PLUGIN(LogicalCameraPlugin);
void LogicalCameraPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf){
// Get the parent sensor.
this->parentSensor = std::dynamic_pointer_cast(_sensor);
// Make sure the parent sensor is valid.
if (!this->parentSensor){
gzerr << "LogicalCameraPlugin requires a LogicalCameraSensor.\n";
return;
}
// Connect to the sensor update event.
this->updateConnection = this->parentSensor->ConnectUpdated(std::bind(&LogicalCameraPlugin::OnUpdate, this));
// Make sure the parent sensor is active.
this->parentSensor->SetActive(true);
...
nh = new ros::NodeHandle("~");
image_pub = nh->advertise("logical_camera_image", 1, true);
}
void LogicalCameraPlugin::OnUpdate(){
msgs::LogicalCameraImage logical_image;
lucrezio_simulation_environments::LogicalImage msg;
logical_image = this->parentSensor->Image();
gazebo::rendering::ScenePtr scene = gazebo::rendering::get_scene();
if (!scene || !scene->Initialized())
return;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "logical_camera_link";
...
this->image_pub.publish(msg);
}
My only concern was:
Is it possible to get from the msgs::LogicalCameraImage a timestamp?
Because, right now, I'm giving ros::Time::now() to the ROS message and I guess this is not the right solution.
Thanks.
↧