void UnixDatagramSocketImpl::send(Datagram &sendDatagram) { if (!isConnected) { UnixSocketAddressImpl *impl = dynamic_cast<UnixSocketAddressImpl *>(sendDatagram.getAddress().getImpl()); if (impl == NULL) throw Exception("Dest address is not compatible with datagramsocket implementation"); struct sockaddr_in outAddr; bzero((char *) &outAddr, sizeof(outAddr)); outAddr.sin_family = AF_INET; outAddr.sin_addr.s_addr = htonl(impl->getAddress()); outAddr.sin_port = htons(sendDatagram.getPortNum()); sendto(socketFd, sendDatagram.getMessage(), sendDatagram.getSize(), 0, (struct sockaddr *) &outAddr, sizeof(outAddr)); } else { ::send(socketFd, sendDatagram.getMessage(), sendDatagram.getSize(), 0); } }
//----------------------------------------------------------------------------------------------------------- bool UDPSocket::sendDatagram(const Datagram& dgram) { return sendto(socketFD_, dgram.getMessage().data(), dgram.getMessage().length(), 0, (struct sockaddr*)&dgram.getPeer(), sizeof(dgram.getPeer())) == static_cast<ssize_t>(dgram.getMessage().length()); }
int main(int argc, char* argv[]) { // Create UDP socket that listens on port 8856 and is non-blocking UDPSocket udpSocket(8856, false); if (!udpSocket.prepareUDPServerSocket()) { cerr << "Failed to create UDP socket on port " << udpSocket.getPort() << "." << endl; return 1; } cout << "Listening on port " << udpSocket.getPort() << "..." << endl; ros::init(argc, argv, "Location Manager"); ros::NodeHandle n; ros::Publisher position_pub = n.advertise<irobot_test::Locations>("topDownLocations", 100); ros::Rate loop_rate(5); while (true) { while(ros::ok()){ // Read packets as long as there are some available irobot_test::Locations locations; while (udpSocket.dataAvailable()) { Datagram* datagram = udpSocket.receiveDatagram(); ObjectPosePacket* packet = ObjectPosePacket::parse(datagram->getMessage()); if (!packet) { cerr << "Received invalid UDP packet." << endl; delete datagram; continue; } cout << "Content of packet from " << datagram->getPeerInfo() << ":" << endl; for (unsigned int i = 0; i < packet->getLocalizableObjectCount(); ++i) { const LocalizableObject& object = packet->getLocalizableObject(i); irobot_test::LocalizableObject ros_object; cout << "Object type: "; if (object.getType() == LocalizableObject::OBJECT_TYPE_SMURV) { ros_object.objecttype=OBJ_TYPE_SMURV; cout << "SmURV "<< OBJ_TYPE_SMURV << " "; } else if (object.getType() == LocalizableObject::OBJECT_TYPE_BALL) { ros_object.objecttype=OBJ_TYPE_BALL; cout << "Ball "<< OBJ_TYPE_BALL << " "; } else { ros_object.objecttype=OBJ_TYPE_OBSTACLE; cout << "Obstacle" << "SmURV "<< OBJ_TYPE_OBSTACLE << " "; } cout << ros_object.objecttype; cout << ", ID: " << object.getId(); cout << ", X: " << object.getPose().getX(); cout << ", Y: " << object.getPose().getY(); cout << ", Yaw: " << object.getPose().getYaw(); cout << ", age of pose in ms: " << object.getPoseAge() << endl; // ros_object.objecttype=object.getType(); ros_object.objectid=object.getId(); ros_object.posx=object.getPose().getX(); ros_object.posy=object.getPose().getY(); ros_object.yaw=object.getPose().getYaw(); locations.objectlist.push_back(ros_object); } cout << "------------------------------------------------------" << endl; delete packet; delete datagram; } //send messages to ros position_pub.publish(locations); ros::spinOnce(); loop_rate.sleep(); } } return 0; }