diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..eeb7590 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,119 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rosserial_qt) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES rosserial_qt +# CATKIN_DEPENDS message_runtime +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) + +## Declare a C++ library +# add_library(rosserial_qt +# src/${PROJECT_NAME}/rosserial_qt.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(rosserial_qt ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(rosserial_qt_node src/rosserial_qt_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(rosserial_qt_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(rosserial_qt_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/make_library.py +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS rosserial_qt rosserial_qt_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +# install(DIRECTORY +# src/ros_lib +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_rosserial_qt.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/launch/rosserial_python_server.launch b/launch/rosserial_python_server.launch new file mode 100644 index 0000000..c5ba357 --- /dev/null +++ b/launch/rosserial_python_server.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/launch/server.launch b/launch/server.launch new file mode 100644 index 0000000..7069d00 --- /dev/null +++ b/launch/server.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..8f4d914 --- /dev/null +++ b/package.xml @@ -0,0 +1,49 @@ + + + rosserial_qt + 0.0.0 + The rosserial_qt package + + + + + rov + + + + + + TODO + + + + + + + + + + + + + + + + + + + roscpp + rospy + + catkin + + roscpp + rospy + + + + + + + + diff --git a/scripts/make_library.py b/scripts/make_library.py new file mode 100755 index 0000000..450f1d6 --- /dev/null +++ b/scripts/make_library.py @@ -0,0 +1,803 @@ +#!/usr/bin/env python + +##################################################################### +# Software License Agreement (BSD License) +# +# Copyright (c) 2011, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from __future__ import print_function + +__author__ = "mferguson@willowgarage.com (Michael Ferguson)" + +import roslib +import roslib.srvs +import roslib.message +import rospkg +import traceback + +import os, sys, re + +# for copying files +import shutil + + +def type_to_var(ty): + lookup = { + 1: 'uint8_t', + 2: 'uint16_t', + 4: 'uint32_t', + 8: 'uint64_t', + } + return lookup[ty] + + +##################################################################### +# Data Types + +class EnumerationType: + """ For data values. """ + + def __init__(self, name, ty, value): + self.name = name + self.type = ty + self.value = value + + def make_declaration(self, f): + f.write(' enum { %s = %s };\n' % (self.name, self.value)) + + +class PrimitiveDataType: + """ Our datatype is a C/C++ primitive. """ + + def __init__(self, name, ty, bytes): + self.name = name + self.type = ty + self.bytes = bytes + + def make_initializer(self, f, trailer): + f.write(' %s(0)%s\n' % (self.name, trailer)) + + def make_declaration(self, f): + f.write(' typedef %s _%s_type;\n _%s_type %s;\n' % (self.type, self.name, self.name, self.name)) + + def serialize_json_value(self): + return '(%s)this->%s' % (ROS_TO_QJSON_TYPES[self.type][0], self.name) + + def serialize_json(self, f): + f.write( + ' jsonObj.insert("%s", (%s)this->%s);\n' % (self.name, ROS_TO_QJSON_TYPES[self.type][0], self.name)) + + def deserialize_json_value(self): + return 'val.%s()' % (ROS_TO_QJSON_TYPES[self.type][1]) + + def deserialize_json(self, f): + f.write(' this->%s = rootObj.value("%s").%s();\n' % (self.name, self.name, ROS_TO_QJSON_TYPES[self.type][1])) + + def serialize(self, f): + cn = self.name.replace("[", "").replace("]", "").split(".")[-1] + if self.type != type_to_var(self.bytes): + f.write(' union {\n') + f.write(' %s real;\n' % self.type) + f.write(' %s base;\n' % type_to_var(self.bytes)) + f.write(' } u_%s;\n' % cn) + f.write(' u_%s.real = this->%s;\n' % (cn, self.name)) + for i in range(self.bytes): + f.write(' *(outbuffer + offset + %d) = (u_%s.base >> (8 * %d)) & 0xFF;\n' % (i, cn, i)) + else: + for i in range(self.bytes): + f.write(' *(outbuffer + offset + %d) = (this->%s >> (8 * %d)) & 0xFF;\n' % (i, self.name, i)) + f.write(' offset += sizeof(this->%s);\n' % self.name) + + def serialize_size(self, f): + f.write(' offset += sizeof(this->%s);\n' % self.name) + + def deserialize(self, f): + cn = self.name.replace("[", "").replace("]", "").split(".")[-1] + if self.type != type_to_var(self.bytes): + f.write(' union {\n') + f.write(' %s real;\n' % self.type) + f.write(' %s base;\n' % type_to_var(self.bytes)) + f.write(' } u_%s;\n' % cn) + f.write(' u_%s.base = 0;\n' % cn) + for i in range(self.bytes): + f.write(' u_%s.base |= ((%s) (*(inbuffer + offset + %d))) << (8 * %d);\n' % ( + cn, type_to_var(self.bytes), i, i)) + f.write(' this->%s = u_%s.real;\n' % (self.name, cn)) + else: + f.write(' this->%s = ((%s) (*(inbuffer + offset)));\n' % (self.name, self.type)) + for i in range(self.bytes - 1): + f.write(' this->%s |= ((%s) (*(inbuffer + offset + %d))) << (8 * %d);\n' % ( + self.name, self.type, i + 1, i + 1)) + f.write(' offset += sizeof(this->%s);\n' % self.name) + + +class MessageDataType(PrimitiveDataType): + """ For when our data type is another message. """ + + def make_initializer(self, f, trailer): + f.write(' %s()%s\n' % (self.name, trailer)) + + def serialize_json_value(self): + return 'this->%s.serializeAsJson()' % (self.name) + + def serialize_json(self, f): + f.write(' jsonObj.insert("%s", this->%s.serializeAsJson());\n' % (self.name, self.name)) + + def serialize(self, f): + f.write(' offset += this->%s.serialize(outbuffer + offset);\n' % self.name) + + def serialize_size(self, f): + f.write(' offset += this->%s.serialize_size();\n' % self.name) + + def deserialize(self, f): + f.write(' offset += this->%s.deserialize(inbuffer + offset);\n' % self.name) + + def deserialize_json(self, f): + f.write(' this->%s.deserializeFromJson(rootObj.value("%s").toObject());\n' % (self.name, self.name)) + + def deserialize_json_value(self): + return '%s::sDeserializeFromJson(val.toObject())' % self.type + + +class StringDataType(PrimitiveDataType): + """ Need to convert to signed char *. """ + + def make_initializer(self, f, trailer): + f.write(' %s("")%s\n' % (self.name, trailer)) + + def make_declaration(self, f): + f.write(' typedef std::string _%s_type;\n _%s_type %s;\n' % (self.name, self.name, self.name)) + + def serialize_json_value(self): + return 'QString::fromStdString(this->%s)' % (self.name) + + def serialize_json(self, f): + f.write(' jsonObj.insert("%s", QString::fromStdString(this->%s));\n' % (self.name, self.name)) + + def serialize(self, f): + cn = self.name.replace("[", "").replace("]", "") + f.write(' const char * cstr_%s = this->%s.c_str();\n' % (cn, self.name)) + f.write(' uint32_t length_%s = this->%s.size();\n' % (cn, self.name)) + f.write(' varToArr(outbuffer + offset, length_%s);\n' % cn) + f.write(' offset += 4;\n') + f.write(' memcpy(outbuffer + offset, cstr_%s, length_%s);\n' % (cn, cn)) + f.write(' offset += length_%s;\n' % cn) + + def serialize_size(self, f): + cn = self.name.replace("[", "").replace("]", "") + f.write(' offset += 4 + this->%s.size();\n' % (self.name)) + + def deserialize(self, f): + cn = self.name.replace("[", "").replace("]", "") + f.write(' uint32_t length_%s;\n' % cn) + f.write(' arrToVar(length_%s, (inbuffer + offset));\n' % cn) + f.write(' offset += 4;\n') + f.write(' this->%s = std::string((const char*)(inbuffer + offset), length_%s);' % (self.name, cn)) + f.write(' offset += length_%s;\n' % cn) + + def deserialize_json(self, f): + f.write(' this->%s = rootObj.value("%s").toString().toStdString();\n' % (self.name, self.name)) + + def deserialize_json_value(self): + return 'val.toString().toStdString()' + + +class TimeDataType(PrimitiveDataType): + def __init__(self, name, ty, bytes): + self.name = name + self.type = ty + self.sec = PrimitiveDataType(name + '.sec', 'uint32_t', 4) + self.nsec = PrimitiveDataType(name + '.nsec', 'uint32_t', 4) + + def make_initializer(self, f, trailer): + f.write(' %s()%s\n' % (self.name, trailer)) + + def make_declaration(self, f): + f.write(' typedef %s _%s_type;\n _%s_type %s;\n' % (self.type, self.name, self.name, self.name)) + + def serialize_json_value(self): + return 'QJsonObject{QPair("secs", (int)this->%s), QPair("nsecs", (int)this->%s)}' % ( + self.name + '.sec', self.name + '.nsec'); + + def serialize_json(self, f): + f.write(' jsonObj.insert("%s", %s);\n' % (self.name, self.serialize_json_value())) + + def serialize(self, f): + self.sec.serialize(f) + self.nsec.serialize(f) + + def serialize_size(self, f): + f.write(' offset += %d;\n' % (self.sec.bytes+self.nsec.bytes) ) + + def deserialize(self, f): + self.sec.deserialize(f) + self.nsec.deserialize(f) + + def deserialize_json(self, f): + f.write(' this->%s = ros::Time(rootObj.value("%s").toObject().value("secs").toInt(), rootObj.value("%s").toObject().value("nsecs").toInt());\n' % (self.name, self.name, self.name)) + + def deserialize_json_value(self): + return 'ros::Time(val.toObject().value("secs").toInt(), val.toObject().value("nsecs").toInt())' + +class ArrayDataType(PrimitiveDataType): + def __init__(self, name, ty, bytes, cls, array_size=None): + self.name = name + self.type = ty + self.bytes = bytes + self.size = array_size + self.cls = cls + + def make_initializer(self, f, trailer): + f.write(' %s()%s\n' % (self.name, trailer)) + + def make_declaration(self, f): + if self.size == None: + f.write(' typedef std::vector<%s> _%s_type;\n' % (self.type, self.name)) + else: + f.write(' typedef std::array<%s, %d> _%s_type;\n' % (self.type, self.size, self.name)) + f.write(' _%s_type %s;\n' % (self.name, self.name)) + f.write(' %s st_%s;\n' % (self.type, self.name)) # static instance for copy + + def serialize_json(self, f): + c = self.cls(self.name + "[i]", self.type, self.bytes) + + jsonArrName = self.name + '_jsonArr' + f.write(' QJsonArray %s;\n' % jsonArrName) + f.write(' for( size_t i = 0; i < %s.size(); i++){\n' % self.name) + f.write(' %s.append(%s);\n' % (jsonArrName, c.serialize_json_value())) + f.write(' }\n') + f.write(' jsonObj.insert("%s", %s);\n' % (self.name, jsonArrName)) + + def serialize(self, f): + c = self.cls(self.name + "[i]", self.type, self.bytes) + if self.size == None: + # serialize length + f.write(' uint32_t %s_length = this->%s.size();\n' % (self.name, self.name)) + f.write(' *(outbuffer + offset + 0) = (%s_length >> (8 * 0)) & 0xFF;\n' % self.name) + f.write(' *(outbuffer + offset + 1) = (%s_length >> (8 * 1)) & 0xFF;\n' % self.name) + f.write(' *(outbuffer + offset + 2) = (%s_length >> (8 * 2)) & 0xFF;\n' % self.name) + f.write(' *(outbuffer + offset + 3) = (%s_length >> (8 * 3)) & 0xFF;\n' % self.name) + f.write(' offset += sizeof(%s_length);\n' % self.name) + f.write(' for( uint32_t i = 0; i < %s_length; i++){\n' % self.name) + c.serialize(f) + f.write(' }\n') + else: + f.write(' for( uint32_t i = 0; i < %d; i++){\n' % (self.size)) + c.serialize(f) + f.write(' }\n') + + def serialize_size(self, f): + c = self.cls(self.name + "[i]", self.type, self.bytes) + if self.size == None: + f.write(' offset += sizeof(uint32_t);\n') + f.write(' for( uint32_t i = 0; i < %s.size(); i++){\n' % self.name) + c.serialize_size(f) + f.write(' }\n') + else: + f.write(' for( uint32_t i = 0; i < %d; i++){\n' % (self.size)) + c.serialize_size(f) + f.write(' }\n') + + def deserialize(self, f): + if self.size == None: + c = self.cls("st_" + self.name, self.type, self.bytes) + # deserialize length + f.write(' uint32_t %s_lengthT = ((uint32_t) (*(inbuffer + offset))); \n' % self.name) + f.write(' %s_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); \n' % self.name) + f.write(' %s_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); \n' % self.name) + f.write(' %s_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); \n' % self.name) + f.write(' offset += sizeof(%s_lengthT);\n' % self.name) + if self.size == None: + f.write(' this->%s.resize(%s_lengthT);\n' % (self.name, self.name)) + # copy to array + f.write(' for( uint32_t i = 0; i < %s_lengthT; i++){\n' % (self.name)) + c.deserialize(f) + f.write(' this->%s[i] = st_%s;\n' % (self.name, self.name)) + f.write(' }\n') + else: + c = self.cls(self.name + "[i]", self.type, self.bytes) + f.write(' for( uint32_t i = 0; i < %d; i++){\n' % (self.size)) + c.deserialize(f) + f.write(' }\n') + + def deserialize_json(self, f): + c = self.cls(self.name, self.type, self.bytes) + f.write(' QJsonArray %s_arr = rootObj.value("%s").toArray();\n' % (self.name, self.name)) + f.write(' int %s_lengthT = %s_arr.size();\n' % (self.name, self.name)) + if self.size == None: + f.write(' this->%s.resize(%s_lengthT);\n' % (self.name, self.name)) + f.write(' for(int i = 0; i < %s_lengthT; i++){\n' % (self.name)) + f.write(' QJsonValue val = %s_arr.at(i);\n' % self.name) + f.write(' this->%s[i] = %s;\n' % (self.name, c.deserialize_json_value())) + f.write(' }\n') + + +##################################################################### +# Messages + +class Message: + """ Parses message definitions into something we can export. """ + global ROS_TO_EMBEDDED_TYPES + + def __init__(self, name, package, definition, md5): + + self.name = name # name of message/class + self.package = package # package we reside in + self.md5 = md5 # checksum + self.includes = list() # other files we must include + + self.data = list() # data types for code generation + self.enums = list() + + # parse definition + for line in definition: + # prep work + line = line.strip().rstrip() + value = None + if line.find("#") > -1: + line = line[0:line.find("#")] + if line.find("=") > -1: + try: + value = line[line.find("=") + 1:] + except: + value = '"' + line[line.find("=") + 1:] + '"'; + line = line[0:line.find("=")] + + # find package/class name + line = line.replace("\t", " ") + l = line.split(" ") + while "" in l: + l.remove("") + if len(l) < 2: + continue + ty, name = l[0:2] + if value != None: + self.enums.append(EnumerationType(name, ty, value)) + continue + + try: + type_package, type_name = ty.split("/") + except: + type_package = None + type_name = ty + type_array = False + if type_name.find('[') > 0: + type_array = True + try: + type_array_size = int(type_name[type_name.find('[') + 1:type_name.find(']')]) + except: + type_array_size = None + type_name = type_name[0:type_name.find('[')] + + # convert to C type if primitive, expand name otherwise + try: + code_type = ROS_TO_EMBEDDED_TYPES[type_name][0] + size = ROS_TO_EMBEDDED_TYPES[type_name][1] + cls = ROS_TO_EMBEDDED_TYPES[type_name][2] + for include in ROS_TO_EMBEDDED_TYPES[type_name][3]: + if include not in self.includes: + self.includes.append(include) + except: + if type_package == None: + type_package = self.package + if type_package + "/" + type_name not in self.includes: + self.includes.append(type_package + "/" + type_name) + cls = MessageDataType + code_type = type_package + "::" + type_name + size = 0 + if type_array: + self.data.append(ArrayDataType(name, code_type, size, cls, type_array_size)) + else: + self.data.append(cls(name, code_type, size)) + + def _write_serializer(self, f): + # serializer + f.write(' virtual size_t serialize(unsigned char *outbuffer) const\n') + f.write(' {\n') + f.write(' (void)outbuffer;\n') + f.write(' size_t offset = 0;\n') + for d in self.data: + d.serialize(f) + f.write(' return offset;\n'); + f.write(' }\n') + f.write('\n') + + def _write_serializer_size(self, f): + # serializer + f.write(' virtual size_t serialize_size() const\n') + f.write(' {\n') + f.write(' size_t offset = 0;\n') + for d in self.data: + d.serialize_size(f) + f.write(' return offset;\n'); + f.write(' }\n') + f.write('\n') + + def _write_json_serializer(self, f): + # serializer + f.write(' virtual QJsonObject serializeAsJson() const\n') + f.write(' {\n') + f.write(' QJsonObject jsonObj;\n') + for d in self.data: + d.serialize_json(f) + f.write(' return jsonObj;\n'); + f.write(' }\n') + f.write('\n') + + def _write_deserializer(self, f): + # deserializer + f.write(' virtual size_t deserialize(unsigned char *inbuffer)\n') + f.write(' {\n') + f.write(' (void)inbuffer;\n') + f.write(' size_t offset = 0;\n') + for d in self.data: + d.deserialize(f) + f.write(' return offset;\n'); + f.write(' }\n') + f.write('\n') + + def _write_json_deserializer(self, f): + # deserializer + f.write(' static %s::%s sDeserializeFromJson(const QJsonObject &obj)\n' % (self.package, self.name)) + f.write(' {\n') + f.write(' %s::%s msg;\n' % (self.package, self.name)) + f.write(' msg.deserializeFromJson(obj);\n') + f.write(' return msg;\n') + f.write(' }\n') + f.write('\n') + f.write(' virtual void deserializeFromJson(const QByteArray &json)\n') + f.write(' {\n') + f.write(' QJsonParseError error;\n') + f.write(' QJsonDocument doc = QJsonDocument::fromJson(json, &error);\n') + f.write(' if(error.error != QJsonParseError::NoError)\n') + f.write(' throw std::runtime_error("Failed to parse json");\n') + f.write(' QJsonObject rootObj = doc.object();\n') + f.write(' deserializeFromJson(rootObj);\n') + f.write(' }\n') + f.write('\n') + f.write(' virtual void deserializeFromJson(const QJsonObject &rootObj)\n') + f.write(' {\n') + f.write(' (void)rootObj;\n') + for d in self.data: + d.deserialize_json(f) + f.write(' }\n') + f.write('\n') + + def _write_std_includes(self, f): + f.write('#include \n') + f.write('#include \n') + f.write('#include \n') + f.write('#include \n') + f.write('#include \n') + f.write('#include \n') + f.write('#include \n') + f.write('#include \n') + f.write('#include \n') + f.write('#include \n') + f.write('#include "ros/msg.h"\n') + + def _write_msg_includes(self, f): + for i in self.includes: + f.write('#include "%s.h"\n' % i) + + def _write_constructor(self, f): + f.write(' %s()%s\n' % (self.name, ':' if self.data else '')) + if self.data: + for d in self.data[:-1]: + d.make_initializer(f, ',') + self.data[-1].make_initializer(f, '') + f.write(' {\n }\n\n') + + def _write_data(self, f): + for d in self.data: + d.make_declaration(f) + for e in self.enums: + e.make_declaration(f) + f.write('\n') + + def _write_getType(self, f): + f.write(' static const std::string& getType(){\n') + f.write(' static std::string type = "%s/%s";\n' % (self.package, self.name)) + f.write(' return type;\n') + f.write(' }\n') + + def _write_getMD5(self, f): + f.write(' static const std::string& getMD5(){\n') + f.write(' static std::string md5 = "%s";\n' % (self.md5)) + f.write(' return md5;\n') + f.write(' }\n') + + def _write_impl(self, f): + f.write(' class %s : public ros::Msg\n' % self.name) + f.write(' {\n') + f.write(' public:\n') + self._write_data(f) + self._write_constructor(f) + self._write_json_serializer(f) + self._write_json_deserializer(f) + self._write_serializer_size(f) + self._write_serializer(f) + self._write_deserializer(f) + self._write_getType(f) + self._write_getMD5(f) + f.write('\n') + f.write(' };\n') + + def make_header(self, f): + f.write('#ifndef _ROS_%s_%s_h\n' % (self.package, self.name)) + f.write('#define _ROS_%s_%s_h\n' % (self.package, self.name)) + f.write('\n') + self._write_std_includes(f) + self._write_msg_includes(f) + + f.write('\n') + f.write('namespace %s\n' % self.package) + f.write('{\n') + f.write('\n') + self._write_impl(f) + f.write('\n') + f.write('}\n') + + f.write('#endif') + + +class Service: + def __init__(self, name, package, definition, md5req, md5res): + """ + @param name - name of service + @param package - name of service package + @param definition - list of lines of definition + """ + + self.name = name + self.package = package + + sep_line = len(definition) + sep = re.compile('---*') + for i in range(0, len(definition)): + if (None != re.match(sep, definition[i])): + sep_line = i + break + self.req_def = definition[0:sep_line] + self.resp_def = definition[sep_line + 1:] + + self.req = Message(name + "Request", package, self.req_def, md5req) + self.resp = Message(name + "Response", package, self.resp_def, md5res) + + def make_header(self, f): + f.write('#ifndef _ROS_SERVICE_%s_h\n' % self.name) + f.write('#define _ROS_SERVICE_%s_h\n' % self.name) + + self.req._write_std_includes(f) + includes = self.req.includes + includes.extend(self.resp.includes) + includes = list(set(includes)) + for inc in includes: + f.write('#include "%s.h"\n' % inc) + + f.write('\n') + f.write('namespace %s\n' % self.package) + f.write('{\n') + f.write('\n') + f.write('static const std::string %s_TYPE = "%s/%s";\n' % (self.name.upper(), self.package, self.name)) + + def write_type(out, name): + out.write(' static const std::string & getType(){ return %s_TYPE; }\n' % (name)) + + _write_getType = lambda out: write_type(out, self.name.upper()) + self.req._write_getType = _write_getType + self.resp._write_getType = _write_getType + + f.write('\n') + self.req._write_impl(f) + f.write('\n') + self.resp._write_impl(f) + f.write('\n') + f.write(' class %s {\n' % self.name) + f.write(' public:\n') + f.write(' typedef %s Request;\n' % self.req.name) + f.write(' typedef %s Response;\n' % self.resp.name) + f.write(' };\n') + f.write('\n') + + f.write('}\n') + + f.write('#endif\n') + + +##################################################################### +# Make a Library + +def MakeLibrary(package, output_path, rospack): + pkg_dir = rospack.get_path(package) + + # find the messages in this package + messages = list() + if os.path.exists(pkg_dir + "/msg"): + print('Exporting %s\n' % package) + sys.stdout.write(' Messages:') + sys.stdout.write('\n ') + for f in os.listdir(pkg_dir + "/msg"): + if f.endswith(".msg"): + msg_file = pkg_dir + "/msg/" + f + # add to list of messages + print('%s,' % f[0:-4], end='') + definition = open(msg_file).readlines() + msg_class = roslib.message.get_message_class(package + '/' + f[0:-4]) + if msg_class: + md5sum = msg_class._md5sum + messages.append(Message(f[0:-4], package, definition, md5sum)) + else: + err_msg = "Unable to build message: %s/%s\n" % (package, f[0:-4]) + sys.stderr.write(err_msg) + + # find the services in this package + if (os.path.exists(pkg_dir + "/srv/")): + if messages == list(): + print('Exporting %s\n' % package) + else: + print('\n') + sys.stdout.write(' Services:') + sys.stdout.write('\n ') + for f in os.listdir(pkg_dir + "/srv"): + if f.endswith(".srv"): + srv_file = pkg_dir + "/srv/" + f + # add to list of messages + print('%s,' % f[0:-4], end='') + definition, service = roslib.srvs.load_from_file(srv_file) + definition = open(srv_file).readlines() + srv_class = roslib.message.get_service_class(package + '/' + f[0:-4]) + if srv_class: + md5req = srv_class._request_class._md5sum + md5res = srv_class._response_class._md5sum + messages.append(Service(f[0:-4], package, definition, md5req, md5res)) + else: + err_msg = "Unable to build service: %s/%s\n" % (package, f[0:-4]) + sys.stderr.write(err_msg) + print('\n') + elif messages != list(): + print('\n') + + # generate for each message + output_path = output_path + "/" + package + for msg in messages: + if not os.path.exists(output_path): + os.makedirs(output_path) + header = open(output_path + "/" + msg.name + ".h", "w") + msg.make_header(header) + header.close() + + +def rosserial_generate(rospack, path): + # gimme messages + failed = [] + for p in sorted(rospack.list()): + try: + MakeLibrary(p, path, rospack) + except Exception as e: + failed.append(p + " (" + str(e) + ")") + print('[%s]: Unable to build messages: %s\n' % (p, str(e))) + print(traceback.format_exc()) + print('\n') + if len(failed) > 0: + print('*** Warning, failed to generate libraries for the following packages: ***') + for f in failed: + print(' %s' % f) + raise Exception("Failed to generate libraries for: " + str(failed)) + print('\n') + + +def rosserial_client_copy_files(rospack, path): + os.makedirs(path + "/ros") + os.makedirs(path + "/tf") + files = ['duration.cpp', + 'time.cpp', + 'ros/duration.h', + 'ros/msg.h', + 'ros/node_handle.h', + 'ros/publisher.h', + 'ros/service_client.h', + 'ros/service_server.h', + 'ros/subscriber.h', + 'ros/time.h', + 'tf/tf.h', + 'tf/transform_broadcaster.h'] + mydir = rospack.get_path("rosserial_client") + for f in files: + shutil.copy(mydir + "/src/ros_lib/" + f, path + f) + + +ROS_TO_QJSON_TYPES = { + 'bool': ('bool', 'toBool'), + 'int8_t': ('int', 'toInt'), + 'uint8_t': ('int', 'toInt'), + 'int16_t': ('int', 'toInt'), + 'uint16_t': ('int', 'toInt'), + 'int32_t': ('qint64', 'toInt'), + 'uint32_t': ('qint64', 'toInt'), + 'int64_t': ('qint64', 'toInt'), + 'uint64_t': ('qint64', 'toInt'), + 'float': ('double', 'toDouble'), + 'double': ('double', 'toDouble') +} + +ROS_TO_EMBEDDED_TYPES = { + 'bool': ('bool', 1, PrimitiveDataType, []), + 'byte': ('int8_t', 1, PrimitiveDataType, []), + 'int8': ('int8_t', 1, PrimitiveDataType, []), + 'char': ('uint8_t', 1, PrimitiveDataType, []), + 'uint8': ('uint8_t', 1, PrimitiveDataType, []), + 'int16': ('int16_t', 2, PrimitiveDataType, []), + 'uint16': ('uint16_t', 2, PrimitiveDataType, []), + 'int32': ('int32_t', 4, PrimitiveDataType, []), + 'uint32': ('uint32_t', 4, PrimitiveDataType, []), + 'int64': ('int64_t', 8, PrimitiveDataType, []), + 'uint64': ('uint64_t', 4, PrimitiveDataType, []), + 'float32': ('float', 4, PrimitiveDataType, []), + 'float64': ('double', 8, PrimitiveDataType, []), + 'time': ('ros::Time', 8, TimeDataType, ['ros/time']), + 'duration': ('ros::Duration', 8, TimeDataType, ['ros/duration']), + 'string': ('std::string', 0, StringDataType, []), + 'Header': ('std_msgs::Header', 0, MessageDataType, ['std_msgs/Header']) +} + +__usage__ = """ +make_libraries.py generates rosserial-like library files. It +requires the location of your project's ros_lib folder. + +rosrun rosserial_qt make_libraries.py +""" + + +def main(): + # need correct inputs + if (len(sys.argv) < 2): + print(__usage__) + exit() + + rospack = rospkg.RosPack() + + # get output path + path = sys.argv[1] + if path[-1] == "/": + path = path[0:-1] + print + "\nExporting to %s" % path + + # copy ros_lib stuff in + src_dir = rospack.get_path("rosserial_qt") + #shutil.copytree(src_dir + "/src/ros_lib", path + "/ros_lib") + + rosserial_generate(rospack, path + "/ros_lib") + + +if __name__ == "__main__": + main() diff --git a/src/ros_lib/RosQtSocket.cpp b/src/ros_lib/RosQtSocket.cpp new file mode 100644 index 0000000..7cb6f09 --- /dev/null +++ b/src/ros_lib/RosQtSocket.cpp @@ -0,0 +1,82 @@ +#include "ros/RosQtSocket.h" + +#define DEFAULT_PORT 11411 + +using std::string; +using std::cerr; +using std::endl; + + +RosQtSocket::RosQtSocket(QObject *parent): + QObject(parent) +{ + QObject::connect(&m_socket, SIGNAL(readyRead()), this, SIGNAL(readyRead())); + QObject::connect(&m_socket, SIGNAL(connected()), this, SLOT(onConnected())); +} + +void RosQtSocket::doConnect() +{ + if(m_socket.state() == QAbstractSocket::UnconnectedState) + { + m_socket.connectToHost(m_address, m_port); + } + +} + +void RosQtSocket::init(const std::string &server_hostname) +{ + m_address = QHostAddress(QString::fromStdString(server_hostname)); + m_port = DEFAULT_PORT; + + doConnect(); +} + +int64_t RosQtSocket::read(unsigned char *data, int max_length) +{ + if(m_socket.state() != QAbstractSocket::ConnectedState) + { + std::cerr << "Failed to receive data from server, not connected " << std::endl; + doConnect(); + return -1; + } + + int64_t result = m_socket.read((char*)data, max_length); + if(result < 0) + { + std::cerr << "Failed to receive data from server" << std::endl; + return -1; + } + + return result; +} + +bool RosQtSocket::write(const unsigned char *data, int length) +{ + if(m_socket.state() != QAbstractSocket::ConnectedState) + { + std::cerr << "Failed to write data to the server, not connected " << std::endl; + doConnect(); + return false; + } + else + { + qint64 result = m_socket.write((const char*)data, length); + if(result != length) + { + std::cerr << "Failed to write all the data to the server" << std::endl; + return false; + } + } + + return true; +} + +unsigned long RosQtSocket::time() +{ + return static_cast(QDateTime::currentMSecsSinceEpoch()); +} + +void RosQtSocket::onConnected() +{ + m_socket.setSocketOption(QAbstractSocket::LowDelayOption, 1); +} diff --git a/src/ros_lib/duration.cpp b/src/ros_lib/duration.cpp new file mode 100644 index 0000000..f471c33 --- /dev/null +++ b/src/ros_lib/duration.cpp @@ -0,0 +1,81 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include "ros/duration.h" + +namespace ros +{ + void normalizeSecNSecSigned(int32_t &sec, int32_t &nsec) + { + int32_t nsec_part = nsec; + int32_t sec_part = sec; + + while (nsec_part > 1000000000L) + { + nsec_part -= 1000000000L; + ++sec_part; + } + while (nsec_part < 0) + { + nsec_part += 1000000000L; + --sec_part; + } + sec = sec_part; + nsec = nsec_part; + } + + Duration& Duration::operator+=(const Duration &rhs) + { + sec += rhs.sec; + nsec += rhs.nsec; + normalizeSecNSecSigned(sec, nsec); + return *this; + } + + Duration& Duration::operator-=(const Duration &rhs){ + sec += -rhs.sec; + nsec += -rhs.nsec; + normalizeSecNSecSigned(sec, nsec); + return *this; + } + + Duration& Duration::operator*=(double scale){ + sec *= scale; + nsec *= scale; + normalizeSecNSecSigned(sec, nsec); + return *this; + } + +} diff --git a/src/ros_lib/ros/RosQtSocket.h b/src/ros_lib/ros/RosQtSocket.h new file mode 100644 index 0000000..72b60a4 --- /dev/null +++ b/src/ros_lib/ros/RosQtSocket.h @@ -0,0 +1,66 @@ +/** +Software License Agreement (BSD) + +\file WindowsSocket.h +\authors Kareem Shehata +\copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: + * Redistributions of source code must retain the above copyright notice, this list of conditions and the + following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef ROS_QT_SOCKET_H_ +#define ROS_QT_SOCKET_H_ + +#include +#include "ros/RosQtSocket.h" +#include +#include +#include +#include +#include + +class RosQtSocket : public QObject +{ + Q_OBJECT +public: + + RosQtSocket (QObject *parent=0); + + void doConnect(); + + void init (const std::string &server_hostname); + + int64_t read (unsigned char *data, int max_length); + + bool write (const unsigned char *data, int length); + + unsigned long time (); + +private slots: + void onConnected(); + +signals: + void readyRead(); + +private: + QTcpSocket m_socket; + quint16 m_port; + QHostAddress m_address; +}; + +#endif diff --git a/src/ros_lib/ros/duration.h b/src/ros_lib/ros/duration.h new file mode 100644 index 0000000..8f87db8 --- /dev/null +++ b/src/ros_lib/ros/duration.h @@ -0,0 +1,67 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROS_DURATION_H_ +#define _ROS_DURATION_H_ + +#include +#include + +namespace ros { + + void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec); + + class Duration + { + public: + int32_t sec, nsec; + + Duration() : sec(0), nsec(0) {} + Duration(int32_t _sec, int32_t _nsec) : sec(_sec), nsec(_nsec) + { + normalizeSecNSecSigned(sec, nsec); + } + + double toSec() const { return (double)sec + 1e-9*(double)nsec; } + void fromSec(double t) { sec = (uint32_t) floor(t); nsec = (uint32_t) round((t-sec) * 1e9); } + + Duration& operator+=(const Duration &rhs); + Duration& operator-=(const Duration &rhs); + Duration& operator*=(double scale); + }; + +} + +#endif + diff --git a/src/ros_lib/ros/msg.h b/src/ros_lib/ros/msg.h new file mode 100644 index 0000000..336f7b4 --- /dev/null +++ b/src/ros_lib/ros/msg.h @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROS_MSG_H_ +#define _ROS_MSG_H_ + +#include +#include +#include +#include + +namespace ros { + +/* Base Message Type */ +class Msg +{ +public: + virtual size_t serialize_size() const = 0; + virtual size_t serialize(unsigned char *outbuffer) const = 0; + virtual size_t deserialize(unsigned char *data) = 0; + + virtual QJsonObject serializeAsJson() const = 0; + virtual void deserializeFromJson(const QByteArray &json) = 0; + virtual void deserializeFromJson(const QJsonObject &rootObj) = 0; + + /** + * @brief This tricky function handles promoting a 32bit float to a 64bit + * double, so that AVR can publish messages containing float64 + * fields, despite AVV having no native support for double. + * + * @param[out] outbuffer pointer for buffer to serialize to. + * @param[in] f value to serialize. + * + * @return number of bytes to advance the buffer pointer. + * + */ + static int serializeAvrFloat64(unsigned char* outbuffer, const float f) + { + const int32_t* val = (int32_t*) &f; + int32_t exp = ((*val >> 23) & 255); + if (exp != 0) + { + exp += 1023 - 127; + } + + int32_t sig = *val; + *(outbuffer++) = 0; + *(outbuffer++) = 0; + *(outbuffer++) = 0; + *(outbuffer++) = (sig << 5) & 0xff; + *(outbuffer++) = (sig >> 3) & 0xff; + *(outbuffer++) = (sig >> 11) & 0xff; + *(outbuffer++) = ((exp << 4) & 0xF0) | ((sig >> 19) & 0x0F); + *(outbuffer++) = (exp >> 4) & 0x7F; + + // Mark negative bit as necessary. + if (f < 0) + { + *(outbuffer - 1) |= 0x80; + } + + return 8; + } + + /** + * @brief This tricky function handles demoting a 64bit double to a + * 32bit float, so that AVR can understand messages containing + * float64 fields, despite AVR having no native support for double. + * + * @param[in] inbuffer pointer for buffer to deserialize from. + * @param[out] f pointer to place the deserialized value in. + * + * @return number of bytes to advance the buffer pointer. + */ + static int deserializeAvrFloat64(const unsigned char* inbuffer, float* f) + { + uint32_t* val = (uint32_t*)f; + inbuffer += 3; + + // Copy truncated mantissa. + *val = ((uint32_t)(*(inbuffer++)) >> 5 & 0x07); + *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 3; + *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 11; + *val |= ((uint32_t)(*inbuffer) & 0x0f) << 19; + + // Copy truncated exponent. + uint32_t exp = ((uint32_t)(*(inbuffer++)) & 0xf0)>>4; + exp |= ((uint32_t)(*inbuffer) & 0x7f) << 4; + if (exp != 0) + { + *val |= ((exp) - 1023 + 127) << 23; + } + + // Copy negative sign. + *val |= ((uint32_t)(*(inbuffer++)) & 0x80) << 24; + + return 8; + } + + // Copy data from variable into a byte array + template + static void varToArr(A arr, const V var) + { + for(size_t i = 0; i < sizeof(V); i++) + arr[i] = (var >> (8 * i)); + } + + // Copy data from a byte array into variable + template + static void arrToVar(V& var, const A arr) + { + var = 0; + for(size_t i = 0; i < sizeof(V); i++) + var |= (arr[i] << (8 * i)); + } + +}; + +} // namespace ros + +#endif diff --git a/src/ros_lib/ros/node_handle.h b/src/ros_lib/ros/node_handle.h new file mode 100644 index 0000000..d90b89c --- /dev/null +++ b/src/ros_lib/ros/node_handle.h @@ -0,0 +1,581 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_NODE_HANDLE_H_ +#define ROS_NODE_HANDLE_H_ + +#include + +#include "std_msgs/Time.h" +#include "rosserial_msgs/TopicInfo.h" +#include "rosserial_msgs/Log.h" +#include "rosserial_msgs/RequestParam.h" +#include "ros/msg.h" + +#include + +namespace ros { + +class NodeHandleBase_ : public QObject{ + Q_OBJECT +public: + NodeHandleBase_(QObject *parent=0) : QObject(parent){} + + virtual int publish(int id, const Msg* msg)=0; + virtual int spinOnce()=0; + virtual bool connected()=0; +}; +} + +#include "RosQtSocket.h" +#include "ros/publisher.h" +#include "ros/subscriber.h" +#include "ros/service_server.h" +#include "ros/service_client.h" + +namespace ros { + +const uint8_t SYNC_SECONDS = 5; +const uint8_t MODE_FIRST_FF = 0; +/* + * The second sync byte is a protocol version. It's value is 0xff for the first + * version of the rosserial protocol (used up to hydro), 0xfe for the second version + * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable + * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy + * rosserial_arduino. It must be changed in both this file and in + * rosserial_python/src/rosserial_python/SerialClient.py + */ +const uint8_t MODE_PROTOCOL_VER = 1; +const uint8_t PROTOCOL_VER1 = 0xff; // through groovy +const uint8_t PROTOCOL_VER2 = 0xfe; // in hydro +const uint8_t PROTOCOL_VER = PROTOCOL_VER2; +const uint8_t MODE_SIZE_B1 = 2; // LSB +const uint8_t MODE_SIZE_B2 = 3; +const uint8_t MODE_SIZE_B3 = 4; +const uint8_t MODE_SIZE_B4 = 5; // MSB +const uint8_t MODE_SIZE_CHECKSUM = 6; // checksum for msg size received from size L and H +const uint8_t MODE_TOPIC_L = 7; // waiting for topic id +const uint8_t MODE_TOPIC_H = 8; +const uint8_t MODE_MESSAGE = 9; +const uint8_t MODE_MSG_CHECKSUM = 10; // checksum for msg and topic id + +const uint32_t SERIAL_MSG_TIMEOUT = 2000; // 2000 milliseconds to recieve all of message data +const uint32_t READ_BUFFER_SIZE = 4096; + +using rosserial_msgs::TopicInfo; + +/* Node Handle */ +class NodeHandle : public NodeHandleBase_ +{ + Q_OBJECT +protected: + RosQtSocket hardware_; + + /* time used for syncing */ + uint32_t rt_time; + + /* used for computing current time */ + uint32_t sec_offset, nsec_offset; + + std::vector message_in; + std::vector message_out; + + std::vector publishers; + std::vector subscribers; + + uint32_t INPUT_SIZE; + uint32_t OUTPUT_SIZE; + + /* + * Setup Functions + */ +public: + NodeHandle(uint32_t input_size=10000000, uint32_t output_size=10000000, QObject *parent=0) : + NodeHandleBase_(parent), + INPUT_SIZE(input_size), + OUTPUT_SIZE(output_size), + configured_(false) + { + message_in.resize(INPUT_SIZE); + message_out.resize(OUTPUT_SIZE); + + for(unsigned int i=0; i< INPUT_SIZE; i++) + message_in[i] = 0; + + for(unsigned int i=0; i< OUTPUT_SIZE; i++) + message_out[i] = 0; + + connect(&hardware_, SIGNAL(readyRead()), this, SLOT(onReadyRead())); + startTimer(100); + } + + void timerEvent(QTimerEvent *event) + { + (void)event; + spinOnce(); + } + + /* Start serial, initialize buffers */ + void initNode(){ + + } + + /* Start a named port, which may be network server IP, initialize buffers */ + void initNode(const std::string &portName){ + hardware_.init(portName); + mode_ = 0; + bytes_ = 0; + index_ = 0; + topic_ = 0; + } + +protected: + //State machine variables for spinOnce + int mode_; + uint32_t bytes_; + int topic_; + int index_; + unsigned int checksum_; + + bool configured_; + + /* used for syncing the time */ + uint32_t last_sync_time; + uint32_t last_sync_receive_time; + uint32_t last_msg_timeout_time; + +public: + /* This function goes in your loop() function, it handles + * serial input and callbacks for subscribers. + */ + + + virtual int spinOnce(){ + unsigned char buffer[READ_BUFFER_SIZE]; + + /* restart if timed out */ + uint32_t c_time = hardware_.time(); + if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){ + configured_ = false; + } + + /* reset if message has timed out */ + if ( mode_ != MODE_FIRST_FF){ + if (c_time > last_msg_timeout_time){ + mode_ = MODE_FIRST_FF; + } + } + + /* while available buffer, read data */ + while(true) + { + int64_t result = hardware_.read(buffer, READ_BUFFER_SIZE); + if(result == 0) + { + // no more data + break; + } + else if(result < 0) + { + // connection lost + configured_ = false; + return -2; + } + + for(size_t i=0 ; i<(size_t)result ; i++) + { + unsigned char data = buffer[i]; + + checksum_ += data; + if( mode_ == MODE_MESSAGE ){ /* message data being recieved */ + message_in[index_++] = data; + bytes_--; + if(bytes_ == 0) /* is message complete? if so, checksum */ + mode_ = MODE_MSG_CHECKSUM; + }else if( mode_ == MODE_FIRST_FF ){ + if(data == 0xff){ + mode_++; + last_msg_timeout_time = c_time + SERIAL_MSG_TIMEOUT; + } + else if( hardware_.time() - c_time > (SYNC_SECONDS*1000)){ + /* We have been stuck in spinOnce too long, return error */ + configured_=false; + return -2; + } + }else if( mode_ == MODE_PROTOCOL_VER ){ + if(data == PROTOCOL_VER){ + mode_++; + }else{ + mode_ = MODE_FIRST_FF; + if (configured_ == false) + requestSyncTime(); /* send a msg back showing our protocol version */ + } + }else if( mode_ == MODE_SIZE_B1 ){ /* bottom half of message size */ + bytes_ = data; + index_ = 0; + mode_++; + checksum_ = data; /* first byte for calculating size checksum */ + }else if( mode_ == MODE_SIZE_B2 ){ /* part 2 of message size */ + bytes_ += data<<8; + mode_++; + }else if( mode_ == MODE_SIZE_B3 ){ /* part 3 of message size */ + bytes_ += data<<16; + mode_++; + }else if( mode_ == MODE_SIZE_B4 ){ /* part 4 of message size */ + bytes_ += data<<24; + mode_++; + } + else if( mode_ == MODE_SIZE_CHECKSUM ){ + if( (checksum_%256) == 255) + { + mode_++; + } + else + { + mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */ + } + }else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */ + topic_ = data; + mode_++; + checksum_ = data; /* first byte included in checksum */ + }else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */ + topic_ += data<<8; + mode_ = MODE_MESSAGE; + if(bytes_ == 0) + mode_ = MODE_MSG_CHECKSUM; + }else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */ + mode_ = MODE_FIRST_FF; + if( (checksum_%256) == 255){ + if(topic_ == TopicInfo::ID_PUBLISHER){ + requestSyncTime(); + negotiateTopics(); + last_sync_time = c_time; + last_sync_receive_time = c_time; + return -1; + }else if(topic_ == TopicInfo::ID_TIME){ + syncTime(message_in.data()); + }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){ + req_param_resp.deserialize(message_in.data()); + param_recieved= true; + }else if(topic_ == TopicInfo::ID_TX_STOP){ + configured_ = false; + }else{ + if(subscribers[topic_-100]) + subscribers[topic_-100]->callback( message_in.data()); + } + } + } + } + } + + /* occasionally sync time */ + if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){ + requestSyncTime(); + last_sync_time = c_time; + } + + return 0; + } + + + /* Are we connected to the PC? */ + virtual bool connected() { + return configured_; + } + + /******************************************************************** + * Time functions + */ + + void requestSyncTime() + { + std_msgs::Time t; + publish(TopicInfo::ID_TIME, &t); + rt_time = hardware_.time(); + } + + void syncTime(uint8_t * data) + { + std_msgs::Time t; + uint32_t offset = hardware_.time() - rt_time; + + t.deserialize(data); + t.data.sec += offset/1000; + t.data.nsec += (offset%1000)*1000000UL; + + this->setNow(t.data); + last_sync_receive_time = hardware_.time(); + } + + Time now() + { + uint32_t ms = hardware_.time(); + Time current_time; + current_time.sec = ms/1000 + sec_offset; + current_time.nsec = (ms%1000)*1000000UL + nsec_offset; + normalizeSecNSec(current_time.sec, current_time.nsec); + return current_time; + } + + void setNow( Time & new_now ) + { + uint32_t ms = hardware_.time(); + sec_offset = new_now.sec - ms/1000 - 1; + nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL; + normalizeSecNSec(sec_offset, nsec_offset); + } + + /******************************************************************** + * Topic Management + */ + + /* Register a new publisher */ + template + bool advertise(Publisher & p) + { + size_t i = publishers.size(); + publishers.push_back(&p); + p.id_ = i+100; + p.nh_ = this; + return true; + } + + /* Register a new subscriber */ + template + bool subscribe(SubscriberT& s){ + size_t i = subscribers.size(); + subscribers.push_back(static_cast(&s)); + s.id_ = i+100; + return true; + } + + /* Register a new Service Server */ + template + bool advertiseService(ServiceServer& srv){ + bool v = advertise(srv.pub); + size_t i = subscribers.size(); + subscribers.push_back(static_cast(&srv)); + srv.id_ = i+100; + return v; + } + + /* Register a new Service Client */ + template + bool serviceClient(ServiceClient& srv){ + bool v = advertise(srv.pub); + size_t i = subscribers.size(); + subscribers.push_back(static_cast(&srv)); + srv.id_ = i+100; + return v; + } + + void negotiateTopics() + { + rosserial_msgs::TopicInfo ti; + size_t i; + for(i = 0; i < publishers.size(); i++) + { + if(publishers[i] != 0) // non-empty slot + { + ti.topic_id = publishers[i]->id_; + ti.topic_name = publishers[i]->topic_; + ti.message_type = publishers[i]->getMsgType(); + ti.md5sum = publishers[i]->getMsgMD5(); + ti.buffer_size = OUTPUT_SIZE; + publish( publishers[i]->getEndpointType(), &ti ); + } + } + for(i = 0; i < subscribers.size(); i++) + { + if(subscribers[i] != 0) // non-empty slot + { + ti.topic_id = subscribers[i]->id_; + ti.topic_name = subscribers[i]->topic_; + ti.message_type = subscribers[i]->getMsgType(); + ti.md5sum = subscribers[i]->getMsgMD5(); + ti.buffer_size = INPUT_SIZE; + publish( subscribers[i]->getEndpointType(), &ti ); + } + } + configured_ = true; + } + + virtual int publish(int id, const Msg * msg) + { + if(id >= 100 && !configured_) + return 0; + + /* serialize message */ + size_t expected_l = msg->serialize_size(); + if( expected_l+9 >= OUTPUT_SIZE ){ + logerror("Message from device dropped: message larger than buffer."); + return -1; + } + + size_t l = msg->serialize(message_out.data()+9); + + /* setup the header */ + message_out[0] = 0xff; + message_out[1] = PROTOCOL_VER; + message_out[2] = (uint8_t) ((uint32_t)l&255); + message_out[3] = (uint8_t) ((uint32_t)l>>8); + message_out[4] = (uint8_t) ((uint32_t)l>>16); + message_out[5] = (uint8_t) ((uint32_t)l>>24); + message_out[6] = 255 - ((message_out[2] + message_out[3] + message_out[4] + message_out[5])%256); + message_out[7] = (uint8_t) ((int16_t)id&255); + message_out[8] = (uint8_t) ((int16_t)id>>8); + + /* calculate checksum */ + int chk = 0; + l += 9; + for(size_t i =7; i end_time) { + logwarn("Failed to get param: timeout expired"); + return false; + } + } + return true; + } + +public: + bool getParam(const char* name, int* param, unsigned int length =1, int timeout = 1000){ + if (requestParam(name, timeout) ){ + if (length == req_param_resp.ints.size()){ + //copy it over + for(size_t i=0; i +#include "rosserial_msgs/TopicInfo.h" +#include "ros/node_handle.h" + +namespace ros { + +class Publisher_ +{ + public: + virtual const std::string& getMsgType() = 0; + virtual const std::string& getMsgMD5() = 0; + virtual int getEndpointType() = 0; + + std::string topic_; + // id_ and no_ are set by NodeHandle when we advertise + int id_; +}; + + /* Generic Publisher */ + template + class Publisher: public Publisher_ + { + public: + Publisher( const std::string & topic_name, int endpoint=rosserial_msgs::TopicInfo::ID_PUBLISHER) : + endpoint_(endpoint) + { + topic_ = topic_name; + } + + int publish( const Msg & msg ) { return nh_->publish(id_, &msg); } + int publish( const Msg * msg ) { return nh_->publish(id_, msg); } + int getEndpointType() override{ return endpoint_; } + + const std::string& getMsgType() override { return MsgT::getType(); } + const std::string& getMsgMD5() override { return MsgT::getMD5(); } + + NodeHandleBase_* nh_; + + private: + int endpoint_; + }; + +} + +#endif diff --git a/src/ros_lib/ros/ros.h b/src/ros_lib/ros/ros.h new file mode 100644 index 0000000..da0e3e8 --- /dev/null +++ b/src/ros_lib/ros/ros.h @@ -0,0 +1,31 @@ +/** +Software License Agreement (BSD) + +\file ros.h +\authors Kareem Shehata +\copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: + * Redistributions of source code must retain the above copyright notice, this list of conditions and the + following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef _ROS_H_ +#define _ROS_H_ + +#include "ros/node_handle.h" + +#endif diff --git a/src/ros_lib/ros/service_client.h b/src/ros_lib/ros/service_client.h new file mode 100644 index 0000000..f247b3c --- /dev/null +++ b/src/ros_lib/ros/service_client.h @@ -0,0 +1,99 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROS_SERVICE_CLIENT_H_ +#define _ROS_SERVICE_CLIENT_H_ + +#include +#include "rosserial_msgs/TopicInfo.h" + +#include "ros/publisher.h" +#include "ros/subscriber.h" + +namespace ros { + + template + class ServiceClient : public Subscriber_ { + + typedef typename MType::Request MReq; + typedef typename MType::Response MRes; + typedef std::function CallBackT; + + public: + ServiceClient(const char* topic_name) : + pub(topic_name, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER) + { + this->topic_ = topic_name; + this->waiting = false; + } + + void call(const MReq & request, CallBackT callback) + { + if(waiting) + { + std::cerr << "Double service call on " << this->topic_ << std::endl; + } + if(!pub.nh_->connected()) return; + waiting = true; + cb_ = callback; + pub.publish(request); + } + + const std::string & getMsgType() override { return MRes::getType(); } + const std::string & getMsgMD5() override { return MRes::getMD5(); } + int getEndpointType() override{ return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } + + private: + // these refer to the subscriber + void callback(unsigned char *data) override{ + MRes ret; + ret.deserialize(data); + waiting = false; + if(cb_) + { + cb_(ret); + } + cb_ = nullptr; + } + + bool waiting; + CallBackT cb_; + + public: + Publisher pub; + }; + +} + +#endif diff --git a/src/ros_lib/ros/service_server.h b/src/ros_lib/ros/service_server.h new file mode 100644 index 0000000..ee0fb3f --- /dev/null +++ b/src/ros_lib/ros/service_server.h @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef _ROS_SERVICE_SERVER_H_ +#define _ROS_SERVICE_SERVER_H_ + +#include "rosserial_msgs/TopicInfo.h" + +#include "ros/publisher.h" +#include "ros/subscriber.h" + +namespace ros { + + template + class ServiceServer : public Subscriber_ { + public: + typedef void(ObjT::*CallbackT)(const MReq&, MRes&); + + ServiceServer(const char* topic_name, CallbackT cb, ObjT* obj) : + pub(topic_name, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER), + obj_(obj) + { + this->topic_ = topic_name; + this->cb_ = cb; + } + + // these refer to the subscriber + virtual void callback(unsigned char *data){ + req.deserialize(data); + (obj_->*cb_)(req,resp); + pub.publish(resp); + } + virtual const char * getMsgType(){ return this->req.getType(); } + virtual const char * getMsgMD5(){ return this->req.getMD5(); } + virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } + + MReq req; + MRes resp; + Publisher pub; + private: + CallbackT cb_; + ObjT* obj_; + }; + + template + class ServiceServer : public Subscriber_ { + public: + typedef void(*CallbackT)(const MReq&, MRes&); + + ServiceServer(const char* topic_name, CallbackT cb) : + pub(topic_name, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER) + { + this->topic_ = topic_name; + this->cb_ = cb; + } + + // these refer to the subscriber + virtual void callback(unsigned char *data){ + req.deserialize(data); + cb_(req,resp); + pub.publish(&resp); + } + virtual const char * getMsgType(){ return this->req.getType(); } + virtual const char * getMsgMD5(){ return this->req.getMD5(); } + virtual int getEndpointType(){ return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } + + MReq req; + MRes resp; + Publisher pub; + private: + CallbackT cb_; + }; + +} + +#endif diff --git a/src/ros_lib/ros/subscriber.h b/src/ros_lib/ros/subscriber.h new file mode 100644 index 0000000..d08ca77 --- /dev/null +++ b/src/ros_lib/ros/subscriber.h @@ -0,0 +1,90 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_SUBSCRIBER_H_ +#define ROS_SUBSCRIBER_H_ + +#include "rosserial_msgs/TopicInfo.h" +#include + +namespace ros { + + /* Base class for objects subscribers. */ + class Subscriber_ + { + public: + virtual void callback(unsigned char *data)=0; + virtual int getEndpointType()=0; + + // id_ is set by NodeHandle when we advertise + int id_; + + virtual const std::string& getMsgType()=0; + virtual const std::string& getMsgMD5()=0; + std::string topic_; + }; + + /* Standalone function subscriber. */ + template + class Subscriber: public Subscriber_ + { + public: + typedef std::function< void(const MsgT&) > CallbackT; + MsgT msg; + + Subscriber(const std::string &topic_name, CallbackT cb, int endpoint=rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : + cb_(cb), + endpoint_(endpoint) + { + topic_ = topic_name; + } + + virtual void callback(unsigned char* data) + { + msg.deserialize(data); + this->cb_(msg); + } + + virtual const std::string& getMsgType() { return MsgT::getType(); } + virtual const std::string& getMsgMD5() { return MsgT::getMD5(); } + virtual int getEndpointType() { return endpoint_; } + + private: + CallbackT cb_; + int endpoint_; + }; + +} + +#endif diff --git a/src/ros_lib/ros/time.h b/src/ros_lib/ros/time.h new file mode 100644 index 0000000..1d7e171 --- /dev/null +++ b/src/ros_lib/ros/time.h @@ -0,0 +1,72 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_TIME_H_ +#define ROS_TIME_H_ + +#include "ros/duration.h" +#include +#include + +namespace ros +{ + void normalizeSecNSec(uint32_t &sec, uint32_t &nsec); + + class Time + { + public: + uint32_t sec, nsec; + + Time() : sec(0), nsec(0) {} + Time(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) + { + normalizeSecNSec(sec, nsec); + } + + double toSec() const { return (double)sec + 1e-9*(double)nsec; } + void fromSec(double t) { sec = (uint32_t) floor(t); nsec = (uint32_t) round((t-sec) * 1e9); } + + uint32_t toNsec() { return (uint32_t)sec*1000000000ull + (uint32_t)nsec; } + Time& fromNSec(uint64_t t); + + Time& operator +=(const Duration &rhs); + Time& operator -=(const Duration &rhs); + + static Time now(); + static void setNow( Time & new_now); + }; + +} + +#endif diff --git a/src/ros_lib/time.cpp b/src/ros_lib/time.cpp new file mode 100644 index 0000000..a62efee --- /dev/null +++ b/src/ros_lib/time.cpp @@ -0,0 +1,68 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "ros/time.h" + +namespace ros +{ + void normalizeSecNSec(uint32_t& sec, uint32_t& nsec){ + uint32_t nsec_part= nsec % 1000000000UL; + uint32_t sec_part = nsec / 1000000000UL; + sec += sec_part; + nsec = nsec_part; + } + + Time& Time::fromNSec(uint64_t t) + { + sec = t / 1000000000; + nsec = t % 1000000000; + normalizeSecNSec(sec, nsec); + return *this; + } + + Time& Time::operator +=(const Duration &rhs) + { + sec += rhs.sec; + nsec += rhs.nsec; + normalizeSecNSec(sec, nsec); + return *this; + } + + Time& Time::operator -=(const Duration &rhs){ + sec += -rhs.sec; + nsec += -rhs.nsec; + normalizeSecNSec(sec, nsec); + return *this; + } +} diff --git a/test/.gitignore b/test/.gitignore new file mode 100644 index 0000000..e2f7bf4 --- /dev/null +++ b/test/.gitignore @@ -0,0 +1,2 @@ +/ros_lib +/build*/ diff --git a/test/MessageReceive/MessageReceive.pro b/test/MessageReceive/MessageReceive.pro new file mode 100644 index 0000000..4b415b1 --- /dev/null +++ b/test/MessageReceive/MessageReceive.pro @@ -0,0 +1,43 @@ +QT += core +QT -= gui +QT += network + +CONFIG += c++11 + +TARGET = MessageReceive +CONFIG += console +CONFIG -= app_bundle + +TEMPLATE = app + +INCLUDEPATH += ../../src/ros_lib +INCLUDEPATH += ../ros_lib + +SOURCES += main.cpp \ + ../../src/ros_lib/duration.cpp \ + ../../src/ros_lib/time.cpp \ + ../../src/ros_lib/RosQtSocket.cpp \ + node.cpp +# The following define makes your compiler emit warnings if you use +# any feature of Qt which as been marked deprecated (the exact warnings +# depend on your compiler). Please consult the documentation of the +# deprecated API in order to know how to port your code away from it. +DEFINES += QT_DEPRECATED_WARNINGS + +# You can also make your code fail to compile if you use deprecated APIs. +# In order to do so, uncomment the following line. +# You can also select to disable deprecated APIs only up to a certain version of Qt. +#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 + +HEADERS += \ + ../../src/ros_lib/ros/duration.h \ + ../../src/ros_lib/ros/msg.h \ + ../../src/ros_lib/ros/node_handle.h \ + ../../src/ros_lib/ros/publisher.h \ + ../../src/ros_lib/ros/ros.h \ + ../../src/ros_lib/ros/service_client.h \ + ../../src/ros_lib/ros/service_server.h \ + ../../src/ros_lib/ros/subscriber.h \ + ../../src/ros_lib/ros/time.h \ + ../../src/ros_lib/ros/RosQtSocket.h \ + node.h diff --git a/test/MessageReceive/MessageReceive.pro.user b/test/MessageReceive/MessageReceive.pro.user new file mode 100644 index 0000000..a22c65e --- /dev/null +++ b/test/MessageReceive/MessageReceive.pro.user @@ -0,0 +1,216 @@ + + + + + + EnvironmentId + {b4f30ced-7693-4a4b-b90f-54df2b2dcced} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + UTF-8 + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 0 + 8 + true + 1 + true + true + true + false + + + + ProjectExplorer.Project.PluginSettings + + + + ProjectExplorer.Project.Target.0 + + Desktop Qt 5.9.1 GCC 64bit + Desktop Qt 5.9.1 GCC 64bit + qt.591.gcc_64_kit + 0 + 0 + 0 + + /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-MessageReceive-Desktop_Qt_5_9_1_GCC_64bit-Debug + + + true + qmake + + QtProjectManager.QMakeBuildStep + true + + false + false + false + + + true + Make + + Qt4ProjectManager.MakeStep + + -w + -r + + false + + + + 2 + Build + + ProjectExplorer.BuildSteps.Build + + + + true + Make + + Qt4ProjectManager.MakeStep + + -w + -r + + true + clean + + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Debug + + Qt4ProjectManager.Qt4BuildConfiguration + 2 + true + + 1 + + + 0 + Deploy + + ProjectExplorer.BuildSteps.Deploy + + 1 + Deploy locally + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + 2 + + MessageReceive + + Qt4ProjectManager.Qt4RunConfiguration:/home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/MessageReceive/MessageReceive.pro + true + + MessageReceive.pro + false + + /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-MessageReceive-Desktop_Qt_5_9_1_GCC_64bit-Debug + 3768 + false + true + false + false + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 18 + + + Version + 18 + + diff --git a/test/MessageReceive/main.cpp b/test/MessageReceive/main.cpp new file mode 100644 index 0000000..8a8189c --- /dev/null +++ b/test/MessageReceive/main.cpp @@ -0,0 +1,37 @@ +/** +Software License Agreement (BSD) + +\file messagereceive.cpp +\authors Robin VANHOVE +\copyright Copyright (c) 2017, Robopec, All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: + * Redistributions of source code must retain the above copyright notice, this list of conditions and the + following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "ros/ros.h" +#include +#include "node.h" + +using std::string; + +int main(int argc, char *argv[]) +{ + QCoreApplication app(argc, argv); + Node node; + return app.exec(); +} diff --git a/test/MessageReceive/node.cpp b/test/MessageReceive/node.cpp new file mode 100644 index 0000000..f59439b --- /dev/null +++ b/test/MessageReceive/node.cpp @@ -0,0 +1,46 @@ +#include "node.h" +#include + +void estimated_pose_callback (const geometry_msgs::PoseWithCovarianceStamped & pose) +{ + printf ("Received pose %f, %f, %f\n", pose.pose.pose.position.x, + pose.pose.pose.position.y, pose.pose.pose.position.z); + + std::cout << "pose : " << QJsonDocument(pose.serializeAsJson()).toJson(QJsonDocument::Indented).toStdString() << std::endl; +} + +Node::Node(): + chatter("chatter"), + poseSub("estimated_pose", &estimated_pose_callback), + serviceClient("/rosout/get_loggers") +{ + std::string ros_master = "127.0.0.1"; + + printf ("Connecting to server at %s\n", ros_master.c_str()); + nh.initNode (ros_master.c_str()); + + nh.subscribe (poseSub); + nh.advertise(chatter); + + m_timer = new QTimer(); + connect(m_timer, SIGNAL(timeout()), this, SLOT(onTimer())); + m_timer->start(1000); + + nh.serviceClient(serviceClient); +} + +void Node::onTimer() +{ + static int i=1; + i++; + std_msgs::String str_msg; + str_msg.data = QString("Hello %1").arg(i).toStdString(); + chatter.publish(str_msg); + + roscpp::GetLoggersRequest req; + serviceClient.call(req, [](const roscpp::GetLoggers::Response &loggers){ + std::cout << "loggers : " << QJsonDocument(loggers.serializeAsJson()).toJson(QJsonDocument::Indented).toStdString() << std::endl; + + }); +} + diff --git a/test/MessageReceive/node.h b/test/MessageReceive/node.h new file mode 100644 index 0000000..4e50b3e --- /dev/null +++ b/test/MessageReceive/node.h @@ -0,0 +1,30 @@ +#ifndef NODE_H +#define NODE_H + +#include +#include "ros/ros.h" +#include +#include +#include +#include + +class Node : public QObject +{ + Q_OBJECT + +public: + explicit Node(); + +public slots: + void onTimer(); + +private: + ros::NodeHandle nh; + QTimer *m_timer; + ros::Publisher< std_msgs::String > chatter; + ros::Subscriber < geometry_msgs::PoseWithCovarianceStamped > poseSub; + ros::ServiceClient serviceClient; +}; + + +#endif // NODE_H diff --git a/test/TestDrive/readme.md b/test/TestDrive/readme.md new file mode 100644 index 0000000..7716383 --- /dev/null +++ b/test/TestDrive/readme.md @@ -0,0 +1,24 @@ +rosserial windows TestDrive +=========================== + +This is an example of how to use rosserial_windows to send commands from a +windows app to a robot. All it does is send a cmd_vel Twist to the robot +with a static set of values. This can be updated to change the values based on +joystick or keyboard inputs, or to send other messages. + +For a full tutorial see http://wiki.ros.org/rosserial_windows/Tutorials/Hello%20World + +To use the example: + +1. Create a new visual studio command line app project +2. Copy the ros_lib generated by rosserial_windows into your project +3. Copy the testdrive.cpp instead of your main cpp file +4. Add the .h and .cpp files from the *top level* of the ros_lib to your VS project (should only be WindowsSocket.h WindowsSocket.cpp duration.cpp time.cpp and ros.h) +5. Compile the project +6. Run the exe with the command line argument of the IP address of your robot +7. Watch your robot spin around! + +Some tips: + +* Do not enable unicode support. If your command line arguments are not working correctly, try turning off unicode as the character set. +* Turn off precompiled headers or WindowsSocket will not compile properly. diff --git a/test/TestDrive/testdrive.cpp b/test/TestDrive/testdrive.cpp new file mode 100644 index 0000000..6cb3490 --- /dev/null +++ b/test/TestDrive/testdrive.cpp @@ -0,0 +1,70 @@ +/** +Software License Agreement (BSD) + +\file testdrive.cpp +\authors Kareem Shehata +\copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: + * Redistributions of source code must retain the above copyright notice, this list of conditions and the + following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +// testdrive.cpp : An example of how to use rosserial in Windows +// + +#include "stdafx.h" +#include +#include +#include "ros.h" +#include +#include + +using std::string; + +int main (int argc, char *argv[]) +{ + ros::NodeHandle nh; + if (argc != 2) + { + printf ("Usage: testdrive host[:port]\n"); + return 0; + } + + printf ("Connecting to server at %s\n", argv[1]); + nh.initNode (argv[1]); + + printf ("Advertising cmd_vel message\n"); + geometry_msgs::Twist twist_msg; + ros::Publisher cmd_vel_pub ("cmd_vel", &twist_msg); + nh.advertise (cmd_vel_pub); + + printf ("Go robot go!\n"); + while (1) + { + twist_msg.linear.x = 5.1; + twist_msg.linear.y = 0; + twist_msg.linear.z = 0; + twist_msg.angular.x = 0; + twist_msg.angular.y = 0; + twist_msg.angular.z = -1.8; + cmd_vel_pub.publish (&twist_msg); + + nh.spinOnce (); + Sleep (100); + } + return 0; +}