Skip to content
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 32 additions & 1 deletion prosilica_camera/src/nodes/prosilica_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@
#include <sensor_msgs/SetCameraInfo.h>

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nitpick: leave this line in

#include <boost/thread.hpp>
#include <netdb.h>
#include <arpa/inet.h>

#include <prosilica_camera/ProsilicaCameraConfig.h>
#include "prosilica/prosilica.h"
Expand Down Expand Up @@ -123,6 +125,7 @@ class ProsilicaNodelet : public nodelet::Nodelet
unsigned long guid_;
std::string hw_id_;
std::string ip_address_;
std::string hostname_;
double open_camera_retry_period_;
std::string trig_timestamp_topic_;
ros::Time trig_time_;
Expand Down Expand Up @@ -204,6 +207,8 @@ class ProsilicaNodelet : public nodelet::Nodelet

pn.param<std::string>("ip_address", ip_address_, "");
NODELET_INFO("Loaded ip address: %s", ip_address_.c_str());
pn.param<std::string>("hostname", hostname_, "");
NODELET_INFO("Loaded hostname: %s", hostname_.c_str());

pn.param<double>("open_camera_retry_period", open_camera_retry_period_, 3.);
NODELET_INFO("Retry period: %f", open_camera_retry_period_);
Expand Down Expand Up @@ -260,6 +265,29 @@ class ProsilicaNodelet : public nodelet::Nodelet

NODELET_INFO("Started Prosilica camera with guid \"%d\"", (int)camera_->guid());
}
else if(!hostname_.empty())
{
state_info_ = "Trying to load camera with hostname: " + hostname_;
NODELET_INFO("%s", state_info_.c_str());
/* Resolve hostname */
struct hostent *he;
if ((he = gethostbyname(hostname_.c_str())) != NULL)
{
std::string resolved_hostname = std::string(inet_ntoa(*(struct in_addr*)(he->h_addr)));
NODELET_INFO("hostname resolved as %s", resolved_hostname.c_str());
camera_ = boost::make_shared<prosilica::Camera>(resolved_hostname.c_str());
guid_ = camera_->guid();
hw_id_ = boost::lexical_cast<std::string>(guid_);
updater.setHardwareIDf("%d", guid_);

NODELET_INFO("Started Prosilica camera with guid \"%d\"", (int)camera_->guid());
}
else
{
NODELET_FATAL("Failed to resolve hostname: %s", hostname_.c_str());
throw std::runtime_error("Failed to resolve hostname");
}
}
else
{
updater.setHardwareID("unknown");
Expand Down Expand Up @@ -296,7 +324,10 @@ class ProsilicaNodelet : public nodelet::Nodelet
{
err << "Unable to open prosilica camera with ip address " << ip_address_ <<": "<<e.what();
}

else if (hostname_ != "")
{
err << "Unable to open prosilica camera with hostname " << hostname_ <<": "<<e.what();
}
state_info_ = err.str();
NODELET_WARN("%s", state_info_.c_str());
if(prosilica::numCameras() > 0)
Expand Down