Commit 4e3d7cfc authored by Rafael Mosberger's avatar Rafael Mosberger
Browse files

bugfixes

parent ed47d0ad
......@@ -30,6 +30,9 @@
using namespace reflex;
ros::Time _tsStartROS;
double _tsStartCameraUnit;
ReflexCameraUnitInfo _cameraUnitInfo;
ReflexSystemStatus _systemStatus;
......@@ -347,7 +350,9 @@ void convertToROS(const ReflexCameraUnitInfo& msgSrc, reflex_ros::ReflexCameraUn
void convertToROS(const ReflexSystemStatus& status, reflex_ros::ReflexSystemStatus& msgStatus)
{
msgStatus.header.stamp = ros::Time(status.timestamp);
ros::Time tsROS = _tsStartROS + ros::Duration(status.timestamp - _tsStartCameraUnit);
msgStatus.header.stamp = tsROS;
msgStatus.temperature = status.temperature;
msgStatus.error = status.error;
msgStatus.state = status.state;
......@@ -358,7 +363,9 @@ void convertToROS(const ReflexSystemStatus& status, reflex_ros::ReflexSystemStat
void convertToROS(const ReflexSensorStatus& status, reflex_ros::ReflexSensorStatus& msgStatus)
{
msgStatus.header.stamp = ros::Time(status.timestamp);
ros::Time tsROS = _tsStartROS + ros::Duration(status.timestamp - _tsStartCameraUnit);
msgStatus.header.stamp = tsROS;
msgStatus.sensor_id = status.sensor_id;
msgStatus.error = status.error;
msgStatus.frame_id = status.frame_id;
......@@ -372,6 +379,9 @@ void convertToROS(const ReflexSensorStatus& status, reflex_ros::ReflexSensorStat
void convertToROS(const std::vector<float>& pose, geometry_msgs::PoseStamped& msgCameraPose)
{
if(pose.size() != 6)
return;
msgCameraPose.header.stamp = ros::Time::now();
msgCameraPose.pose.position.x = pose[0];
......@@ -410,6 +420,8 @@ int main(int argc, char **argv)
ros::init(argc, argv, "reflex_driver");
ros::NodeHandle nh("~");
int errConnection = 0;
// Read parameters
std::string ipAddress;
......@@ -451,9 +463,8 @@ int main(int argc, char **argv)
// Get camera unit time stamp
ros::Time tsStartROS = ros::Time::now();
double tsStartCameraUnit;
err = reflex_get_time_stamp(_cameraUnitInfo.ip_address, tsStartCameraUnit);
_tsStartROS = ros::Time::now();
err = reflex_get_time_stamp(_cameraUnitInfo.ip_address, _tsStartCameraUnit);
if(err != ERROR_NONE)
{
ROS_ERROR("Camera Unit %s (IP %s): Failed to get time stamp (Error %i)", _cameraUnitInfo.serial_number.c_str(), _cameraUnitInfo.ip_address.c_str(), err);
......@@ -479,13 +490,13 @@ int main(int argc, char **argv)
msgCameraInfo[sensorID] = sensor_msgs::CameraInfoPtr(new sensor_msgs::CameraInfo());
msgCameraInfo[sensorID]->header.frame_id = "reflex_cam" + std::to_string(sensorID+1);
pubCamSig[sensorID] = itSig[sensorID].advertiseCamera("cam" + std::to_string(sensorID+1) + "/sig/image", 1);
pubCamRef[sensorID] = itRef[sensorID].advertiseCamera("cam" + std::to_string(sensorID+1) + "/ref/image", 1);
pubCamSig[sensorID] = itSig[sensorID].advertiseCamera("cam" + std::to_string(sensorID+1) + "/image_sig", 1);
pubCamRef[sensorID] = itRef[sensorID].advertiseCamera("cam" + std::to_string(sensorID+1) + "/image_ref", 1);
}
image_transport::CameraPublisher pubCamSigDrawing;
if(drawDetections)
pubCamSigDrawing = itSigDrawing.advertiseCamera("cam1/sig/image/drawings", 1);
pubCamSigDrawing = itSigDrawing.advertiseCamera("cam1/image_sig_detections", 1);
// Advertise camera unit info
......@@ -520,7 +531,7 @@ int main(int argc, char **argv)
reflex_ros::ReflexData msgData;
msgData.header.frame_id = "reflex_cam1";
ros::Publisher pubData = nh.advertise<reflex_ros::ReflexData>("cam1/detections", 1);
ros::Publisher pubData = nh.advertise<reflex_ros::ReflexData>("data", 1);
// Advertise Services
......@@ -541,19 +552,11 @@ int main(int argc, char **argv)
ros::Time tsUpdateSensorStatus = ros::Time::now() + ros::Duration(0.5);
ros::Time tsUpdateCameraPose = ros::Time::now() + ros::Duration(0.75);
while (ros::ok())
while (ros::ok() && errConnection < 10)
{
ros::Time tLoop = ros::Time::now();
if(_systemStatus.state == SYSTEM_STATE_UNDEFINED)
{
}
else if(_systemStatus.state == SYSTEM_STATE_IDLE)
{
}
else if(_systemStatus.state == SYSTEM_STATE_DETECTION)
if(_systemStatus.state == SYSTEM_STATE_DETECTION)
{
ReflexData data;
std::vector<ReflexImageCV> images;
......@@ -564,11 +567,14 @@ int main(int argc, char **argv)
err = reflex_get_data_and_images(ipAddress, data, sensorIDs, images);
if(err != ERROR_NONE && err != ERROR_DATA_EMPTY)
{
ROS_ERROR("Camera Unit %s (IP %s): Failed to read data and images (Error %i)", _cameraUnitInfo.serial_number.c_str(), _cameraUnitInfo.ip_address.c_str(), err);
errConnection++;
ROS_WARN("Camera Unit %s (IP %s): Failed to read data and images (Error %i)", _cameraUnitInfo.serial_number.c_str(), _cameraUnitInfo.ip_address.c_str(), err);
}
else if(err == ERROR_NONE)
{
ros::Time tsROS = tsStartROS + ros::Duration(data.timestamp - tsStartCameraUnit);
errConnection = 0;
ros::Time tsROS = _tsStartROS + ros::Duration(data.timestamp - _tsStartCameraUnit);
for(size_t sensorID = 0; sensorID < sensorIDs.size(); sensorID++)
{
......@@ -612,29 +618,32 @@ int main(int argc, char **argv)
err = reflex_get_images(ipAddress, sensorIDs, imagesSig, imagesRef);
if(err != ERROR_NONE && err != ERROR_DATA_EMPTY)
{
ROS_ERROR("Camera Unit %s (IP %s): Failed to read images (Error %i)", _cameraUnitInfo.serial_number.c_str(), _cameraUnitInfo.ip_address.c_str(), err);
errConnection++;
ROS_WARN("Camera Unit %s (IP %s): Failed to read images (Error %i)", _cameraUnitInfo.serial_number.c_str(), _cameraUnitInfo.ip_address.c_str(), err);
}
else if(err == ERROR_NONE)
{
errConnection = 0;
for(size_t sensorID = 0; sensorID < sensorIDs.size(); sensorID++)
{
if(pubCamSig[sensorID].getNumSubscribers() > 0)
{
ros::Time ts = tsStartROS + ros::Duration(imagesRef[0].timestamp - tsStartCameraUnit);
ros::Time tsROS = _tsStartROS + ros::Duration(imagesRef[0].timestamp - _tsStartCameraUnit);
sensor_msgs::ImagePtr msgImg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imagesSig[sensorID].img).toImageMsg();
msgImg->header.stamp = ts;
msgImg->header.frame_id = "reflex_cam" + sensorID+1;
msgCameraInfo[sensorID]->header.stamp = ts;
msgImg->header.stamp = tsROS;
msgImg->header.frame_id = "reflex_cam" + sensorID + 1;
msgCameraInfo[sensorID]->header.stamp = tsROS;
pubCamSig[sensorID].publish(msgImg, msgCameraInfo[sensorID]);
}
if(pubCamRef[sensorID].getNumSubscribers() > 0)
{
ros::Time ts = tsStartROS + ros::Duration(imagesRef[0].timestamp - tsStartCameraUnit);
ros::Time tsROS = _tsStartROS + ros::Duration(imagesRef[0].timestamp - _tsStartCameraUnit);
sensor_msgs::ImagePtr msgImg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imagesRef[sensorID].img).toImageMsg();
msgImg->header.stamp = ts;
msgImg->header.frame_id = "reflex_cam" + sensorID+1;
msgCameraInfo[sensorID]->header.stamp = ts;
msgImg->header.stamp = tsROS;
msgImg->header.frame_id = "reflex_cam" + sensorID + 1;
msgCameraInfo[sensorID]->header.stamp = tsROS;
pubCamRef[sensorID].publish(msgImg, msgCameraInfo[sensorID]);
}
}
......@@ -652,10 +661,13 @@ int main(int argc, char **argv)
err = reflex_get_system_status(ipAddress, _systemStatus);
if(err != ERROR_NONE)
{
errConnection++;
ROS_WARN("Camera Unit %s (IP %s): Failed to read system status (Error %i)", _cameraUnitInfo.serial_number.c_str(), _cameraUnitInfo.ip_address.c_str(), err);
}
else
{
convertToROS(_systemStatus, msgSystemStatus);
convertToROS(_systemStatus, msgSystemStatus);
pubSystemStatus.publish(msgSystemStatus);
}
......@@ -670,9 +682,14 @@ int main(int argc, char **argv)
{
err = reflex_get_sensor_status(ipAddress, sensorID, sensorStatus[sensorID]);
if(err != ERROR_NONE)
{
errConnection++;
ROS_WARN("Camera Unit %s (IP %s): Failed to read sensor status (Error %i)", _cameraUnitInfo.serial_number.c_str(), _cameraUnitInfo.ip_address.c_str(), err);
}
else
{
errConnection = 0;
convertToROS(sensorStatus[sensorID], msgSensorStatus);
pubSensorStatus[sensorID].publish(msgSensorStatus);
}
......@@ -688,6 +705,8 @@ int main(int argc, char **argv)
if(err != ERROR_NONE)
ROS_WARN("Camera Unit %s (IP %s): Failed to read camera pose (Error %i)", _cameraUnitInfo.serial_number.c_str(), _cameraUnitInfo.ip_address.c_str(), err);
{
errConnection = 0;
convertToROS(pose, msgCameraPose);
pubCameraPose.publish(msgCameraPose);
}
......@@ -695,7 +714,6 @@ int main(int argc, char **argv)
tsUpdateCameraPose = ros::Time::now();
}
ros::Rate loopRate(20.);
ros::spinOnce();
ros::Duration dT = ros::Time::now() - tLoop;
......@@ -704,6 +722,7 @@ int main(int argc, char **argv)
ros::Duration(loopRate.expectedCycleTime() - dT).sleep();
}
ROS_INFO("Node terminated.");
return 0;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment