├── CMakeLists.txt ├── package.xml ├── LICENSE ├── src ├── ros_lib │ ├── ros.h │ ├── tf │ │ ├── tf.h │ │ └── transform_broadcaster.h │ ├── time.cpp │ ├── ros │ │ ├── publisher.h │ │ ├── duration.h │ │ ├── time.h │ │ ├── service_client.h │ │ ├── subscriber.h │ │ ├── service_server.h │ │ ├── msg.h │ │ └── node_handle.h │ ├── duration.cpp │ └── STM32Hardware.h └── rosserial_stm32f7 │ └── make_libraries.py ├── README.md ├── cmake └── rosserial_stm32f7-extras.cmake └── CHANGELOG.rst /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosserial_stm32f7) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package( 6 | CFG_EXTRAS ${PROJECT_NAME}-extras.cmake 7 | ) 8 | 9 | if(CATKIN_ENABLE_TESTING) 10 | find_package(rosserial_msgs REQUIRED) 11 | include_directories(src ${rosserial_msgs_INCLUDE_DIRS}) 12 | endif() 13 | 14 | install( 15 | DIRECTORY src/ros_lib 16 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 17 | ) 18 | 19 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | rosserial_stm32f7 3 | 0.8.0 4 | 5 | STM32F7 client side source for rosserial. 6 | 7 | Michael Ferguson 8 | Adam Stambler 9 | Federica Di Lauro 10 | Federica Di Lauro 11 | BSD 12 | http://ros.org/wiki/rosserial_client 13 | 14 | catkin 15 | rosbash 16 | 17 | rosserial_msgs 18 | std_msgs 19 | rospy 20 | tf 21 | 22 | rosunit 23 | rosserial_msgs 24 | 25 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2018, Kenta Yonekura 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /src/ros_lib/ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Kenta Yonekura (a.k.a. yoneken) 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_H_ 36 | #define _ROS_H_ 37 | 38 | #include "ros/node_handle.h" 39 | #include "STM32Hardware.h" 40 | 41 | namespace ros 42 | { 43 | typedef NodeHandle_ NodeHandle; // default 25, 25, 512, 512 44 | } 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rosserial_stm32f7 2 | 3 | # DISCLAIMER: 4 | ## THIS IMPLEMENTATION CURRENTLY WORKS ONLY WITH STDS MESSAGES AND ISN'T MAINTAINED 5 | ### IF YOU DO MANAGE TO MAKE IT WORK WITH EVERY MESSAGE A PULL REQUEST WOULD BE NICE 6 | 7 | 8 | Rosserial implementation for STM32F767, developed to work with STM32CubeIDE projects 9 | 10 | Heavily based on [yoneken's rosserial_stm32](https://github.com/yoneken/rosserial_stm32) 11 | 12 | ## Generate code 13 | ``` 14 | $ cd path/to/your/project/Core 15 | 16 | $ rosrun rosserial_stm32f7 make_libraries.py . 17 | ``` 18 | 19 | Note that older versions of STM32CubeIde do not have a "Core" folder, instead "Src" and "Inc" folders are inside the root of your project. 20 | As a general rule you should cd into the folder which has "Src" and "Inc" inside. 21 | 22 | ## Usage 23 | 1. Create a new STM32CubeIDE project: 24 | * Enable USARTx and select TX and RX pins 25 | * Enable USARTx global interrupt 26 | * Enable DMA for USARTx_TX and USARTx_RX and set their priority to HIGH 27 | 28 | 2. Create ros libraries in your project 29 | ``` 30 | cd your/catkin/workspace/src 31 | ``` 32 | ``` 33 | git clone https://github.com/fdila/rosserial_stm32f7 34 | ``` 35 | ``` 36 | cd .. 37 | ``` 38 | ``` 39 | catkin_make 40 | ``` 41 | ``` 42 | source devel/setup.bash 43 | ``` 44 | ``` 45 | cd path/to/your/stm32_project 46 | ``` 47 | ``` 48 | rosrun rosserial_stm32f7 make_libraries.py . 49 | ``` 50 | 3. Set header files to be compiled with g++ 51 | * Right click on your project -> Properties 52 | * C/C++ generals 53 | * File Types 54 | * Use project settings 55 | * New.. 56 | * Pattern: *.h Type: C++ Header File 57 | 58 | 4. Convert your stm32 project to C++ 59 | * Right click on your project inside the IDE -> Convert to C++ 60 | * Rename main.c to main.cpp 61 | 62 | 5. On your computer start roscore and rosserial_server: 63 | ``` 64 | roscore 65 | ``` 66 | ``` 67 | rosrun rosserial_server serial_node _port:=/dev/ttyYourBoard _baud:=YourBaudRate 68 | ``` 69 | 70 | 71 | **If you have to modify the initial configuration through CubeMX:** 72 | * Rename main.cpp to main.c 73 | * Do your changes 74 | * Repeat step 4 75 | -------------------------------------------------------------------------------- /src/ros_lib/tf/tf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_TF_H_ 36 | #define ROS_TF_H_ 37 | 38 | #include "geometry_msgs/TransformStamped.h" 39 | 40 | namespace tf 41 | { 42 | 43 | static inline geometry_msgs::Quaternion createQuaternionFromYaw(double yaw) 44 | { 45 | geometry_msgs::Quaternion q; 46 | q.x = 0; 47 | q.y = 0; 48 | q.z = sin(yaw * 0.5); 49 | q.w = cos(yaw * 0.5); 50 | return q; 51 | } 52 | 53 | } 54 | 55 | #endif 56 | 57 | -------------------------------------------------------------------------------- /src/ros_lib/tf/transform_broadcaster.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_TRANSFORM_BROADCASTER_H_ 36 | #define ROS_TRANSFORM_BROADCASTER_H_ 37 | 38 | #include "ros.h" 39 | #include "tfMessage.h" 40 | 41 | namespace tf 42 | { 43 | 44 | class TransformBroadcaster 45 | { 46 | public: 47 | TransformBroadcaster() : publisher_("/tf", &internal_msg) {} 48 | 49 | void init(ros::NodeHandle &nh) 50 | { 51 | nh.advertise(publisher_); 52 | } 53 | 54 | void sendTransform(geometry_msgs::TransformStamped &transform) 55 | { 56 | internal_msg.transforms_length = 1; 57 | internal_msg.transforms = &transform; 58 | publisher_.publish(&internal_msg); 59 | } 60 | 61 | private: 62 | tf::tfMessage internal_msg; 63 | ros::Publisher publisher_; 64 | }; 65 | 66 | } 67 | 68 | #endif 69 | 70 | -------------------------------------------------------------------------------- /src/ros_lib/time.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #include "ros/time.h" 36 | 37 | namespace ros 38 | { 39 | void normalizeSecNSec(uint32_t& sec, uint32_t& nsec) 40 | { 41 | uint32_t nsec_part = nsec % 1000000000UL; 42 | uint32_t sec_part = nsec / 1000000000UL; 43 | sec += sec_part; 44 | nsec = nsec_part; 45 | } 46 | 47 | Time& Time::fromNSec(int32_t t) 48 | { 49 | sec = t / 1000000000; 50 | nsec = t % 1000000000; 51 | normalizeSecNSec(sec, nsec); 52 | return *this; 53 | } 54 | 55 | Time& Time::operator +=(const Duration &rhs) 56 | { 57 | sec += rhs.sec; 58 | nsec += rhs.nsec; 59 | normalizeSecNSec(sec, nsec); 60 | return *this; 61 | } 62 | 63 | Time& Time::operator -=(const Duration &rhs) 64 | { 65 | sec += -rhs.sec; 66 | nsec += -rhs.nsec; 67 | normalizeSecNSec(sec, nsec); 68 | return *this; 69 | } 70 | } 71 | -------------------------------------------------------------------------------- /src/ros_lib/ros/publisher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_PUBLISHER_H_ 36 | #define _ROS_PUBLISHER_H_ 37 | 38 | #include "rosserial_msgs/TopicInfo.h" 39 | #include "ros/node_handle.h" 40 | 41 | namespace ros 42 | { 43 | 44 | /* Generic Publisher */ 45 | class Publisher 46 | { 47 | public: 48 | Publisher(const char * topic_name, Msg * msg, int endpoint = rosserial_msgs::TopicInfo::ID_PUBLISHER) : 49 | topic_(topic_name), 50 | msg_(msg), 51 | endpoint_(endpoint) {}; 52 | 53 | int publish(const Msg * msg) 54 | { 55 | return nh_->publish(id_, msg); 56 | }; 57 | int getEndpointType() 58 | { 59 | return endpoint_; 60 | } 61 | 62 | const char * topic_; 63 | Msg *msg_; 64 | // id_ and no_ are set by NodeHandle when we advertise 65 | int id_; 66 | NodeHandleBase_* nh_; 67 | 68 | private: 69 | int endpoint_; 70 | }; 71 | 72 | } 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /src/ros_lib/ros/duration.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_DURATION_H_ 36 | #define _ROS_DURATION_H_ 37 | 38 | #include 39 | #include 40 | 41 | namespace ros 42 | { 43 | 44 | void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec); 45 | 46 | class Duration 47 | { 48 | public: 49 | int32_t sec, nsec; 50 | 51 | Duration() : sec(0), nsec(0) {} 52 | Duration(int32_t _sec, int32_t _nsec) : sec(_sec), nsec(_nsec) 53 | { 54 | normalizeSecNSecSigned(sec, nsec); 55 | } 56 | 57 | double toSec() const 58 | { 59 | return (double)sec + 1e-9 * (double)nsec; 60 | }; 61 | void fromSec(double t) 62 | { 63 | sec = (uint32_t) floor(t); 64 | nsec = (uint32_t) round((t - sec) * 1e9); 65 | }; 66 | 67 | Duration& operator+=(const Duration &rhs); 68 | Duration& operator-=(const Duration &rhs); 69 | Duration& operator*=(double scale); 70 | }; 71 | 72 | } 73 | 74 | #endif 75 | 76 | -------------------------------------------------------------------------------- /src/ros_lib/ros/time.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_TIME_H_ 36 | #define ROS_TIME_H_ 37 | 38 | #include "ros/duration.h" 39 | #include 40 | #include 41 | 42 | namespace ros 43 | { 44 | void normalizeSecNSec(uint32_t &sec, uint32_t &nsec); 45 | 46 | class Time 47 | { 48 | public: 49 | uint32_t sec, nsec; 50 | 51 | Time() : sec(0), nsec(0) {} 52 | Time(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) 53 | { 54 | normalizeSecNSec(sec, nsec); 55 | } 56 | 57 | double toSec() const 58 | { 59 | return (double)sec + 1e-9 * (double)nsec; 60 | }; 61 | void fromSec(double t) 62 | { 63 | sec = (uint32_t) floor(t); 64 | nsec = (uint32_t) round((t - sec) * 1e9); 65 | }; 66 | 67 | uint32_t toNsec() 68 | { 69 | return (uint32_t)sec * 1000000000ull + (uint32_t)nsec; 70 | }; 71 | Time& fromNSec(int32_t t); 72 | 73 | Time& operator +=(const Duration &rhs); 74 | Time& operator -=(const Duration &rhs); 75 | 76 | static Time now(); 77 | static void setNow(Time & new_now); 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /src/ros_lib/duration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #include 36 | #include "ros/duration.h" 37 | 38 | namespace ros 39 | { 40 | void normalizeSecNSecSigned(int32_t &sec, int32_t &nsec) 41 | { 42 | int32_t nsec_part = nsec; 43 | int32_t sec_part = sec; 44 | 45 | while (nsec_part > 1000000000L) 46 | { 47 | nsec_part -= 1000000000L; 48 | ++sec_part; 49 | } 50 | while (nsec_part < 0) 51 | { 52 | nsec_part += 1000000000L; 53 | --sec_part; 54 | } 55 | sec = sec_part; 56 | nsec = nsec_part; 57 | } 58 | 59 | Duration& Duration::operator+=(const Duration &rhs) 60 | { 61 | sec += rhs.sec; 62 | nsec += rhs.nsec; 63 | normalizeSecNSecSigned(sec, nsec); 64 | return *this; 65 | } 66 | 67 | Duration& Duration::operator-=(const Duration &rhs) 68 | { 69 | sec += -rhs.sec; 70 | nsec += -rhs.nsec; 71 | normalizeSecNSecSigned(sec, nsec); 72 | return *this; 73 | } 74 | 75 | Duration& Duration::operator*=(double scale) 76 | { 77 | sec *= scale; 78 | nsec *= scale; 79 | normalizeSecNSecSigned(sec, nsec); 80 | return *this; 81 | } 82 | 83 | } 84 | -------------------------------------------------------------------------------- /src/ros_lib/ros/service_client.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_SERVICE_CLIENT_H_ 36 | #define _ROS_SERVICE_CLIENT_H_ 37 | 38 | #include "rosserial_msgs/TopicInfo.h" 39 | 40 | #include "ros/publisher.h" 41 | #include "ros/subscriber.h" 42 | 43 | namespace ros 44 | { 45 | 46 | template 47 | class ServiceClient : public Subscriber_ 48 | { 49 | public: 50 | ServiceClient(const char* topic_name) : 51 | pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER) 52 | { 53 | this->topic_ = topic_name; 54 | this->waiting = true; 55 | } 56 | 57 | virtual void call(const MReq & request, MRes & response) 58 | { 59 | if (!pub.nh_->connected()) return; 60 | ret = &response; 61 | waiting = true; 62 | pub.publish(&request); 63 | while (waiting && pub.nh_->connected()) 64 | if (pub.nh_->spinOnce() < 0) break; 65 | } 66 | 67 | // these refer to the subscriber 68 | virtual void callback(unsigned char *data) 69 | { 70 | ret->deserialize(data); 71 | waiting = false; 72 | } 73 | virtual const char * getMsgType() 74 | { 75 | return this->resp.getType(); 76 | } 77 | virtual const char * getMsgMD5() 78 | { 79 | return this->resp.getMD5(); 80 | } 81 | virtual int getEndpointType() 82 | { 83 | return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; 84 | } 85 | 86 | MReq req; 87 | MRes resp; 88 | MRes * ret; 89 | bool waiting; 90 | Publisher pub; 91 | }; 92 | 93 | } 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /cmake/rosserial_stm32f7-extras.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | # 4 | # Generate a rosserial_stm32f7 ros_lib folder using the make_libraries 5 | # script supplied by a particular package. The default is to fall back 6 | # on the generic script in rosserial_stm32f7 itself. 7 | # 8 | # :param PACKAGE: name of package to look in for lib generating script. 9 | # :type PACKAGE: string 10 | # :param SCRIPT: name of script, for example `make_libraries.py`. 11 | # :type SCRIPT: string 12 | # 13 | # @public 14 | # 15 | function(rosserial_generate_ros_lib) 16 | cmake_parse_arguments(make_libraries "" "PACKAGE;SCRIPT" "" ${ARGN}) 17 | if(NOT make_libraries_PACKAGE) 18 | set(make_libraries_PACKAGE rosserial_stm32f7) 19 | endif() 20 | if(NOT make_libraries_SCRIPT) 21 | set(make_libraries_SCRIPT make_libraries) 22 | endif() 23 | 24 | message(STATUS "Using ${make_libraries_PACKAGE}/${make_libraries_SCRIPT} to make rosserial client library.") 25 | 26 | add_custom_command( 27 | OUTPUT ${PROJECT_BINARY_DIR}/ros_lib 28 | COMMAND ${CATKIN_ENV} rosrun ${make_libraries_PACKAGE} ${make_libraries_SCRIPT} ${PROJECT_BINARY_DIR} 29 | ) 30 | add_custom_target(${PROJECT_NAME}_ros_lib DEPENDS ${PROJECT_BINARY_DIR}/ros_lib) 31 | 32 | find_package(rosserial_msgs) 33 | find_package(std_msgs) 34 | add_dependencies(${PROJECT_NAME}_ros_lib 35 | rosserial_msgs_generate_messages_py 36 | std_msgs_generate_messages_py 37 | ) 38 | 39 | set(${PROJECT_NAME}_ROS_LIB_DIR "${PROJECT_BINARY_DIR}/ros_lib" PARENT_SCOPE) 40 | endfunction() 41 | 42 | # 43 | # Configure a CMake project located in a subfolder of a catkin project, 44 | # optionally specifying a CMake toolchain to use in the build of the 45 | # subproject. 46 | # 47 | # :param DIRECTORY: subdirectory of current package to configure. 48 | # :type DIRECTORY: string 49 | # :param TOOLCHAIN_FILE: full path to toolchain file. 50 | # :type TOOLCHAIN_FILE: string 51 | # 52 | # @public 53 | # 54 | function(rosserial_configure_client) 55 | cmake_parse_arguments(client "" "DIRECTORY;TOOLCHAIN_FILE" "CMAKE_ARGUMENTS" ${ARGN}) 56 | if(NOT client_DIRECTORY) 57 | message(SEND_ERROR "rosserial_stm32f7_add_client called without DIRECTORY argument.") 58 | endif() 59 | 60 | if(client_TOOLCHAIN_FILE) 61 | set(DTOOLCHAIN_FILE -DCMAKE_TOOLCHAIN_FILE=${client_TOOLCHAIN_FILE}) 62 | endif() 63 | 64 | # Create a build tree directory for configuring the client's CMake project. 65 | file(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/${client_DIRECTORY}) 66 | add_custom_target(${PROJECT_NAME}_${client_DIRECTORY} 67 | WORKING_DIRECTORY ${PROJECT_BINARY_DIR}/${client_DIRECTORY} 68 | COMMAND ${CMAKE_COMMAND} ${PROJECT_SOURCE_DIR}/${client_DIRECTORY} 69 | -DROS_LIB_DIR=${${PROJECT_NAME}_ROS_LIB_DIR} 70 | -DEXECUTABLE_OUTPUT_PATH=${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} 71 | ${client_CMAKE_ARGUMENTS} 72 | ${DTOOLCHAIN_FILE} 73 | ) 74 | add_dependencies(${PROJECT_NAME}_${client_DIRECTORY} ${PROJECT_NAME}_ros_lib) 75 | endfunction() 76 | 77 | # 78 | # Create a catkin target which builds a target in the subproject. 79 | # 80 | # :param client_directory: subdirectory of current package with subproject. 81 | # :type client_directory: string 82 | # :param client_target: name of target in subproject to build. 83 | # :type client_target: string 84 | # :param ARGN: additional arguments for target (eg, ALL). 85 | # :type ARGN: list of strings 86 | # 87 | # @public 88 | # 89 | function(rosserial_add_client_target client_directory client_target) 90 | add_custom_target(${PROJECT_NAME}_${client_directory}_${client_target} ${ARGN} 91 | WORKING_DIRECTORY ${PROJECT_BINARY_DIR}/${client_directory} 92 | COMMAND ${CMAKE_COMMAND} --build ${PROJECT_BINARY_DIR}/${client_directory} -- ${client_target} 93 | ) 94 | add_dependencies(${PROJECT_NAME}_${client_directory}_${client_target} 95 | ${PROJECT_NAME}_${client_directory}) 96 | endfunction() 97 | -------------------------------------------------------------------------------- /src/ros_lib/STM32Hardware.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Kenta Yonekura (a.k.a. yoneken) 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_STM32_HARDWARE_H_ 36 | #define ROS_STM32_HARDWARE_H_ 37 | 38 | #include "stm32f7xx_hal.h" 39 | #include "stm32f7xx_hal_uart.h" 40 | 41 | extern UART_HandleTypeDef huart3; 42 | 43 | class STM32Hardware { 44 | protected: 45 | UART_HandleTypeDef *huart; 46 | 47 | const static uint16_t rbuflen = 128; 48 | uint8_t rbuf[rbuflen]; 49 | uint32_t rind; 50 | inline uint32_t getRdmaInd(void){ return (rbuflen - huart->hdmarx->Instance->NDTR) & (rbuflen - 1); } 51 | 52 | const static uint16_t tbuflen = 256; 53 | uint8_t tbuf[tbuflen]; 54 | uint32_t twind, tfind; 55 | 56 | public: 57 | STM32Hardware(): 58 | huart(&huart3), rind(0), twind(0), tfind(0){ 59 | } 60 | 61 | STM32Hardware(UART_HandleTypeDef *huart_): 62 | huart(huart_), rind(0), twind(0), tfind(0){ 63 | } 64 | 65 | void init(){ 66 | reset_rbuf(); 67 | } 68 | 69 | void reset_rbuf(void){ 70 | HAL_UART_Receive_DMA(huart, rbuf, rbuflen); 71 | } 72 | 73 | int read(){ 74 | int c = -1; 75 | if(rind != getRdmaInd()){ 76 | c = rbuf[rind++]; 77 | rind &= rbuflen - 1; 78 | } 79 | return c; 80 | } 81 | 82 | void flush(void){ 83 | static bool mutex = false; 84 | 85 | if((huart->gState == HAL_UART_STATE_READY) && !mutex){ 86 | mutex = true; 87 | 88 | if(twind != tfind){ 89 | uint16_t len = tfind < twind ? twind - tfind : tbuflen - tfind; 90 | HAL_UART_Transmit_DMA(huart, &(tbuf[tfind]), len); 91 | tfind = (tfind + len) & (tbuflen - 1); 92 | } 93 | mutex = false; 94 | } 95 | } 96 | 97 | void write(uint8_t* data, int length){ 98 | 99 | 100 | int n = length; 101 | n = n <= tbuflen ? n : tbuflen; 102 | 103 | int n_tail = n <= tbuflen - twind ? n : tbuflen - twind; 104 | memcpy(&(tbuf[twind]), data, n_tail); 105 | twind = (twind + n) & (tbuflen - 1); 106 | 107 | if(n != n_tail){ 108 | memcpy(tbuf, &(data[n_tail]), n - n_tail); 109 | } 110 | 111 | flush(); 112 | } 113 | 114 | unsigned long time(){ return HAL_GetTick(); } 115 | 116 | protected: 117 | }; 118 | 119 | #endif 120 | 121 | -------------------------------------------------------------------------------- /src/ros_lib/ros/subscriber.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_SUBSCRIBER_H_ 36 | #define ROS_SUBSCRIBER_H_ 37 | 38 | #include "rosserial_msgs/TopicInfo.h" 39 | 40 | namespace ros 41 | { 42 | 43 | /* Base class for objects subscribers. */ 44 | class Subscriber_ 45 | { 46 | public: 47 | virtual void callback(unsigned char *data) = 0; 48 | virtual int getEndpointType() = 0; 49 | 50 | // id_ is set by NodeHandle when we advertise 51 | int id_; 52 | 53 | virtual const char * getMsgType() = 0; 54 | virtual const char * getMsgMD5() = 0; 55 | const char * topic_; 56 | }; 57 | 58 | /* Bound function subscriber. */ 59 | template 60 | class Subscriber: public Subscriber_ 61 | { 62 | public: 63 | typedef void(ObjT::*CallbackT)(const MsgT&); 64 | MsgT msg; 65 | 66 | Subscriber(const char * topic_name, CallbackT cb, ObjT* obj, int endpoint = rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : 67 | cb_(cb), 68 | obj_(obj), 69 | endpoint_(endpoint) 70 | { 71 | topic_ = topic_name; 72 | }; 73 | 74 | virtual void callback(unsigned char* data) 75 | { 76 | msg.deserialize(data); 77 | (obj_->*cb_)(msg); 78 | } 79 | 80 | virtual const char * getMsgType() 81 | { 82 | return this->msg.getType(); 83 | } 84 | virtual const char * getMsgMD5() 85 | { 86 | return this->msg.getMD5(); 87 | } 88 | virtual int getEndpointType() 89 | { 90 | return endpoint_; 91 | } 92 | 93 | private: 94 | CallbackT cb_; 95 | ObjT* obj_; 96 | int endpoint_; 97 | }; 98 | 99 | /* Standalone function subscriber. */ 100 | template 101 | class Subscriber: public Subscriber_ 102 | { 103 | public: 104 | typedef void(*CallbackT)(const MsgT&); 105 | MsgT msg; 106 | 107 | Subscriber(const char * topic_name, CallbackT cb, int endpoint = rosserial_msgs::TopicInfo::ID_SUBSCRIBER) : 108 | cb_(cb), 109 | endpoint_(endpoint) 110 | { 111 | topic_ = topic_name; 112 | }; 113 | 114 | virtual void callback(unsigned char* data) 115 | { 116 | msg.deserialize(data); 117 | this->cb_(msg); 118 | } 119 | 120 | virtual const char * getMsgType() 121 | { 122 | return this->msg.getType(); 123 | } 124 | virtual const char * getMsgMD5() 125 | { 126 | return this->msg.getMD5(); 127 | } 128 | virtual int getEndpointType() 129 | { 130 | return endpoint_; 131 | } 132 | 133 | private: 134 | CallbackT cb_; 135 | int endpoint_; 136 | }; 137 | 138 | } 139 | 140 | #endif 141 | -------------------------------------------------------------------------------- /src/ros_lib/ros/service_server.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_SERVICE_SERVER_H_ 36 | #define _ROS_SERVICE_SERVER_H_ 37 | 38 | #include "rosserial_msgs/TopicInfo.h" 39 | 40 | #include "ros/publisher.h" 41 | #include "ros/subscriber.h" 42 | 43 | namespace ros 44 | { 45 | 46 | template 47 | class ServiceServer : public Subscriber_ 48 | { 49 | public: 50 | typedef void(ObjT::*CallbackT)(const MReq&, MRes&); 51 | 52 | ServiceServer(const char* topic_name, CallbackT cb, ObjT* obj) : 53 | pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER), 54 | obj_(obj) 55 | { 56 | this->topic_ = topic_name; 57 | this->cb_ = cb; 58 | } 59 | 60 | // these refer to the subscriber 61 | virtual void callback(unsigned char *data) 62 | { 63 | req.deserialize(data); 64 | (obj_->*cb_)(req, resp); 65 | pub.publish(&resp); 66 | } 67 | virtual const char * getMsgType() 68 | { 69 | return this->req.getType(); 70 | } 71 | virtual const char * getMsgMD5() 72 | { 73 | return this->req.getMD5(); 74 | } 75 | virtual int getEndpointType() 76 | { 77 | return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; 78 | } 79 | 80 | MReq req; 81 | MRes resp; 82 | Publisher pub; 83 | private: 84 | CallbackT cb_; 85 | ObjT* obj_; 86 | }; 87 | 88 | template 89 | class ServiceServer : public Subscriber_ 90 | { 91 | public: 92 | typedef void(*CallbackT)(const MReq&, MRes&); 93 | 94 | ServiceServer(const char* topic_name, CallbackT cb) : 95 | pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER) 96 | { 97 | this->topic_ = topic_name; 98 | this->cb_ = cb; 99 | } 100 | 101 | // these refer to the subscriber 102 | virtual void callback(unsigned char *data) 103 | { 104 | req.deserialize(data); 105 | cb_(req, resp); 106 | pub.publish(&resp); 107 | } 108 | virtual const char * getMsgType() 109 | { 110 | return this->req.getType(); 111 | } 112 | virtual const char * getMsgMD5() 113 | { 114 | return this->req.getMD5(); 115 | } 116 | virtual int getEndpointType() 117 | { 118 | return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; 119 | } 120 | 121 | MReq req; 122 | MRes resp; 123 | Publisher pub; 124 | private: 125 | CallbackT cb_; 126 | }; 127 | 128 | } 129 | 130 | #endif 131 | -------------------------------------------------------------------------------- /src/rosserial_stm32f7/make_libraries.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ##################################################################### 4 | # Software License Agreement (BSD License) 5 | # 6 | # Copyright (c) 2018, Kenta Yonekura (a.k.a. yoneken), Federica Di Lauro (a.k.a fdila) 7 | # All rights reserved. 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions 11 | # are met: 12 | # 13 | # * Redistributions of source code must retain the above copyright 14 | # notice, this list of conditions and the following disclaimer. 15 | # * Redistributions in binary form must reproduce the above 16 | # copyright notice, this list of conditions and the following 17 | # disclaimer in the documentation and/or other materials provided 18 | # with the distribution. 19 | # * Neither the name of Willow Garage, Inc. nor the names of its 20 | # contributors may be used to endorse or promote products derived 21 | # from this software without specific prior written permission. 22 | # 23 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | # POSSIBILITY OF SUCH DAMAGE. 35 | 36 | THIS_PACKAGE = "rosserial_stm32f7" 37 | 38 | __usage__ = """ 39 | make_libraries.py generates the STM32 rosserial library files for STM32CubeIDE. 40 | It requires the location of your STM32CubeIDE project folder. 41 | 42 | rosrun rosserial_stm32f7 make_libraries.py 43 | """ 44 | 45 | import rospkg 46 | import rosserial_client 47 | from rosserial_client.make_library import * 48 | 49 | # for copying files 50 | import shutil 51 | import os 52 | 53 | ROS_TO_EMBEDDED_TYPES = { 54 | 'bool' : ('bool', 1, PrimitiveDataType, []), 55 | 'byte' : ('int8_t', 1, PrimitiveDataType, []), 56 | 'int8' : ('int8_t', 1, PrimitiveDataType, []), 57 | 'char' : ('uint8_t', 1, PrimitiveDataType, []), 58 | 'uint8' : ('uint8_t', 1, PrimitiveDataType, []), 59 | 'int16' : ('int16_t', 2, PrimitiveDataType, []), 60 | 'uint16' : ('uint16_t', 2, PrimitiveDataType, []), 61 | 'int32' : ('int32_t', 4, PrimitiveDataType, []), 62 | 'uint32' : ('uint32_t', 4, PrimitiveDataType, []), 63 | 'int64' : ('int64_t', 8, PrimitiveDataType, []), 64 | 'uint64' : ('uint64_t', 8, PrimitiveDataType, []), 65 | 'float32' : ('float', 4, PrimitiveDataType, []), 66 | 'float64' : ('float', 4, AVR_Float64DataType, []), 67 | 'time' : ('ros::Time', 8, TimeDataType, ['ros/time']), 68 | 'duration': ('ros::Duration', 8, TimeDataType, ['ros/duration']), 69 | 'string' : ('char*', 0, StringDataType, []), 70 | 'Header' : ('std_msgs::Header', 0, MessageDataType, ['std_msgs/Header']) 71 | } 72 | 73 | # need correct inputs 74 | if (len(sys.argv) < 2): 75 | print __usage__ 76 | exit() 77 | 78 | # get output path 79 | path = sys.argv[1] 80 | if path[-1] == "/": 81 | path = path[0:-1] 82 | print "\nExporting to %s" % path 83 | 84 | rospack = rospkg.RosPack() 85 | 86 | # copy ros_lib stuff in 87 | if os.path.exists(path+"/Inc/ros/"): 88 | shutil.rmtree(path+"/Inc/ros/") 89 | if os.path.exists(path+"/Inc/tf/"): 90 | shutil.rmtree(path+"/Inc/tf/") 91 | 92 | 93 | rosserial_stm32f7_dir = rospack.get_path(THIS_PACKAGE) 94 | files = os.listdir(rosserial_stm32f7_dir+"/src/ros_lib") 95 | for f in files: 96 | if os.path.isfile(rosserial_stm32f7_dir+"/src/ros_lib/"+f): 97 | shutil.copy(rosserial_stm32f7_dir+"/src/ros_lib/"+f, path+"/Inc/") 98 | rosserial_client_copy_files(rospack, path+"/Inc/") 99 | 100 | # generate messages 101 | rosserial_generate(rospack, path+"/Inc/", ROS_TO_EMBEDDED_TYPES) 102 | 103 | 104 | sourcefiles = os.listdir(path+"/Inc/") 105 | 106 | for file in sourcefiles: 107 | if file.endswith('.cpp'): 108 | shutil.move(os.path.join(path+"/Inc/",file), os.path.join(path+"/Src/",file)) 109 | 110 | -------------------------------------------------------------------------------- /src/ros_lib/ros/msg.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef _ROS_MSG_H_ 36 | #define _ROS_MSG_H_ 37 | 38 | #include 39 | #include 40 | 41 | namespace ros 42 | { 43 | 44 | /* Base Message Type */ 45 | class Msg 46 | { 47 | public: 48 | virtual int serialize(unsigned char *outbuffer) const = 0; 49 | virtual int deserialize(unsigned char *data) = 0; 50 | virtual const char * getType() = 0; 51 | virtual const char * getMD5() = 0; 52 | 53 | /** 54 | * @brief This tricky function handles promoting a 32bit float to a 64bit 55 | * double, so that AVR can publish messages containing float64 56 | * fields, despite AVV having no native support for double. 57 | * 58 | * @param[out] outbuffer pointer for buffer to serialize to. 59 | * @param[in] f value to serialize. 60 | * 61 | * @return number of bytes to advance the buffer pointer. 62 | * 63 | */ 64 | static int serializeAvrFloat64(unsigned char* outbuffer, const float f) 65 | { 66 | const int32_t* val = (int32_t*) &f; 67 | int32_t exp = ((*val >> 23) & 255); 68 | if (exp != 0) 69 | { 70 | exp += 1023 - 127; 71 | } 72 | 73 | int32_t sig = *val; 74 | *(outbuffer++) = 0; 75 | *(outbuffer++) = 0; 76 | *(outbuffer++) = 0; 77 | *(outbuffer++) = (sig << 5) & 0xff; 78 | *(outbuffer++) = (sig >> 3) & 0xff; 79 | *(outbuffer++) = (sig >> 11) & 0xff; 80 | *(outbuffer++) = ((exp << 4) & 0xF0) | ((sig >> 19) & 0x0F); 81 | *(outbuffer++) = (exp >> 4) & 0x7F; 82 | 83 | // Mark negative bit as necessary. 84 | if (f < 0) 85 | { 86 | *(outbuffer - 1) |= 0x80; 87 | } 88 | 89 | return 8; 90 | } 91 | 92 | /** 93 | * @brief This tricky function handles demoting a 64bit double to a 94 | * 32bit float, so that AVR can understand messages containing 95 | * float64 fields, despite AVR having no native support for double. 96 | * 97 | * @param[in] inbuffer pointer for buffer to deserialize from. 98 | * @param[out] f pointer to place the deserialized value in. 99 | * 100 | * @return number of bytes to advance the buffer pointer. 101 | */ 102 | static int deserializeAvrFloat64(const unsigned char* inbuffer, float* f) 103 | { 104 | uint32_t* val = (uint32_t*)f; 105 | inbuffer += 3; 106 | 107 | // Copy truncated mantissa. 108 | *val = ((uint32_t)(*(inbuffer++)) >> 5 & 0x07); 109 | *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 3; 110 | *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 11; 111 | *val |= ((uint32_t)(*inbuffer) & 0x0f) << 19; 112 | 113 | // Copy truncated exponent. 114 | uint32_t exp = ((uint32_t)(*(inbuffer++)) & 0xf0) >> 4; 115 | exp |= ((uint32_t)(*inbuffer) & 0x7f) << 4; 116 | if (exp != 0) 117 | { 118 | *val |= ((exp) - 1023 + 127) << 23; 119 | } 120 | 121 | // Copy negative sign. 122 | *val |= ((uint32_t)(*(inbuffer++)) & 0x80) << 24; 123 | 124 | return 8; 125 | } 126 | 127 | // Copy data from variable into a byte array 128 | template 129 | static void varToArr(A arr, const V var) 130 | { 131 | for (size_t i = 0; i < sizeof(V); i++) 132 | arr[i] = (var >> (8 * i)); 133 | } 134 | 135 | // Copy data from a byte array into variable 136 | template 137 | static void arrToVar(V& var, const A arr) 138 | { 139 | var = 0; 140 | for (size_t i = 0; i < sizeof(V); i++) 141 | var |= (arr[i] << (8 * i)); 142 | } 143 | 144 | }; 145 | 146 | } // namespace ros 147 | 148 | #endif 149 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rosserial_client 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.8.0 (2018-10-11) 6 | ------------------ 7 | * Add an empty newline to the generated headers. (`#389 `_) 8 | * Add support for boolean parameters (`#355 `_) 9 | * Contributors: Miklós Márton, Pikrass 10 | 11 | 0.7.7 (2017-11-29) 12 | ------------------ 13 | * Add overall spin timeout to rosserial read. (`#334 `_) 14 | * Fixing formatting on files. (`#333 `_) 15 | * Fix catkin lint errors (`#296 `_) 16 | * fix spinOnce timeout : 5ms -> 5s (`#326 `_) 17 | * [Client] Fix a warning in comparison. (`#323 `_) 18 | * Use const in ros namespace instead of #define for constants. Fix `#283 `_ (`#318 `_) 19 | * Fix CMP0046 warnings in catkin-built firmwares (`#320 `_) 20 | * Prevent time variable overflow leading to parameter timeout error (`#293 `_) 21 | * Add class member method callback support for Service Server. (`#282 `_) 22 | * Added capability to specify timeout in getParam methods (`#278 `_) 23 | * Contributors: 1r0b1n0, Alessandro Francescon, Bei Chen Liu, David Portugal, Dmitry Kargin, Mike O'Driscoll, Mike Purvis, Romain Reignier 24 | 25 | 0.7.6 (2017-03-01) 26 | ------------------ 27 | * Fixing message has no attribute _md5sum (`#257 `_) 28 | * Contributors: Mike O'Driscoll 29 | 30 | 0.7.5 (2016-11-22) 31 | ------------------ 32 | * rosserial client variable typedefs (`#254 `_) 33 | * Add typedefs to generated messages 34 | This brings rosserial message headers in line with 35 | roscpp headers that provide a typedef for the message 36 | variable member. 37 | * Removing unused imports and variables. 38 | * Added functions for endian-agnostic memory copying (`#240 `_) 39 | * Contributors: Mike O'Driscoll, ivan 40 | 41 | 0.7.4 (2016-09-21) 42 | ------------------ 43 | * Integration tests for rosserial (`#243 `_) 44 | * Support member functions as subscriber callbacks. 45 | * Contributors: Mike O'Driscoll, Mike Purvis 46 | 47 | 0.7.3 (2016-08-05) 48 | ------------------ 49 | * Order packages by alpha rather than topologically. (`#234 `_) 50 | * Contributors: Mike Purvis 51 | 52 | 0.7.2 (2016-07-15) 53 | ------------------ 54 | * Add ros.h include to transform_broadcaster 55 | * Add environment variable for arduino location 56 | * Supported 32bit array lengths in python make_libraries script 57 | * Contributors: Alan Meekins, David Lavoie-Boutin 58 | 59 | 0.7.1 (2015-07-06) 60 | ------------------ 61 | * Provide option to pass through CMake arguments in the CMAKE_COMMAND 62 | invocation. The use-case is primarily specifying additional paths to 63 | modules, for separately-packaged libraries. 64 | * Contributors: Mike Purvis 65 | 66 | 0.7.0 (2015-04-23) 67 | ------------------ 68 | * Initial release for Jade. 69 | * Make message generating error message more verbose. 70 | * Fix initializer for fixed-length arrays. 71 | * Generate constructors for messages. 72 | * Switch to stdint integers. This allows the client to run on 64-bit systems. 73 | * Contributors: Mickaël, Mike Purvis, Mitchell Wills, chuck-h 74 | 75 | 0.6.3 (2014-11-05) 76 | ------------------ 77 | * Move avr serialization logic to Msg class, add gtest to exercise it. 78 | * Fixed the deserialization of avr64bit in order to support negative numbers. 79 | * Contributors: Martin Gerdzhev, Mike Purvis 80 | 81 | 0.6.2 (2014-09-10) 82 | ------------------ 83 | * Generic CMake helpers for ros_lib generation and in-package firmwares. 84 | * Fix output of make_library when package has only messages 85 | * Added time out to the state machine 86 | * Contributors: Jason Scatena, Michael Ferguson, Mike Purvis 87 | 88 | 0.6.1 (2014-06-30) 89 | ------------------ 90 | * Remove ID_TX_STOP define 91 | * Fix ID_TX_STOP in the client lib. 92 | * Contributors: Mike Purvis 93 | 94 | 0.6.0 (2014-06-11) 95 | ------------------ 96 | * Remove include of ros.h from time.cpp 97 | * No xx_val pointers for fixed-length arrays of messages. 98 | * Use const char* instead of char* for strings in messages. 99 | * Contributors: Mike Purvis 100 | 101 | 0.5.6 (2014-06-11) 102 | ------------------ 103 | * Add Mike Purvis as maintainer 104 | * make tf topic absolute instead of relative to prevent remapping with tag 105 | * fix: msg id serialization 106 | * fix: wrong message lenght, if message size more than 255 107 | * fix odometry deserialization error http://answers.ros.org/question/73807/rosserial-deserialization-error/ 108 | * add better debugging information when packages are missing dependencies 109 | * remove ID_TX_STOP from rosserial_msgs/msg/TopicInfo.msg, using hardcode modification. 110 | * fix the dupilcated registration problem of subscriber 111 | * Contributors: Michael Ferguson, Mike Purvis, Moju Zhao, agentx3r, denis 112 | 113 | 0.5.5 (2014-01-14) 114 | ------------------ 115 | 116 | 0.5.4 (2013-10-17) 117 | ------------------ 118 | * fix an uninitialized data bug on arduino 119 | 120 | 0.5.3 (2013-09-21) 121 | ------------------ 122 | * Added some missing return values 123 | * Fixed uninitialized arrays that would cause random segfaults on spinOnce 124 | and advertise. Fixed other ininitialized variables. 125 | * fixed misalignment for 32 bit architectures 126 | 127 | 0.5.2 (2013-07-17) 128 | ------------------ 129 | 130 | * Fix release version 131 | 132 | 0.5.1 (2013-07-15) 133 | ------------------ 134 | * Modified the return value of publish() 135 | * Modified the frame structure for serial communication, particularly add the checksum for msg_len 136 | * Associated protocol version ID in message and version mismatch handling 137 | 138 | 0.4.5 (2013-07-02) 139 | ------------------ 140 | * fail gently when messages/packages are corrupt. update print statements while at it 141 | * Fixed a bug in ros_lib install logic which took an exception because it copied files to themselves 142 | Added execute permission to make_libraries.py in rosserial_embeddedlinux 143 | Moved examples under src in rosserial_embeddedlinux 144 | 145 | 0.4.4 (2013-03-20) 146 | ------------------ 147 | 148 | 0.4.3 (2013-03-13 14:08) 149 | ------------------------ 150 | 151 | 0.4.2 (2013-03-13 01:15) 152 | ------------------------ 153 | * fix build issues when in isolation by moving more stuff into make_library 154 | 155 | 0.4.1 (2013-03-09) 156 | ------------------ 157 | 158 | 0.4.0 (2013-03-08) 159 | ------------------ 160 | * initial catkin version on github 161 | * Temporary patch for `#30 `_ 162 | * Added missing math.h include. 163 | * Changed DEBUG log level to ROSDEBUG. 164 | -------------------------------------------------------------------------------- /src/ros_lib/ros/node_handle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote prducts derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef ROS_NODE_HANDLE_H_ 36 | #define ROS_NODE_HANDLE_H_ 37 | 38 | #include 39 | 40 | #include "std_msgs/Time.h" 41 | #include "rosserial_msgs/TopicInfo.h" 42 | #include "rosserial_msgs/Log.h" 43 | #include "rosserial_msgs/RequestParam.h" 44 | 45 | #include "ros/msg.h" 46 | 47 | namespace ros 48 | { 49 | 50 | class NodeHandleBase_ 51 | { 52 | public: 53 | virtual int publish(int id, const Msg* msg) = 0; 54 | virtual int spinOnce() = 0; 55 | virtual bool connected() = 0; 56 | }; 57 | } 58 | 59 | #include "ros/publisher.h" 60 | #include "ros/subscriber.h" 61 | #include "ros/service_server.h" 62 | #include "ros/service_client.h" 63 | 64 | namespace ros 65 | { 66 | 67 | const int SPIN_OK = 0; 68 | const int SPIN_ERR = -1; 69 | const int SPIN_TIMEOUT = -2; 70 | 71 | const uint8_t SYNC_SECONDS = 5; 72 | const uint8_t MODE_FIRST_FF = 0; 73 | /* 74 | * The second sync byte is a protocol version. It's value is 0xff for the first 75 | * version of the rosserial protocol (used up to hydro), 0xfe for the second version 76 | * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable 77 | * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy 78 | * rosserial_arduino. It must be changed in both this file and in 79 | * rosserial_python/src/rosserial_python/SerialClient.py 80 | */ 81 | const uint8_t MODE_PROTOCOL_VER = 1; 82 | const uint8_t PROTOCOL_VER1 = 0xff; // through groovy 83 | const uint8_t PROTOCOL_VER2 = 0xfe; // in hydro 84 | const uint8_t PROTOCOL_VER = PROTOCOL_VER2; 85 | const uint8_t MODE_SIZE_L = 2; 86 | const uint8_t MODE_SIZE_H = 3; 87 | const uint8_t MODE_SIZE_CHECKSUM = 4; // checksum for msg size received from size L and H 88 | const uint8_t MODE_TOPIC_L = 5; // waiting for topic id 89 | const uint8_t MODE_TOPIC_H = 6; 90 | const uint8_t MODE_MESSAGE = 7; 91 | const uint8_t MODE_MSG_CHECKSUM = 8; // checksum for msg and topic id 92 | 93 | 94 | const uint8_t SERIAL_MSG_TIMEOUT = 20; // 20 milliseconds to recieve all of message data 95 | 96 | using rosserial_msgs::TopicInfo; 97 | 98 | /* Node Handle */ 99 | template 104 | class NodeHandle_ : public NodeHandleBase_ 105 | { 106 | protected: 107 | Hardware hardware_; 108 | 109 | /* time used for syncing */ 110 | uint32_t rt_time; 111 | 112 | /* used for computing current time */ 113 | uint32_t sec_offset, nsec_offset; 114 | 115 | /* Spinonce maximum work timeout */ 116 | uint32_t spin_timeout_; 117 | 118 | uint8_t message_in[INPUT_SIZE]; 119 | uint8_t message_out[OUTPUT_SIZE]; 120 | 121 | Publisher * publishers[MAX_PUBLISHERS]; 122 | Subscriber_ * subscribers[MAX_SUBSCRIBERS]; 123 | 124 | /* 125 | * Setup Functions 126 | */ 127 | public: 128 | NodeHandle_() : configured_(false) 129 | { 130 | 131 | for (unsigned int i = 0; i < MAX_PUBLISHERS; i++) 132 | publishers[i] = 0; 133 | 134 | for (unsigned int i = 0; i < MAX_SUBSCRIBERS; i++) 135 | subscribers[i] = 0; 136 | 137 | for (unsigned int i = 0; i < INPUT_SIZE; i++) 138 | message_in[i] = 0; 139 | 140 | for (unsigned int i = 0; i < OUTPUT_SIZE; i++) 141 | message_out[i] = 0; 142 | 143 | req_param_resp.ints_length = 0; 144 | req_param_resp.ints = NULL; 145 | req_param_resp.floats_length = 0; 146 | req_param_resp.floats = NULL; 147 | req_param_resp.ints_length = 0; 148 | req_param_resp.ints = NULL; 149 | 150 | spin_timeout_ = 0; 151 | } 152 | 153 | Hardware* getHardware() 154 | { 155 | return &hardware_; 156 | } 157 | 158 | /* Start serial, initialize buffers */ 159 | void initNode() 160 | { 161 | hardware_.init(); 162 | mode_ = 0; 163 | bytes_ = 0; 164 | index_ = 0; 165 | topic_ = 0; 166 | }; 167 | 168 | /* Start a named port, which may be network server IP, initialize buffers */ 169 | void initNode(char *portName) 170 | { 171 | hardware_.init(portName); 172 | mode_ = 0; 173 | bytes_ = 0; 174 | index_ = 0; 175 | topic_ = 0; 176 | }; 177 | 178 | /** 179 | * @brief Sets the maximum time in millisconds that spinOnce() can work. 180 | * This will not effect the processing of the buffer, as spinOnce processes 181 | * one byte at a time. It simply sets the maximum time that one call can 182 | * process for. You can choose to clear the buffer if that is beneficial if 183 | * SPIN_TIMEOUT is returned from spinOnce(). 184 | * @param timeout The timeout in milliseconds that spinOnce will function. 185 | */ 186 | void setSpinTimeout(const uint32_t& timeout) 187 | { 188 | spin_timeout_ = timeout; 189 | } 190 | 191 | protected: 192 | //State machine variables for spinOnce 193 | int mode_; 194 | int bytes_; 195 | int topic_; 196 | int index_; 197 | int checksum_; 198 | 199 | bool configured_; 200 | 201 | /* used for syncing the time */ 202 | uint32_t last_sync_time; 203 | uint32_t last_sync_receive_time; 204 | uint32_t last_msg_timeout_time; 205 | 206 | public: 207 | /* This function goes in your loop() function, it handles 208 | * serial input and callbacks for subscribers. 209 | */ 210 | 211 | 212 | virtual int spinOnce() 213 | { 214 | /* restart if timed out */ 215 | uint32_t c_time = hardware_.time(); 216 | if ((c_time - last_sync_receive_time) > (SYNC_SECONDS * 2200)) 217 | { 218 | configured_ = false; 219 | } 220 | 221 | /* reset if message has timed out */ 222 | if (mode_ != MODE_FIRST_FF) 223 | { 224 | if (c_time > last_msg_timeout_time) 225 | { 226 | mode_ = MODE_FIRST_FF; 227 | } 228 | } 229 | 230 | /* while available buffer, read data */ 231 | while (true) 232 | { 233 | // If a timeout has been specified, check how long spinOnce has been running. 234 | if (spin_timeout_ > 0) 235 | { 236 | // If the maximum processing timeout has been exceeded, exit with error. 237 | // The next spinOnce can continue where it left off, or optionally 238 | // based on the application in use, the hardware buffer could be flushed 239 | // and start fresh. 240 | if ((hardware_.time() - c_time) > spin_timeout_) 241 | { 242 | // Exit the spin, processing timeout exceeded. 243 | return SPIN_TIMEOUT; 244 | } 245 | } 246 | int data = hardware_.read(); 247 | if (data < 0) 248 | break; 249 | checksum_ += data; 250 | if (mode_ == MODE_MESSAGE) /* message data being recieved */ 251 | { 252 | message_in[index_++] = data; 253 | bytes_--; 254 | if (bytes_ == 0) /* is message complete? if so, checksum */ 255 | mode_ = MODE_MSG_CHECKSUM; 256 | } 257 | else if (mode_ == MODE_FIRST_FF) 258 | { 259 | if (data == 0xff) 260 | { 261 | mode_++; 262 | last_msg_timeout_time = c_time + SERIAL_MSG_TIMEOUT; 263 | } 264 | else if (hardware_.time() - c_time > (SYNC_SECONDS * 1000)) 265 | { 266 | /* We have been stuck in spinOnce too long, return error */ 267 | configured_ = false; 268 | return SPIN_TIMEOUT; 269 | } 270 | } 271 | else if (mode_ == MODE_PROTOCOL_VER) 272 | { 273 | if (data == PROTOCOL_VER) 274 | { 275 | mode_++; 276 | } 277 | else 278 | { 279 | mode_ = MODE_FIRST_FF; 280 | if (configured_ == false) 281 | requestSyncTime(); /* send a msg back showing our protocol version */ 282 | } 283 | } 284 | else if (mode_ == MODE_SIZE_L) /* bottom half of message size */ 285 | { 286 | bytes_ = data; 287 | index_ = 0; 288 | mode_++; 289 | checksum_ = data; /* first byte for calculating size checksum */ 290 | } 291 | else if (mode_ == MODE_SIZE_H) /* top half of message size */ 292 | { 293 | bytes_ += data << 8; 294 | mode_++; 295 | } 296 | else if (mode_ == MODE_SIZE_CHECKSUM) 297 | { 298 | if ((checksum_ % 256) == 255) 299 | mode_++; 300 | else 301 | mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */ 302 | } 303 | else if (mode_ == MODE_TOPIC_L) /* bottom half of topic id */ 304 | { 305 | topic_ = data; 306 | mode_++; 307 | checksum_ = data; /* first byte included in checksum */ 308 | } 309 | else if (mode_ == MODE_TOPIC_H) /* top half of topic id */ 310 | { 311 | topic_ += data << 8; 312 | mode_ = MODE_MESSAGE; 313 | if (bytes_ == 0) 314 | mode_ = MODE_MSG_CHECKSUM; 315 | } 316 | else if (mode_ == MODE_MSG_CHECKSUM) /* do checksum */ 317 | { 318 | mode_ = MODE_FIRST_FF; 319 | if ((checksum_ % 256) == 255) 320 | { 321 | if (topic_ == TopicInfo::ID_PUBLISHER) 322 | { 323 | requestSyncTime(); 324 | negotiateTopics(); 325 | last_sync_time = c_time; 326 | last_sync_receive_time = c_time; 327 | return SPIN_ERR; 328 | } 329 | else if (topic_ == TopicInfo::ID_TIME) 330 | { 331 | syncTime(message_in); 332 | } 333 | else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST) 334 | { 335 | req_param_resp.deserialize(message_in); 336 | param_recieved = true; 337 | } 338 | else if (topic_ == TopicInfo::ID_TX_STOP) 339 | { 340 | configured_ = false; 341 | } 342 | else 343 | { 344 | if (subscribers[topic_ - 100]) 345 | subscribers[topic_ - 100]->callback(message_in); 346 | } 347 | } 348 | } 349 | } 350 | 351 | /* occasionally sync time */ 352 | if (configured_ && ((c_time - last_sync_time) > (SYNC_SECONDS * 500))) 353 | { 354 | requestSyncTime(); 355 | last_sync_time = c_time; 356 | } 357 | 358 | return SPIN_OK; 359 | } 360 | 361 | 362 | /* Are we connected to the PC? */ 363 | virtual bool connected() 364 | { 365 | return configured_; 366 | }; 367 | 368 | /******************************************************************** 369 | * Time functions 370 | */ 371 | 372 | void requestSyncTime() 373 | { 374 | std_msgs::Time t; 375 | publish(TopicInfo::ID_TIME, &t); 376 | rt_time = hardware_.time(); 377 | } 378 | 379 | void syncTime(uint8_t * data) 380 | { 381 | std_msgs::Time t; 382 | uint32_t offset = hardware_.time() - rt_time; 383 | 384 | t.deserialize(data); 385 | t.data.sec += offset / 1000; 386 | t.data.nsec += (offset % 1000) * 1000000UL; 387 | 388 | this->setNow(t.data); 389 | last_sync_receive_time = hardware_.time(); 390 | } 391 | 392 | Time now() 393 | { 394 | uint32_t ms = hardware_.time(); 395 | Time current_time; 396 | current_time.sec = ms / 1000 + sec_offset; 397 | current_time.nsec = (ms % 1000) * 1000000UL + nsec_offset; 398 | normalizeSecNSec(current_time.sec, current_time.nsec); 399 | return current_time; 400 | } 401 | 402 | void setNow(Time & new_now) 403 | { 404 | uint32_t ms = hardware_.time(); 405 | sec_offset = new_now.sec - ms / 1000 - 1; 406 | nsec_offset = new_now.nsec - (ms % 1000) * 1000000UL + 1000000000UL; 407 | normalizeSecNSec(sec_offset, nsec_offset); 408 | } 409 | 410 | /******************************************************************** 411 | * Topic Management 412 | */ 413 | 414 | /* Register a new publisher */ 415 | bool advertise(Publisher & p) 416 | { 417 | for (int i = 0; i < MAX_PUBLISHERS; i++) 418 | { 419 | if (publishers[i] == 0) // empty slot 420 | { 421 | publishers[i] = &p; 422 | p.id_ = i + 100 + MAX_SUBSCRIBERS; 423 | p.nh_ = this; 424 | return true; 425 | } 426 | } 427 | return false; 428 | } 429 | 430 | /* Register a new subscriber */ 431 | template 432 | bool subscribe(SubscriberT& s) 433 | { 434 | for (int i = 0; i < MAX_SUBSCRIBERS; i++) 435 | { 436 | if (subscribers[i] == 0) // empty slot 437 | { 438 | subscribers[i] = static_cast(&s); 439 | s.id_ = i + 100; 440 | return true; 441 | } 442 | } 443 | return false; 444 | } 445 | 446 | /* Register a new Service Server */ 447 | template 448 | bool advertiseService(ServiceServer& srv) 449 | { 450 | bool v = advertise(srv.pub); 451 | for (int i = 0; i < MAX_SUBSCRIBERS; i++) 452 | { 453 | if (subscribers[i] == 0) // empty slot 454 | { 455 | subscribers[i] = static_cast(&srv); 456 | srv.id_ = i + 100; 457 | return v; 458 | } 459 | } 460 | return false; 461 | } 462 | 463 | /* Register a new Service Client */ 464 | template 465 | bool serviceClient(ServiceClient& srv) 466 | { 467 | bool v = advertise(srv.pub); 468 | for (int i = 0; i < MAX_SUBSCRIBERS; i++) 469 | { 470 | if (subscribers[i] == 0) // empty slot 471 | { 472 | subscribers[i] = static_cast(&srv); 473 | srv.id_ = i + 100; 474 | return v; 475 | } 476 | } 477 | return false; 478 | } 479 | 480 | void negotiateTopics() 481 | { 482 | rosserial_msgs::TopicInfo ti; 483 | int i; 484 | for (i = 0; i < MAX_PUBLISHERS; i++) 485 | { 486 | if (publishers[i] != 0) // non-empty slot 487 | { 488 | ti.topic_id = publishers[i]->id_; 489 | ti.topic_name = (char *) publishers[i]->topic_; 490 | ti.message_type = (char *) publishers[i]->msg_->getType(); 491 | ti.md5sum = (char *) publishers[i]->msg_->getMD5(); 492 | ti.buffer_size = OUTPUT_SIZE; 493 | publish(publishers[i]->getEndpointType(), &ti); 494 | } 495 | } 496 | for (i = 0; i < MAX_SUBSCRIBERS; i++) 497 | { 498 | if (subscribers[i] != 0) // non-empty slot 499 | { 500 | ti.topic_id = subscribers[i]->id_; 501 | ti.topic_name = (char *) subscribers[i]->topic_; 502 | ti.message_type = (char *) subscribers[i]->getMsgType(); 503 | ti.md5sum = (char *) subscribers[i]->getMsgMD5(); 504 | ti.buffer_size = INPUT_SIZE; 505 | publish(subscribers[i]->getEndpointType(), &ti); 506 | } 507 | } 508 | configured_ = true; 509 | } 510 | 511 | virtual int publish(int id, const Msg * msg) 512 | { 513 | if (id >= 100 && !configured_) 514 | return 0; 515 | 516 | /* serialize message */ 517 | int l = msg->serialize(message_out + 7); 518 | 519 | /* setup the header */ 520 | message_out[0] = 0xff; 521 | message_out[1] = PROTOCOL_VER; 522 | message_out[2] = (uint8_t)((uint16_t)l & 255); 523 | message_out[3] = (uint8_t)((uint16_t)l >> 8); 524 | message_out[4] = 255 - ((message_out[2] + message_out[3]) % 256); 525 | message_out[5] = (uint8_t)((int16_t)id & 255); 526 | message_out[6] = (uint8_t)((int16_t)id >> 8); 527 | 528 | /* calculate checksum */ 529 | int chk = 0; 530 | for (int i = 5; i < l + 7; i++) 531 | chk += message_out[i]; 532 | l += 7; 533 | message_out[l++] = 255 - (chk % 256); 534 | 535 | if (l <= OUTPUT_SIZE) 536 | { 537 | hardware_.write(message_out, l); 538 | return l; 539 | } 540 | else 541 | { 542 | logerror("Message from device dropped: message larger than buffer."); 543 | return -1; 544 | } 545 | } 546 | 547 | /******************************************************************** 548 | * Logging 549 | */ 550 | 551 | private: 552 | void log(char byte, const char * msg) 553 | { 554 | rosserial_msgs::Log l; 555 | l.level = byte; 556 | l.msg = (char*)msg; 557 | publish(rosserial_msgs::TopicInfo::ID_LOG, &l); 558 | } 559 | 560 | public: 561 | void logdebug(const char* msg) 562 | { 563 | log(rosserial_msgs::Log::ROSDEBUG, msg); 564 | } 565 | void loginfo(const char * msg) 566 | { 567 | log(rosserial_msgs::Log::INFO, msg); 568 | } 569 | void logwarn(const char *msg) 570 | { 571 | log(rosserial_msgs::Log::WARN, msg); 572 | } 573 | void logerror(const char*msg) 574 | { 575 | log(rosserial_msgs::Log::ERROR, msg); 576 | } 577 | void logfatal(const char*msg) 578 | { 579 | log(rosserial_msgs::Log::FATAL, msg); 580 | } 581 | 582 | /******************************************************************** 583 | * Parameters 584 | */ 585 | 586 | private: 587 | bool param_recieved; 588 | rosserial_msgs::RequestParamResponse req_param_resp; 589 | 590 | bool requestParam(const char * name, int time_out = 1000) 591 | { 592 | param_recieved = false; 593 | rosserial_msgs::RequestParamRequest req; 594 | req.name = (char*)name; 595 | publish(TopicInfo::ID_PARAMETER_REQUEST, &req); 596 | uint32_t end_time = hardware_.time() + time_out; 597 | while (!param_recieved) 598 | { 599 | spinOnce(); 600 | if (hardware_.time() > end_time) 601 | { 602 | logwarn("Failed to get param: timeout expired"); 603 | return false; 604 | } 605 | } 606 | return true; 607 | } 608 | 609 | public: 610 | bool getParam(const char* name, int* param, int length = 1, int timeout = 1000) 611 | { 612 | if (requestParam(name, timeout)) 613 | { 614 | if (length == req_param_resp.ints_length) 615 | { 616 | //copy it over 617 | for (int i = 0; i < length; i++) 618 | param[i] = req_param_resp.ints[i]; 619 | return true; 620 | } 621 | else 622 | { 623 | logwarn("Failed to get param: length mismatch"); 624 | } 625 | } 626 | return false; 627 | } 628 | bool getParam(const char* name, float* param, int length = 1, int timeout = 1000) 629 | { 630 | if (requestParam(name, timeout)) 631 | { 632 | if (length == req_param_resp.floats_length) 633 | { 634 | //copy it over 635 | for (int i = 0; i < length; i++) 636 | param[i] = req_param_resp.floats[i]; 637 | return true; 638 | } 639 | else 640 | { 641 | logwarn("Failed to get param: length mismatch"); 642 | } 643 | } 644 | return false; 645 | } 646 | bool getParam(const char* name, char** param, int length = 1, int timeout = 1000) 647 | { 648 | if (requestParam(name, timeout)) 649 | { 650 | if (length == req_param_resp.strings_length) 651 | { 652 | //copy it over 653 | for (int i = 0; i < length; i++) 654 | strcpy(param[i], req_param_resp.strings[i]); 655 | return true; 656 | } 657 | else 658 | { 659 | logwarn("Failed to get param: length mismatch"); 660 | } 661 | } 662 | return false; 663 | } 664 | bool getParam(const char* name, bool* param, int length = 1, int timeout = 1000) 665 | { 666 | if (requestParam(name, timeout)) 667 | { 668 | if (length == req_param_resp.ints_length) 669 | { 670 | //copy it over 671 | for (int i = 0; i < length; i++) 672 | param[i] = req_param_resp.ints[i]; 673 | return true; 674 | } 675 | else 676 | { 677 | logwarn("Failed to get param: length mismatch"); 678 | } 679 | } 680 | return false; 681 | } 682 | }; 683 | 684 | } 685 | 686 | #endif 687 | --------------------------------------------------------------------------------