36 #ifndef RC_DYNAMICS_API_DATASTREAM_H 37 #define RC_DYNAMICS_API_DATASTREAM_H 45 #include <netinet/in.h> 48 #include <arpa/inet.h> 57 #include "roboception/msgs/frame.pb.h" 58 #include "roboception/msgs/dynamics.pb.h" 59 #include "roboception/msgs/imu.pb.h" 69 class DataReceiver :
public std::enable_shared_from_this<DataReceiver>
72 using Ptr = std::shared_ptr<DataReceiver>;
85 static Ptr create(
const std::string& ip_address,
unsigned int& port)
122 if (setsockopt(
_sockfd, SOL_SOCKET, SO_RCVTIMEO, (
const char*)&timeout,
sizeof(timeout)) < 0)
127 struct timeval _recvtimeout;
128 _recvtimeout.tv_sec = ms / 1000;
129 _recvtimeout.tv_usec = (ms % 1000) * 1000;
130 if (setsockopt(
_sockfd, SOL_SOCKET, SO_RCVTIMEO, (
const char*)&_recvtimeout,
sizeof(
struct timeval)) < 0)
150 template <
class PbMsgType>
159 int e = WSAGetLastError();
160 if (e == WSAETIMEDOUT)
176 if (e == EAGAIN || e == EWOULDBLOCK)
189 auto pb_msg = std::shared_ptr<PbMsgType>(
new PbMsgType());
190 pb_msg->ParseFromArray(
_buffer, msg_size);
208 virtual std::shared_ptr<::google::protobuf::Message>
receive(
const std::string& pb_msg_type)
213 std::stringstream msg;
214 msg <<
"Unsupported protobuf message type '" << pb_msg_type <<
"'. Only the following types are supported: ";
216 msg << p.first <<
" ";
217 throw std::invalid_argument(msg.str());
228 throw std::invalid_argument(
"Given IP address is not a valid address: " + ip_address);
232 _sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
243 struct sockaddr_in myaddr;
244 myaddr.sin_family = AF_INET;
245 myaddr.sin_addr.s_addr = inet_addr(ip_address.c_str());
246 myaddr.sin_port = htons(static_cast<u_short>(port));
247 if (bind(
_sockfd, (sockaddr*)&myaddr,
sizeof(sockaddr)) < 0)
257 int len =
sizeof(myaddr);
259 socklen_t len =
sizeof(myaddr);
262 if (getsockname(
_sockfd, (
struct sockaddr*)&myaddr, &len) < 0)
272 port_ = port = ntohs(myaddr.sin_port);
277 std::bind(&DataReceiver::receive<roboception::msgs::Frame>,
this);
279 std::bind(&DataReceiver::receive<roboception::msgs::Imu>,
this);
280 _recv_func_map[roboception::msgs::Dynamics::descriptor()->name()] =
281 std::bind(&DataReceiver::receive<roboception::msgs::Dynamics>,
this);
292 typedef std::map<std::string, std::function<std::shared_ptr<::google::protobuf::Message>()>>
map_type;
301 #endif // RC_DYNAMICS_API_DATASTREAM_H std::map< std::string, std::function< std::shared_ptr<::google::protobuf::Message >)> > map_type
std::shared_ptr< DataReceiver > Ptr
virtual std::shared_ptr<::google::protobuf::Message > receive(const std::string &pb_msg_type)
Receives the next message from data stream (string-parameter version)
std::shared_ptr< PbMsgType > receive()
Receives the next message from data stream (template-parameter version)
virtual void setTimeout(unsigned int ms)
Sets a user-specified timeout for the receivePose() method.
unsigned int getPort() const
Returns port for which the receiver was created.
A simple receiver object for handling data streamed by rc_visard's rc_dynamics module.
bool isValidIPAddress(const std::string &ip)
Checks if given string is a valid IP address.
static Ptr create(const std::string &ip_address, unsigned int &port)
Creates a data receiver bound to the user-given IP address and port number.
Exception representing an invalid socket operation.
std::string getIpAddress() const
Returns Ip address for which the receiver was created.
DataReceiver(const std::string &ip_address, unsigned int &port)