├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── matlab_ros_bridge ├── CMakeLists.txt ├── cmake │ └── generate_model.m.cmake ├── include │ └── matlab_ros_bridge │ │ ├── GenericPublisher.hpp │ │ ├── GenericSubscriber.hpp │ │ └── RosMatlabBrigdeDefines.hpp ├── models │ ├── others.slx │ ├── rt_sync.slx │ ├── slblocks.m │ ├── subsystem.slx │ └── test.slx ├── package.xml └── src │ └── matlab_ros_bridge │ └── rtblock.cpp ├── mrb_geometry_msgs ├── CMakeLists.txt ├── models │ └── geometry_msgs.slx ├── package.xml └── src │ └── mrb_geometry_msgs │ ├── rmsg_pub_PointStamped.cpp │ ├── rmsg_pub_PolygonStamped.cpp │ ├── rmsg_pub_PoseStamped.cpp │ ├── rmsg_pub_TransformStamped.cpp │ ├── rmsg_pub_TwistStamped.cpp │ ├── rmsg_pub_Vector3Stamped.cpp │ ├── rmsg_pub_WrenchStamped.cpp │ ├── rmsg_sub_PointStamped.cpp │ ├── rmsg_sub_PoseStamped.cpp │ ├── rmsg_sub_TransformStamped.cpp │ ├── rmsg_sub_TwistStamped.cpp │ ├── rmsg_sub_Vector3Stamped.cpp │ └── rmsg_sub_WrenchStamped.cpp ├── mrb_sensor_msgs ├── CMakeLists.txt ├── models │ └── sensor_msgs.slx ├── package.xml └── src │ └── mrb_sensor_msgs │ ├── rmsg_pub_JointState.cpp │ ├── rmsg_pub_Joy.cpp │ ├── rmsg_pub_PointCloud2_XYZ.cpp │ ├── rmsg_sub_Image.cpp │ ├── rmsg_sub_JointState.cpp │ └── rmsg_sub_Joy.cpp ├── mrb_shape_msgs ├── CMakeLists.txt ├── models │ └── shape_msgs.slx ├── package.xml └── src │ └── mrb_shape_msgs │ └── rmsg_pub_Mesh.cpp ├── mrb_std_msgs ├── CMakeLists.txt ├── models │ └── std_msgs.slx ├── package.xml └── src │ └── mrb_std_msgs │ └── rmsg_pub_String.cpp └── mrb_tf ├── CATKIN_IGNORE ├── CMakeLists.txt ├── models └── tf_lib.slx ├── package.xml └── src └── mrb_tf └── listen_tf.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | ros_stacks/active_estimation_cat/src/active_estimation_rosjava/.gradle 2 | ros_stacks/active_estimation_cat/src/active_estimation_rosjava/active_estimation_msgs/build 3 | ros_stacks/telekyb_common/src/telekyb_common_rosjava/.gradle 4 | ros_stacks/telekyb_common/src/telekyb_common_rosjava/telekyb_msgs/build 5 | slprj/ 6 | *.mex* 7 | *.so 8 | *.o 9 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake_module_matlab"] 2 | path = cmake_module_matlab 3 | url = https://github.com/lagadic/cmake_module_matlab.git 4 | branch = master 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2012, Max-Planck-Gesellschaft 4 | Copyright (c) 2012-2015, Inria 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 are met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | * Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | * Neither the name of the copyright holder nor the names of its 18 | contributors may be used to endorse or promote products derived from 19 | this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # matlab_ros_bridge 2 | 3 | This page contains the source code of a set of Matlab C++ S-functions that can be used to: 4 | 5 | * synchronize simulink with the system clock, thus obtaining a soft-real-time execution; 6 | * interface simulink blocks with other ROS nodes using ROS messages. 7 | 8 | This project is based on a work started by Martin Riedel and Riccardo Spica at the Max Plank Institute for Biological Cybernetics in Tuebingen (Germany). 9 | This fork is currently supported by [Riccardo Spica](mailto:riccardo.spica@irisa.fr) and [Giovanni Claudio](mailto:giovanni.claudio@irisa.fr) at the Inria Istitute in Rennes (France). 10 | 11 | The software is released under the BSD license. See the LICENSE file in this repository for more information. 12 | 13 | Since the compiling procedure is quite involved, please read the entire instructions carefully at least once **before** attempting to compile. 14 | 15 | ## Configuring Matlab 16 | 17 | Before proceeding with the next steps you need to configure Matlab mex compiler. 18 | 19 | For Matlab 2014b and older, run matlab and type the following command in the command window: 20 | ```matlab 21 | mex -setup 22 | ``` 23 | When you get the following message: 24 | 25 | Enter the number of the compiler (0-1): 26 | 27 | Enter 1 to select the first option. This should create a mex option file in ~/.matlab/R2014a/mexopts.sh. 28 | Open this file and make sure that the correct version of the gnu compiler is used. 29 | 30 | For Matlab 2015a and newer, run matlab and type the following commands in the command window 31 | ```matlab 32 | mex -setup 33 | mex -setup C++ 34 | ``` 35 | This should create two files in ~/.matlab// named mex_C_glnxa64.xml and mex_C++_glnxa64.xml. 36 | Open mex_C++_glnxa64.xml and substitute all instances of "g++" and "$GCC" with "g++-4.7" (or 4.4 for older versions of Matlab). 37 | Open mex_C_glnxa64.xml and substitute all instances of "gcc" and "$GCC" with "gcc-4.7" (or 4.4 for older versions of Matlab). 38 | 39 | ## Compiling the bridge 40 | 41 | The process of compiling the matlab_ros_bridge is not straightforward. The main problem is that MATLAB doesn't use the system distribution of boost but instead comes with its ows shipped version, which can be found in, e.g. 'matlabroot/bin/glnxa64/'. 42 | Since the mex files that we generate will run inside matlab it is important that they are linked against the same version of boost that is used in MATLAB. Moreover, since the mex files will also be linked to ROS libraries, we also need to recompile ROS and link it to the same version of boost. 43 | Finally we also need to compile everyting (boost, ros and our mex files) using a c/c++ compiler officially supported by the MATLAB distribution that we are using. 44 | 45 | ### Compiling boost 46 | 47 | This section will guide you through the process of compiling the required version of boost with the compiler supported by your Matlab version. Before doing this you should try using one of the precompiled boost distributions available in the download section of this repository. Check the version correspondance table below in this page to find the correct download link. 48 | If you cannot find a precompiled boost download link for you setup then keep reading, otherwise skip to the following section. 49 | 50 | 1. Download the correct version of boost from [here](http://www.boost.org/users/history/) in a folder of your choice (later referred to as ). To know which version you need you can type, in a matlab command window, the following command: 51 | 52 | ```matlab 53 | ls([matlabroot '/bin/glnxa64/libboost_date_time*']) 54 | ``` 55 | 2. Open a new terminal and navigate to and give the following command: 56 | 57 | ```bash 58 | $ ./bootstrap.sh --prefix=path/to/boost/installation/prefix 59 | ``` 60 | 3. Edit the file /project-config.jam with your favourite tool and substitute: 61 | 62 | >using gcc; 63 | 64 | with (e.g.) 65 | 66 | >using gcc : 4.4 : g++-4.4 : -std=c++0x ; 67 | 68 | The exact version of gcc/g++ that you need to use depends on the matlab release (check it on [Matlab](http://www.mathworks.it/support/sysreq/previous_releases.html) website). 69 | 70 | 4. Build boost by doing: 71 | 72 | ```bash 73 | $ ./bjam link=shared -j8 install 74 | ``` 75 | Note: while building Boost you might encounter [this pseudo-bug](https://svn.boost.org/trac/boost/ticket/6940) due to an incompatibility between older versions of Boost and the new C11 standard. To solve this you can either substitute all occurrences of `TIME_UTC` in all Boost headers with `TIME_UTC_` (as done in more recent versions of Boost). For istance, in version 1.49.0, you need to modify /boost/thread/xtime.hpp and /libs/thread/src/pthread/timeconv.inl. 76 | 77 | In some cases you might be able to solve this issue by modifying /project-config.jam so as to change 78 | 79 | >using gcc : 4.4 : g++-4.4 ; 80 | 81 | to 82 | 83 | >using gcc : 4.4 : g++-4.4 : -U_GNU_SOURCE ; 84 | 85 | If you are building boost on a x64 system you might also encounter [this bug](https://svn.boost.org/trac/boost/ticket/6851). In this case just apply the proposed fix. 86 | 87 | ### Compiling ROS 88 | 89 | 5. Follow the instructions for your ROS distribution on `http://wiki.ros.org//Installation/Source` (e.g. for [Indigo](http://wiki.ros.org/indigo/Installation/Source)), to install ROS-Comm in the "wet" version until you need to compile. DO NOT COMPILE NOW (i.e. do not run catkin_make or catkin_make_isolated). To reduce the number of ros packages to compile, you can also initialize your workspace with 90 | 91 | ```bash 92 | $ rosinstall_generator roscpp geometry_msgs std_msgs sensor_msgs shape_msgs --rosdistro indigo --deps --wet-only --tar > mrb.rosinstall 93 | $ wstool init -j8 src mrb.rosinstall 94 | ``` 95 | To disable unnecessary message generators you can also run ``export ROS_LANG_DISABLE="genlisp;genpy;geneus;gennodejs"``. 96 | 6. In a terminal navigate to the src directory of the catkin workspace created in the previous step and do: 97 | 98 | ```bash 99 | $ wstool set matlab_ros_bridge --git https://github.com/lagadic/matlab_ros_bridge.git 100 | $ wstool update matlab_ros_bridge 101 | ``` 102 | 7. Before compiling you might also need to modify the file src/roscpp/src/libros/param.cpp as described [here](https://github.com/ros/ros_comm/commit/0a589a52f5296bb3002a2f97912989715f064630). 103 | 104 | 8. Compile ros and the bridge with: 105 | 106 | ```bash 107 | $ src/catkin/bin/catkin_make --cmake-args -DBOOST_ROOT=path/to/boost/installation/prefix -DBoost_NO_SYSTEM_PATHS=ON -DCMAKE_C_COMPILER=/usr/bin/gcc-4.7 -DCMAKE_CXX_COMPILER=/usr/bin/g++-4.7 -DMATLAB_DIR=/usr/local/MATLAB/R2015b 108 | ``` 109 | note that you might need to change this command according to your , Matlab path and compiler version. 110 | Add `install` at the end or run `catkin_make install` if desired. 111 | For old versions of ROS (before Indigo) you may need to use the command `catkin_make_isolated` instead of `catkin_make`. 112 | Note: the incompatibility issue discussed in the previous section might cause `rosbag` (and possibly other packages) to fail building. If this is the case, either substitute all occurrences of `TIME_UTC` in all rosbag source files with `TIME_UTC_` or add `-DCMAKE_CXX_FLAGS=-U_GNU_SOURCE` to your `catkin_make` command above. 113 | 114 | 9. Make sure you have "sourced" your workspace by running: 115 | 116 | ```bash 117 | $ source /path/to/your/catkin_ws/devel/setup.bash 118 | ```` 119 | 120 | Navigate to the build directory of the package `matlab_ros_bridge`. It should be `catkin_ws/build/` or `catkin_ws/build_isolated/matlab_ros_bridge` if you used `catkin_make_isolated`. Now generate the simulink block library by running: 121 | 122 | ```bash 123 | $ cmake . 124 | $ make generate_library 125 | ``` 126 | 127 | ### Compiling procedure with ROS kinetic and Matlab 2017a 128 | 129 | When using this combination, a special procedure needs to be followed because of some compiling issues that are still being investigated. 130 | 131 | 1. Follow the above procedure until step 4. 132 | 133 | 5. Initialize your workspace with 134 | 135 | ```bash 136 | $ rosinstall_generator roscpp geometry_msgs std_msgs sensor_msgs shape_msgs --rosdistro kinetic --deps --wet-only --tar --exclude rospack geneus genlisp gennodejs genpy > mrb.rosinstall 137 | $ wstool init -j8 src mrb.rosinstall 138 | ``` 139 | 140 | 6. Clone the console_bridge package and build it using gcc-4.9 141 | ```bash 142 | $ git clone git@github.com:ros/console_bridge.git 143 | $ mkdir console_bridge/build 144 | $ cd console_bridge/build 145 | $ cmake -DCMAKE_C_COMPILER=/usr/bin/gcc-4.9 -DCMAKE_CXX_COMPILER=/usr/bin/g++-4.9 -DCMAKE_CXX_COMPILER=/usr/bin/g++-4.9 -DCMAKE_INSTALL_PREFIX=../install -DCMAKE_CXX_FLAGS="-Wl,--rpath=/usr/local/MATLAB/R2017a/sys/os/glnxa64" .. 146 | $ make install 147 | 148 | 7. Follow steps 6 and 7 of the standard procedure. 149 | 150 | 8. Compile ros and the bridge with: 151 | 152 | ```bash 153 | $ src/catkin/bin/catkin_make --cmake-args -DBOOST_ROOT=path/to/boost/installation/prefix -DBoost_NO_SYSTEM_PATHS=ON -DCMAKE_C_COMPILER=/usr/bin/gcc-4.9 -DCMAKE_CXX_COMPILER=/usr/bin/g++-4.9 -DMATLAB_DIR=/usr/local/MATLAB/R2017a -DROSCONSOLE_BACKEND=print -DCMAKE_CXX_FLAGS="-Wl,-rpath,/usr/local/MATLAB/R2017a/sys/os/glnxa64" -Dconsole_bridge_DIR=path/to/console_bridge/build 154 | ``` 155 | 156 | 9. Follow step 9 of the standard procedure. 157 | 158 | ### Running MATLAB 159 | 160 | 10. In your [MATLAB Startup File](http://www.mathworks.it/it/help/matlab/matlab_env/startup-options.html) add the following lines: 161 | 162 | ```matlab 163 | run(fullfile('path','to','your','catkin_ws','devel_isolated','matlab_ros_bridge','share','matlab_ros_bridge','setup.m')); 164 | ``` 165 | 11. To run matlab open a terminal and type the following: 166 | 167 | ```matlab 168 | $ source /path/to/your/catkin_ws/devel/setup.bash 169 | $ matlab 170 | ``` 171 | 172 | ### Testing that everything works 173 | 174 | 12. In your matlab command window navigate to the folder `/path/to/your/catkin_ws/src/matlab_ros_bridge/matlab_ros_bridge/models` and type 175 | 176 | ```matlab 177 | Tsim = 2e-5; 178 | ``` 179 | 180 | Now open the model `test.slx` and try to run it in all different running mode. 181 | 182 | ## Note 183 | 184 | It might be possible (but it has never been tested) to avoid compiling boost and try to link ros and the mex files against the boost libraries contained in the MATLAB installation directory. 185 | 186 | 187 | ## Matlab boost and cpp version correspondances and download link 188 | 189 | Matlab version | gcc supported version | shipped boost vesion | compiled boost download | Matlab compiled version 190 | ------------- | -------------- | ------------- | ------------- | ------------- | 191 | 2012a | GNU gcc/g++ 4.4.x | [1.44.0](http://sourceforge.net/projects/boost/files/boost/1.44.0/boost_1_44_0.tar.gz/download) | [boost_1_44_0_gcc_4_4.tar.bz2](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_1_44_0_gcc_4_4.tar.bz2) | [boost_R2012a_x64.tar.gz](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_R2012a_x64.tar.gz) 192 | 2012b | GNU gcc/g++ 4.4.x | [1.44.0](http://sourceforge.net/projects/boost/files/boost/1.44.0/boost_1_44_0.tar.gz/download) | [boost_1_44_0_gcc_4_4.tar.bz2](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_1_44_0_gcc_4_4.tar.bz2) | [boost_R2012b_x64.tar.gz](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_R2012b_x64.tar.gz) 193 | 2013a | GNU gcc/g++ 4.4.x | [1.49.0](http://sourceforge.net/projects/boost/files/boost/1.49.0/boost_1_49_0.tar.gz/download) | [boost_1_49_0_gcc_4_4.tar.bz2](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_1_49_0_gcc_4_4.tar.bz2) | [boost_R2013a_x64.tar.gz](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_R2013a_x64.tar.gz) 194 | 2013b | GNU gcc/g++ 4.7.x | [1.49.0](http://sourceforge.net/projects/boost/files/boost/1.49.0/boost_1_49_0.tar.gz/download) | [boost_1_49_0_gcc_4_7.tar.bz2](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_1_49_0_gcc_4_7.tar.bz2) | [boost_R2013b_x64.tar.gz](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_R2013b_x64.tar.gz) 195 | 2014a | GNU gcc/g++ 4.7.x | [1.49.0](http://sourceforge.net/projects/boost/files/boost/1.49.0/boost_1_49_0.tar.gz/download) | [boost_1_49_0_gcc_4_7.tar.bz2](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_1_49_0_gcc_4_7.tar.bz2) | [boost_R2014a_x64.tar.gz](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_R2014a_x64.tar.gz) 196 | 2015a | GNU gcc/g++ 4.7.x | [1.49.0](http://sourceforge.net/projects/boost/files/boost/1.49.0/boost_1_49_0.tar.gz/download) | [boost_1_49_0_gcc_4_7.tar.bz2](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_1_49_0_gcc_4_7.tar.bz2) | [boost_R2014a_x64.tar.gz](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_R2015a_x64.tar.gz) 197 | 2015b | GNU gcc/g++ 4.7.x | [1.49.0](http://sourceforge.net/projects/boost/files/boost/1.49.0/boost_1_49_0.tar.gz/download) | [boost_1_49_0_gcc_4_7.tar.bz2](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_1_49_0_gcc_4_7.tar.bz2) | [boost_R2014a_x64.tar.gz](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_R2015b_x64.tar.gz) 198 | 2016a | GNU gcc/g++ 4.7.x | [1.56.0](http://sourceforge.net/projects/boost/files/boost/1.56.0/boost_1_56_0.tar.gz/download) | [boost_1_56_0_gcc_4_7.tar.bz2](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_1_56_0_gcc_4_7.tar.bz2) | [boost_R2016a_x64.tar.gz](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_R2016a_x64.tar.gz) 199 | 2017a | GNU gcc/g++ 4.9.x | [1.56.0](http://sourceforge.net/projects/boost/files/boost/1.56.0/boost_1_56_0.tar.gz/download) | N/A | [boost_R2017a_x64.tar.gz](https://github.com/lagadic/matlab_ros_bridge/releases/download/v0.1/boost_R2017a_x64.tar.gz) 200 | 201 | ## Ros/Matlab combinatios that have already been tested: 202 | 203 | Ros\Matlab | __2013b__ | __2014a__ | __2014b__ | __2015a__ | __2015b__ | __2016a__ | __2017a__ | 204 | ----------- | --------- | --------- | --------- | --------- | --------- | --------- | --------- | 205 | __Hydro__ | working | working | never tested | never tested | never tested | never tested | never tested | 206 | __Indigo__ | never tested | working | working | working | working | working | never tested | 207 | __Kinetic__ | never tested | never tested | never tested | never tested | never tested | never tested | not working | 208 | 209 | -------------------------------------------------------------------------------- /matlab_ros_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #################################################################### 2 | # 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2012, Max-Planck-Gesellschaft 6 | # Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | #################################################################### 37 | 38 | 39 | cmake_minimum_required(VERSION 2.8.3) 40 | project(matlab_ros_bridge) 41 | 42 | 43 | #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH}) 44 | #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH}) 45 | 46 | ## Find catkin macros and libraries 47 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 48 | ## is used, also find other catkin packages 49 | find_package(catkin REQUIRED COMPONENTS 50 | roscpp 51 | cmake_module_matlab 52 | ) 53 | 54 | find_package(Matlab REQUIRED) 55 | if(MATLAB_FOUND) 56 | include_directories(${MATLAB_INCLUDE_DIRS}) 57 | include_directories(${SIMULINK_INCLUDE_DIRS}) 58 | endif() 59 | 60 | add_definitions(-DMATLAB_MEX_FILE) 61 | 62 | ## Uncomment this if the package has a setup.py. This macro ensures 63 | ## modules and global scripts declared therein get installed 64 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 65 | # catkin_python_setup() 66 | 67 | ################################################ 68 | ## Declare ROS messages, services and actions ## 69 | ################################################ 70 | 71 | ## To declare and build messages, services or actions from within this 72 | ## package, follow these steps: 73 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 74 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 77 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 78 | ## pulled in transitively but can be declared for certainty nonetheless: 79 | ## * add a build_depend tag for "message_generation" 80 | ## * add a run_depend tag for "message_runtime" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "message_generation" and every package in MSG_DEP_SET to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * add "message_runtime" and every package in MSG_DEP_SET to 85 | ## catkin_package(CATKIN_DEPENDS ...) 86 | ## * uncomment the add_*_files sections below as needed 87 | ## and list every .msg/.srv/.action file to be processed 88 | ## * uncomment the generate_messages entry below 89 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 90 | 91 | ## Generate messages in the 'msg' folder 92 | # add_message_files( 93 | # FILES 94 | # Message1.msg 95 | # Message2.msg 96 | # ) 97 | 98 | ## Generate services in the 'srv' folder 99 | # add_service_files( 100 | # FILES 101 | # Service1.srv 102 | # Service2.srv 103 | # ) 104 | 105 | ## Generate actions in the 'action' folder 106 | # add_action_files( 107 | # FILES 108 | # Action1.action 109 | # Action2.action 110 | # ) 111 | 112 | ## Generate added messages and services with any dependencies listed here 113 | # generate_messages( 114 | # DEPENDENCIES 115 | # active_estimation_msgs# geometry_msgs# sensor_msgs# std_msgs# telekyb_msgs 116 | # ) 117 | 118 | ################################### 119 | ## catkin specific configuration ## 120 | ################################### 121 | ## The catkin_package macro generates cmake config files for your package 122 | ## Declare things to be passed to dependent projects 123 | ## INCLUDE_DIRS: uncomment this if you package contains header files 124 | ## LIBRARIES: libraries you create in this project that dependent projects also need 125 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 126 | ## DEPENDS: system dependencies of this project that dependent projects also need 127 | catkin_package( 128 | INCLUDE_DIRS include 129 | # LIBRARIES matlab_ros_bridge 130 | # CATKIN_DEPENDS roscpp 131 | # DEPENDS Matlab 132 | ) 133 | 134 | ########### 135 | ## Build ## 136 | ########### 137 | 138 | ## Specify additional locations of header files 139 | ## Your package locations should be listed before other locations 140 | # include_directories(include) 141 | include_directories( 142 | include ${catkin_INCLUDE_DIRS} 143 | ) 144 | 145 | string(FIND ${CATKIN_DEVEL_PREFIX} ${PROJECT_NAME} FoundPosition REVERSE) 146 | if(FoundPosition EQUAL -1) 147 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${PROJECT_NAME}/${CATKIN_PACKAGE_LIB_DESTINATION}) 148 | else() 149 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) 150 | endif() 151 | 152 | 153 | ## Declare a cpp library 154 | add_library(rtblock 155 | src/${PROJECT_NAME}/rtblock.cpp 156 | ) 157 | 158 | set_target_properties(rtblock PROPERTIES PREFIX "" SUFFIX ".mexa64") 159 | set_target_properties(rtblock PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${${PROJECT_NAME}_MEX_DESTINATION}) 160 | 161 | target_link_libraries(rtblock ${catkin_LIBRARIES}) 162 | install(TARGETS rtblock 163 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 164 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 165 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | ) 167 | 168 | ## Declare a cpp executable 169 | # add_executable(matlab_ros_bridge_node src/matlab_ros_bridge_node.cpp) 170 | 171 | ## Add cmake target dependencies of the executable/library 172 | ## as an example, message headers may need to be generated before nodes 173 | # add_dependencies(matlab_ros_bridge_node matlab_ros_bridge_generate_messages_cpp) 174 | 175 | ## Specify libraries to link a library or executable target against 176 | # target_link_libraries(matlab_ros_bridge_node 177 | # ${catkin_LIBRARIES} 178 | # ) 179 | 180 | #get_property(MAKE_INFO_SOURCE_PATH GLOBAL PROPERTY MAKE_INFO_SOURCE_PATH) 181 | set(MAKE_INFO_SOURCE_PATH "'${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}'") 182 | 183 | #get_property(MAKE_INFO_INCLUDE_PATH GLOBAL PROPERTY MAKE_INFO_INCLUDE_PATH) 184 | #set(MAKE_INFO_INCLUDE_PATH "'${Boost_INCLUDE_DIRS}' '${catkin_INCLUDE_DIRS}'") 185 | set(MAKE_INFO_INCLUDE_PATH "'${catkin_INCLUDE_DIRS}'") 186 | string(REGEX REPLACE ";" "' '" MAKE_INFO_INCLUDE_PATH "${MAKE_INFO_INCLUDE_PATH}") 187 | 188 | #get_property(MAKE_INFO_LINK_LIBS_OBJS GLOBAL PROPERTY MAKE_INFO_LINK_LIBS_OBJS) 189 | 190 | foreach (catkin_LIBERARY ${catkin_LIBRARIES}) 191 | if(IS_ABSOLUTE ${catkin_LIBERARY}) 192 | set(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 193 | else(IS_ABSOLUTE ${catkin_LIBERARY}) 194 | find_library(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 195 | endif(IS_ABSOLUTE ${catkin_LIBERARY}) 196 | list(APPEND MAKE_INFO_LINK_LIBS_OBJS ${${catkin_LIBERARY}_path}) 197 | endforeach(catkin_LIBERARY) 198 | string(REGEX REPLACE ";" "' '" MAKE_INFO_LINK_LIBS_OBJS "'${MAKE_INFO_LINK_LIBS_OBJS}'") 199 | 200 | configure_file(${RTWMAKECFG_TEMPLATE_PATH} ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m) 201 | 202 | execute_process( 203 | COMMAND rospack plugins --attrib=mdl_path matlab_ros_bridge 204 | OUTPUT_VARIABLE MSG_TYPES OUTPUT_STRIP_TRAILING_WHITESPACE) 205 | 206 | if("${MSG_TYPES} " STREQUAL " ") 207 | if(FoundPosition EQUAL -1) 208 | message(WARNING "We didn't find any plugin! You might need to run\n source ${CATKIN_DEVEL_PREFIX}/setup.bash\nbefore you can generate the library.") 209 | else() 210 | string(LENGTH ${CATKIN_DEVEL_PREFIX} CATKIN_DEVEL_PREFIX_LENGTH) 211 | string(LENGTH ${PROJECT_NAME} PROJECT_NAME_LENGTH) 212 | math(EXPR CATKIN_DEVEL_BASE_PREFIX_LENGTH "${CATKIN_DEVEL_PREFIX_LENGTH} - ${PROJECT_NAME_LENGTH} - 1") 213 | string(SUBSTRING ${CATKIN_DEVEL_PREFIX} 0 ${CATKIN_DEVEL_BASE_PREFIX_LENGTH} CATKIN_DEVEL_BASE_PREFIX) 214 | message(WARNING "We didn't find any plugin! You might need to run\n source ${CATKIN_DEVEL_BASE_PREFIX}/setup.bash\nbefore you can generate the library.") 215 | endif() 216 | else() 217 | string(REGEX REPLACE "\n" ";" MSG_TYPES ${MSG_TYPES}) 218 | #separate_arguments(MSG_TYPES) 219 | foreach(MSG_TYPE ${MSG_TYPES}) 220 | #string(REGEX REPLACE " " ";" MSG_TYPE ${MSG_TYPE}) 221 | separate_arguments(MSG_TYPE) 222 | list(GET MSG_TYPE 0 PKG_TYPE) 223 | list(GET MSG_TYPE 1 MDL_PATH) 224 | execute_process( 225 | COMMAND rospack find ${PKG_TYPE} 226 | OUTPUT_VARIABLE ${PKG_TYPE}_PATH OUTPUT_STRIP_TRAILING_WHITESPACE) 227 | set(INSTALLED_MRB_PACKAGES "${INSTALLED_MRB_PACKAGES} '${PKG_TYPE}'") 228 | set(MDL_PATHS "${MDL_PATHS} '${${PKG_TYPE}_PATH}/${MDL_PATH}'") 229 | set(SRC_PATHS "${SRC_PATHS} '${${PKG_TYPE}_PATH}/src/${PKG_TYPE}'") 230 | endforeach(MSG_TYPE) 231 | 232 | set(SRC_PATH ${CMAKE_CURRENT_SOURCE_DIR}) 233 | set(SFUN_PATH "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}") 234 | 235 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/cmake/generate_model.m.cmake ${CMAKE_BINARY_DIR}/generate_model.m) 236 | 237 | add_custom_target(generate_library 238 | COMMAND ${MATLAB_EXE} -nodesktop -nosplash -r \"cd\('${CMAKE_BINARY_DIR}'\)\; run generate_model.m\; exit\" 239 | COMMAND mv ros_msgs.slx setup.m ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} 240 | COMMAND cp ${PROJECT_SOURCE_DIR}/models/slblocks.m ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} 241 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR}) 242 | 243 | endif() 244 | 245 | ############# 246 | ## Install ## 247 | ############# 248 | 249 | # all install targets should use catkin DESTINATION variables 250 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 251 | 252 | ## Mark executable scripts (Python etc.) for installation 253 | ## in contrast to setup.py, you can choose the destination 254 | # install(PROGRAMS 255 | # scripts/my_python_script 256 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 257 | # ) 258 | 259 | ## Mark executables and/or libraries for installation 260 | 261 | ## Mark cpp header files for installation 262 | install(DIRECTORY include/${PROJECT_NAME}/ 263 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 264 | FILES_MATCHING PATTERN "*.hpp" 265 | PATTERN ".svn" EXCLUDE 266 | PATTERN "CMakeFiles" EXCLUDE 267 | ) 268 | 269 | install(DIRECTORY src/ 270 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 271 | FILES_MATCHING PATTERN "*.cpp" 272 | PATTERN ".svn" EXCLUDE 273 | PATTERN "CMakeFiles" EXCLUDE 274 | ) 275 | 276 | install(DIRECTORY models/ 277 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/models 278 | PATTERN ".svn" EXCLUDE 279 | PATTERN "CMakeFiles" EXCLUDE 280 | ) 281 | 282 | ## Mark other files for installation (e.g. launch and bag files, etc.) 283 | install(FILES 284 | ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}/rtwmakecfg.m 285 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 286 | ) 287 | 288 | #install(FILES 289 | # startup_${PROJECT_NAME}.m 290 | # DESTINATION "." 291 | #) 292 | 293 | ############# 294 | ## Testing ## 295 | ############# 296 | 297 | ## Add gtest based cpp test target and link libraries 298 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_matlab_ros_bridge.cpp) 299 | # if(TARGET ${PROJECT_NAME}-test) 300 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 301 | # endif() 302 | 303 | ## Add folders to be run by python nosetests 304 | # catkin_add_nosetests(test) 305 | -------------------------------------------------------------------------------- /matlab_ros_bridge/cmake/generate_model.m.cmake: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Software License Agreement (BSD License) 4 | % 5 | % Copyright (c) 2012, Max-Planck-Gesellschaft 6 | % Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 37 | 38 | 39 | lib_name = 'ros_msgs'; 40 | packages = {... 41 | @INSTALLED_MRB_PACKAGES@ ... 42 | }; 43 | 44 | mdl_paths = {... 45 | @MDL_PATHS@ ... 46 | }; 47 | 48 | src_paths = {... 49 | @SRC_PATHS@ ... 50 | }; 51 | 52 | source_path = fileparts('@SRC_PATH@'); 53 | sfun_path = fileparts('@SFUN_PATH@'); 54 | 55 | if strcmp(sfun_path(end-numel('matlab_ros_bridge')+1:end), 'matlab_ros_bridge') 56 | sfun_path = sfun_path(1:end-numel('matlab_ros_bridge')); 57 | end 58 | 59 | disp('Creating main library file. This may take a while.'); 60 | new_system(lib_name,'Library'); 61 | 62 | fid = fopen( 'setup.m', 'wt' ); 63 | 64 | disp('Loading model...'); 65 | load_system(fullfile(source_path, 'matlab_ros_bridge', 'models', 'subsystem')); 66 | 67 | for j = 1:numel(packages) 68 | disp(['Generating library for package ' packages{j}]); 69 | addpath(fullfile(fileparts(mdl_paths{j}))); 70 | fprintf(fid, 'addpath(fullfile(''%s''));\n', fileparts(mdl_paths{j})); 71 | fprintf(fid, 'addpath(fullfile(''%s''));\n', src_paths{j}); 72 | fprintf(fid, 'addpath(fullfile(''%s'', ''%s'', ''lib''));\n', sfun_path, packages{j}); 73 | 74 | [~, mdl] = fileparts(mdl_paths{j}); 75 | load_system(mdl); 76 | add_block( 'subsystem/Subsystem', [lib_name '/' mdl], 'Position', [0 100*j 200 100*j+80] ) 77 | list = find_system(mdl, 'regexp', 'on'); 78 | for k = 2:numel(list) 79 | add_block( list{k}, [lib_name '/' list{k}], 'Position', [0 100*(k-2) 200 100*(k-2)+80] ); 80 | end 81 | 82 | % add_block( [packages{j} '/' packages{j}], [lib_name '/' packages{j}], 'Position', [0 100*j 200 100*j+80] ) 83 | end 84 | save_system(lib_name); 85 | close_system(lib_name); 86 | -------------------------------------------------------------------------------- /matlab_ros_bridge/include/matlab_ros_bridge/GenericPublisher.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | #ifndef GENERICPUBLISHER_HPP_ 40 | #define GENERICPUBLISHER_HPP_ 41 | 42 | #include 43 | 44 | #include 45 | 46 | template 47 | class GenericPublisher { 48 | protected: 49 | ros::Publisher pub; 50 | 51 | public: 52 | GenericPublisher(ros::NodeHandle handle, const std::string& topicName, int queue_size) { 53 | pub = handle.advertise(topicName, queue_size); 54 | } 55 | 56 | virtual ~GenericPublisher() {} 57 | 58 | void publish(const T_& msg) { 59 | pub.publish(msg); 60 | } 61 | }; 62 | 63 | #endif /* GENERICPUBLISHER_HPP_ */ 64 | -------------------------------------------------------------------------------- /matlab_ros_bridge/include/matlab_ros_bridge/GenericSubscriber.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | #ifndef GENERICSUBSCRIBER_HPP_ 40 | #define GENERICSUBSCRIBER_HPP_ 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | 48 | template 49 | class GenericSubscriber { 50 | protected: 51 | boost::shared_ptr lastMsg; 52 | mutable boost::mutex lastMsgLock; 53 | bool newMsgReceived; 54 | boost::condition_variable newMsgCondition; 55 | 56 | const std::string tName; 57 | 58 | // Subscriber 59 | ros::Subscriber sub; 60 | 61 | void msgCallback(boost::shared_ptr msg) { 62 | boost::mutex::scoped_lock lock(lastMsgLock); 63 | lastMsg = msg; 64 | newMsgReceived = true; 65 | newMsgCondition.notify_one(); 66 | } 67 | 68 | public: 69 | 70 | std::string getTopicName(){return tName;}; 71 | 72 | void resetSem(){ 73 | boost::mutex::scoped_lock lock(lastMsgLock); 74 | newMsgReceived = false; 75 | } 76 | 77 | int waitMsg(const double timeout=-1){ 78 | boost::mutex::scoped_lock lock(lastMsgLock); 79 | if (timeout<0){ 80 | while (!newMsgReceived) newMsgCondition.wait(lock); 81 | } else { 82 | boost::system_time const waitTime = boost::get_system_time() + boost::posix_time::seconds(timeout); 83 | while (!newMsgReceived){ 84 | if(!newMsgCondition.timed_wait(lock, waitTime)){ 85 | return -1; 86 | } 87 | } 88 | } 89 | return 0; 90 | } 91 | 92 | GenericSubscriber(ros::NodeHandle handle, const std::string& topicName, int queue_size) : 93 | newMsgReceived(false), 94 | sub(handle.subscribe(topicName, queue_size, &GenericSubscriber::msgCallback, this)), 95 | tName(topicName){ 96 | 97 | } 98 | 99 | virtual ~GenericSubscriber() { 100 | boost::mutex::scoped_lock lock(lastMsgLock); 101 | } 102 | 103 | boost::shared_ptr getLastMsg() { 104 | boost::mutex::scoped_lock lock(lastMsgLock); 105 | newMsgReceived = false; 106 | return lastMsg; 107 | } 108 | 109 | }; 110 | 111 | 112 | 113 | #endif /* GENERICSUBSCRIBER_HPP_ */ 114 | -------------------------------------------------------------------------------- /matlab_ros_bridge/include/matlab_ros_bridge/RosMatlabBrigdeDefines.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | #ifndef __ROSMATLABBRIDGEDEFINESHPP__ 40 | #define __ROSMATLABBRIDGEDEFINESHPP__ 41 | 42 | #include "simstruc.h" 43 | 44 | #define SFUNPRINTF(format, ...) \ 45 | ssPrintf("[%s:%s:%d] " format, TOSTRING(S_FUNCTION_NAME), __func__ , __LINE__, ##__VA_ARGS__); 46 | 47 | 48 | static void ssMessage(const SimStruct *S, const char* format, ...){ 49 | const int lenFmt = snprintf(NULL, 0, "block '%s': %s", ssGetPath(S), format); 50 | char *fmt = new char[lenFmt+1]; 51 | snprintf(fmt, lenFmt+1, "block '%s': %s", ssGetPath(S), format); 52 | 53 | va_list argptr; 54 | va_start(argptr, format); 55 | const int len = vsnprintf(NULL, 0, fmt, argptr); 56 | char *msg = new char[len+1]; 57 | vsnprintf(msg, len+1, fmt, argptr); 58 | ssPrintf("%s", msg); 59 | 60 | va_end(argptr); 61 | delete(fmt); 62 | delete(msg); 63 | } 64 | 65 | 66 | // Custom ROS INIT 67 | static void initROS(const SimStruct *S) 68 | { 69 | // Don't to anything before here. 70 | if (!ros::isInitialized()) { 71 | 72 | int argc = 0; 73 | char* argv[0]; 74 | ssMessage(S, "Initializing ROS!\n"); 75 | 76 | ros::init(argc, argv, "matlab", ros::init_options::NoSigintHandler 77 | | ros::init_options::AnonymousName 78 | | ros::init_options::NoRosout); 79 | 80 | ssMessage(S, "Done Initializing ROS!\n"); 81 | 82 | } else { 83 | //SFUNPRINTF("ROS already initialized!\n"); 84 | } 85 | } 86 | 87 | #endif //__ROSMATLABBRIDGEDEFINESHPP__ 88 | -------------------------------------------------------------------------------- /matlab_ros_bridge/models/others.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/matlab_ros_bridge/models/others.slx -------------------------------------------------------------------------------- /matlab_ros_bridge/models/rt_sync.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/matlab_ros_bridge/models/rt_sync.slx -------------------------------------------------------------------------------- /matlab_ros_bridge/models/slblocks.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % 3 | % Software License Agreement (BSD License) 4 | % 5 | % Copyright (c) 2012, Max-Planck-Gesellschaft 6 | % Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 37 | 38 | function blkStruct = slblocks 39 | % Specify that the product should appear in the library browser 40 | % and be cached in its repository 41 | Browser.Library = 'ros_msgs'; 42 | Browser.Name = 'ROS-Messages'; 43 | blkStruct.Browser = Browser; 44 | 45 | -------------------------------------------------------------------------------- /matlab_ros_bridge/models/subsystem.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/matlab_ros_bridge/models/subsystem.slx -------------------------------------------------------------------------------- /matlab_ros_bridge/models/test.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/matlab_ros_bridge/models/test.slx -------------------------------------------------------------------------------- /matlab_ros_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | matlab_ros_bridge 4 | 0.0.0 5 | The matlab_ros_bridge core package 6 | 7 | Riccardo Spica 8 | Giovanni Claudio 9 | BSD 10 | 11 | Riccardo Spica 12 | Martin Riedel 13 | 14 | http://wiki.ros.org/matlab_ros_bridge 15 | 16 | 17 | catkin 18 | roscpp 19 | cmake_module_matlab 20 | 21 | roscpp 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /matlab_ros_bridge/src/matlab_ros_bridge/rtblock.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | /* 40 | * You must specify the S_FUNCTION_NAME as the name of your S-function 41 | * (i.e. replace sfuntmpl_basic with the name of your S-function). 42 | */ 43 | 44 | #define S_FUNCTION_NAME rtblock 45 | #define S_FUNCTION_LEVEL 2 46 | 47 | #define STRINGIFY(x) #x 48 | #define TOSTRING(x) STRINGIFY(x) 49 | 50 | /* 51 | * Need to include simstruc.h for the definition of the SimStruct and 52 | * its associated macro definitions. 53 | */ 54 | #include "simstruc.h" 55 | 56 | #pragma push_macro("RT") 57 | #undef RT 58 | 59 | //#include 60 | #include 61 | 62 | #include "matlab_ros_bridge/RosMatlabBrigdeDefines.hpp" 63 | 64 | #pragma pop_macro("RT") 65 | 66 | /* Error handling 67 | * -------------- 68 | * 69 | * You should use the following technique to report errors encountered within 70 | * an S-function: 71 | * 72 | * ssSetErrorStatus(S,"Error encountered due to ..."); 73 | * return; 74 | * 75 | * Note that the 2nd argument to ssSetErrorStatus must be persistent memory. 76 | * It cannot be a local variable. For example the following will cause 77 | * unpredictable errors: 78 | * 79 | * mdlOutputs() 80 | * { 81 | * char msg[256]; {ILLEGAL: to fix use "static char msg[256];"} 82 | * sprintf(msg,"Error due to %s", string); 83 | * ssSetErrorStatus(S,msg); 84 | * return; 85 | * } 86 | * 87 | * See matlabroot/simulink/src/sfuntmpl_doc.c for more details. 88 | */ 89 | 90 | /*====================* 91 | * S-function methods * 92 | *====================*/ 93 | 94 | 95 | //double Tsim; 96 | 97 | /* Function: mdlInitializeSizes =============================================== 98 | * Abstract: 99 | * The sizes information is used by Simulink to determine the S-function 100 | * block's characteristics (number of inputs, outputs, states, etc.). 101 | */ 102 | static void mdlInitializeSizes(SimStruct *S) 103 | { 104 | /* See sfuntmpl_doc.c for more details on the macros below */ 105 | 106 | ssSetNumSFcnParams(S, 4); /* Number of expected parameters */ 107 | if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { 108 | /* Return if number of expected != number of actual parameters */ 109 | return; 110 | } 111 | 112 | ssSetNumContStates(S, 0); 113 | ssSetNumDiscStates(S, 0); 114 | 115 | if (!ssSetNumInputPorts(S, 0)) return; 116 | //ssSetInputPortWidth(S, 0, 1); 117 | //ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/ 118 | /* 119 | * Set direct feedthrough flag (1=yes, 0=no). 120 | * A port has direct feedthrough if the input is used in either 121 | * the mdlOutputs or mdlGetTimeOfNextVarHit functions. 122 | * See matlabroot/simulink/src/sfuntmpl_directfeed.txt. 123 | */ 124 | //ssSetInputPortDirectFeedThrough(S, 0, 1); 125 | 126 | if (!ssSetNumOutputPorts(S, 2)) return; 127 | ssSetOutputPortWidth(S, 0, 1); // port 0 is 1 value (rtTimeDelay) 128 | ssSetOutputPortWidth(S, 1, 1); // port 1 is 1 value (currentRosTime) 129 | 130 | 131 | ssSetNumSampleTimes(S, 1); 132 | ssSetNumRWork(S, 2); // Tsim, simulationTime, showEveryNumSeconds 133 | ssSetNumIWork(S, 1); // CycleCounter 134 | ssSetNumPWork(S, 2); // startTime,lastExecTime 135 | ssSetNumModes(S, 0); 136 | ssSetNumNonsampledZCs(S, 0); 137 | 138 | /* Specify the sim state compliance to be same as a built-in block */ 139 | ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 140 | 141 | ssSetOptions(S, 0); 142 | } 143 | 144 | 145 | 146 | /* Function: mdlInitializeSampleTimes ========================================= 147 | * Abstract: 148 | * This function is used to specify the sample time(s) for your 149 | * S-function. You must register the same number of sample times as 150 | * specified in ssSetNumSampleTimes. 151 | */ 152 | static void mdlInitializeSampleTimes(SimStruct *S) 153 | { 154 | real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0)); 155 | ssSetSampleTime(S, 0, Tsim); //CONTINUOUS_SAMPLE_TIME); 156 | ssSetOffsetTime(S, 0, 0.0); 157 | 158 | } 159 | 160 | 161 | 162 | #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ 163 | #if defined(MDL_INITIALIZE_CONDITIONS) 164 | /* Function: mdlInitializeConditions ======================================== 165 | * Abstract: 166 | * In this function, you should initialize the continuous and discrete 167 | * states for your S-function block. The initial states are placed 168 | * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S). 169 | * You can also perform any other initialization activities that your 170 | * S-function may require. Note, this routine will be called at the 171 | * start of simulation and if it is present in an enabled subsystem 172 | * configured to reset states, it will be call when the enabled subsystem 173 | * restarts execution to reset the states. 174 | */ 175 | static void mdlInitializeConditions(SimStruct *S) 176 | { 177 | } 178 | #endif /* MDL_INITIALIZE_CONDITIONS */ 179 | 180 | #define MDL_START /* Change to #undef to remove function */ 181 | #if defined(MDL_START) 182 | /* Function: mdlStart ======================================================= 183 | * Abstract: 184 | * This function is called once at start of model execution. If you 185 | * have states that should be initialized once, this is the place 186 | * to do it. 187 | */ 188 | 189 | static void mdlStart(SimStruct *S) 190 | { 191 | // Notify 192 | SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 193 | // init ROS if not yet done. 194 | initROS(S); 195 | // ros::NodeHandle nodeHandle(ros::this_node::getName()); 196 | ros::Time::init(); 197 | 198 | void** vecPWork = ssGetPWork(S); 199 | ros::Time* firstExecTime = new ros::Time(ros::Time::now()); 200 | vecPWork[0] = firstExecTime; 201 | 202 | ros::Time* lastExecTime = new ros::Time(*firstExecTime); 203 | vecPWork[1] = lastExecTime; 204 | 205 | real_T* vecRWork = ssGetRWork(S); 206 | vecRWork[0] = mxGetScalar(ssGetSFcnParam(S, 0)); // Tsim 207 | vecRWork[1] = 0.0; // simulationTime 208 | 209 | int_T* vecIWork = ssGetIWork(S); 210 | vecIWork[0] = 1; // initialize step counter 211 | 212 | } 213 | #endif /* MDL_START */ 214 | 215 | 216 | 217 | /* Function: mdlOutputs ======================================================= 218 | * Abstract: 219 | * In this function, you compute the outputs of your S-function 220 | * block. 221 | */ 222 | static void mdlOutputs(SimStruct *S, int_T tid) 223 | { 224 | real_T* vecRWork = ssGetRWork(S); 225 | void** vecPWork = ssGetPWork(S); 226 | int_T* vecIWork = ssGetIWork(S); 227 | ros::Time* firstExecTime = (ros::Time*)vecPWork[0]; 228 | ros::Time* lastExecTime = (ros::Time*)vecPWork[1]; 229 | int_T showEveryNumCycles = (int_T)mxGetScalar(ssGetSFcnParam(S, 2)); 230 | real_T slowDownFactor = (real_T)mxGetScalar(ssGetSFcnParam(S,3)); 231 | 232 | 233 | //simulationTime += Tsim; 234 | vecRWork[1] += slowDownFactor * vecRWork[0]; 235 | 236 | ros::Time currentTime = ros::Time::now(); 237 | if (*firstExecTime > currentTime){ 238 | ssWarning(S, "Resetting all timers due to negative elapsed time! \ 239 | This should not happen in normal running conditions unless \ 240 | the simulation has just started or /use_sim_time has been changed."); 241 | *firstExecTime = currentTime; 242 | vecRWork[1] = 0.0; 243 | } 244 | ros::Time timeElapsed((currentTime - *firstExecTime).toSec()); 245 | *lastExecTime = currentTime; 246 | 247 | ros::Duration sleepTime = ros::Time(vecRWork[1]) - timeElapsed; 248 | 249 | if (sleepTime.toSec() > 0.0) { 250 | sleepTime.sleep(); 251 | } 252 | else if (sleepTime.toSec() < -1.0 * mxGetScalar(ssGetSFcnParam(S, 1))) { // show error only if the delay time is above a certain threshold (2nd parameter) 253 | if (showEveryNumCycles > 0) { // never show error when (3rd parameter) <= 0 254 | if (vecIWork[0] >= showEveryNumCycles) { // show only every (3rd parameter) cycles 255 | 256 | SFUNPRINTF("Beware! rtTimer is negative! Time: %f\n", sleepTime.toSec()); 257 | vecIWork[0] = 1; 258 | } 259 | else { 260 | ++vecIWork[0]; 261 | } 262 | } 263 | } 264 | 265 | // output of sleepTime 266 | real_T *output = (real_T*)ssGetOutputPortSignal(S,0); 267 | output[0] = sleepTime.toSec(); 268 | // output of current ros time 269 | output[1] = ros::Time::now().toSec(); 270 | } 271 | 272 | 273 | 274 | #define MDL_UPDATE /* Change to #undef to remove function */ 275 | #if defined(MDL_UPDATE) 276 | /* Function: mdlUpdate ====================================================== 277 | * Abstract: 278 | * This function is called once for every major integration time step. 279 | * Discrete states are typically updated here, but this function is useful 280 | * for performing any tasks that should only take place once per 281 | * integration step. 282 | */ 283 | static void mdlUpdate(SimStruct *S, int_T tid) 284 | { 285 | } 286 | #endif /* MDL_UPDATE */ 287 | 288 | 289 | 290 | #define MDL_DERIVATIVES /* Change to #undef to remove function */ 291 | #if defined(MDL_DERIVATIVES) 292 | /* Function: mdlDerivatives ================================================= 293 | * Abstract: 294 | * In this function, you compute the S-function block's derivatives. 295 | * The derivatives are placed in the derivative vector, ssGetdX(S). 296 | */ 297 | static void mdlDerivatives(SimStruct *S) 298 | { 299 | } 300 | #endif /* MDL_DERIVATIVES */ 301 | 302 | 303 | 304 | /* Function: mdlTerminate ===================================================== 305 | * Abstract: 306 | * In this function, you should perform any actions that are necessary 307 | * at the termination of a simulation. For example, if memory was 308 | * allocated in mdlStart, this is the place to free it. 309 | */ 310 | static void mdlTerminate(SimStruct *S) 311 | { 312 | real_T* vecRWork = ssGetRWork(S); 313 | void** vecPWork = ssGetPWork(S); 314 | 315 | ros::Time* firstExecTime = (ros::Time*)vecPWork[0]; 316 | ros::Time* lastExecTime = (ros::Time*)vecPWork[1]; 317 | 318 | SFUNPRINTF("Simulation Time: %f\n", vecRWork[1]); 319 | SFUNPRINTF("Real Time: %f\n", (*lastExecTime-*firstExecTime).toSec()); 320 | 321 | delete lastExecTime; 322 | 323 | SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 324 | } 325 | 326 | 327 | /*======================================================* 328 | * See sfuntmpl_doc.c for the optional S-function methods * 329 | *======================================================*/ 330 | 331 | /*=============================* 332 | * Required S-function trailer * 333 | *=============================*/ 334 | 335 | #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ 336 | #include "simulink.c" /* MEX-file interface mechanism */ 337 | #else 338 | #include "cg_sfun.h" /* Code generation registration function */ 339 | #endif 340 | -------------------------------------------------------------------------------- /mrb_geometry_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #################################################################### 2 | # 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2012, Max-Planck-Gesellschaft 6 | # Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | #################################################################### 37 | 38 | 39 | cmake_minimum_required(VERSION 2.8.3) 40 | project(mrb_geometry_msgs) 41 | 42 | 43 | #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH}) 44 | 45 | ## Find catkin macros and libraries 46 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 47 | ## is used, also find other catkin packages 48 | find_package(catkin REQUIRED COMPONENTS 49 | matlab_ros_bridge 50 | roscpp 51 | geometry_msgs 52 | cmake_module_matlab 53 | ) 54 | 55 | find_package(Matlab REQUIRED) 56 | if(MATLAB_FOUND) 57 | include_directories(${MATLAB_INCLUDE_DIRS}) 58 | include_directories(${SIMULINK_INCLUDE_DIRS}) 59 | endif() 60 | 61 | add_definitions(-DMATLAB_MEX_FILE) 62 | 63 | ## System dependencies are found with CMake's conventions 64 | # find_package(Boost REQUIRED COMPONENTS system) 65 | 66 | 67 | ## Uncomment this if the package has a setup.py. This macro ensures 68 | ## modules and global scripts declared therein get installed 69 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 70 | # catkin_python_setup() 71 | 72 | ################################################ 73 | ## Declare ROS messages, services and actions ## 74 | ################################################ 75 | 76 | ## To declare and build messages, services or actions from within this 77 | ## package, follow these steps: 78 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 79 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 82 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 83 | ## pulled in transitively but can be declared for certainty nonetheless: 84 | ## * add a build_depend tag for "message_generation" 85 | ## * add a run_depend tag for "message_runtime" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "message_generation" and every package in MSG_DEP_SET to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * add "message_runtime" and every package in MSG_DEP_SET to 90 | ## catkin_package(CATKIN_DEPENDS ...) 91 | ## * uncomment the add_*_files sections below as needed 92 | ## and list every .msg/.srv/.action file to be processed 93 | ## * uncomment the generate_messages entry below 94 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 95 | 96 | ## Generate messages in the 'msg' folder 97 | # add_message_files( 98 | # FILES 99 | # Message1.msg 100 | # Message2.msg 101 | # ) 102 | 103 | ## Generate services in the 'srv' folder 104 | # add_service_files( 105 | # FILES 106 | # Service1.srv 107 | # Service2.srv 108 | # ) 109 | 110 | ## Generate actions in the 'action' folder 111 | # add_action_files( 112 | # FILES 113 | # Action1.action 114 | # Action2.action 115 | # ) 116 | 117 | ## Generate added messages and services with any dependencies listed here 118 | # generate_messages( 119 | # DEPENDENCIES 120 | # ) 121 | 122 | ################################### 123 | ## catkin specific configuration ## 124 | ################################### 125 | ## The catkin_package macro generates cmake config files for your package 126 | ## Declare things to be passed to dependent projects 127 | ## INCLUDE_DIRS: uncomment this if you package contains header files 128 | ## LIBRARIES: libraries you create in this project that dependent projects also need 129 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 130 | ## DEPENDS: system dependencies of this project that dependent projects also need 131 | catkin_package( 132 | # INCLUDE_DIRS include 133 | # LIBRARIES matlab_ros_bridge 134 | # CATKIN_DEPENDS roscpp geometry_msgs matlab_ros_bridge 135 | # DEPENDS boost Matlab 136 | ) 137 | 138 | ########### 139 | ## Build ## 140 | ########### 141 | 142 | ## Specify additional locations of header files 143 | ## Your package locations should be listed before other locations 144 | # include_directories(include) 145 | include_directories( 146 | ${catkin_INCLUDE_DIRS} 147 | ) 148 | 149 | set(SOURCES 150 | rmsg_pub_PointStamped.cpp 151 | rmsg_pub_TransformStamped.cpp 152 | rmsg_pub_Vector3Stamped.cpp 153 | rmsg_sub_PoseStamped.cpp 154 | rmsg_sub_TwistStamped.cpp 155 | rmsg_pub_PoseStamped.cpp 156 | rmsg_pub_TwistStamped.cpp 157 | rmsg_sub_PointStamped.cpp 158 | rmsg_sub_TransformStamped.cpp 159 | rmsg_sub_Vector3Stamped.cpp 160 | rmsg_sub_WrenchStamped.cpp 161 | rmsg_pub_WrenchStamped.cpp 162 | rmsg_pub_PolygonStamped.cpp 163 | ) 164 | 165 | string(FIND ${CATKIN_DEVEL_PREFIX} ${PROJECT_NAME} FoundPosition REVERSE) 166 | if(FoundPosition EQUAL -1) 167 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${PROJECT_NAME}/${CATKIN_PACKAGE_LIB_DESTINATION}) 168 | else() 169 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) 170 | endif() 171 | 172 | foreach(SOURCE ${SOURCES}) 173 | string(REGEX REPLACE ".cpp" "" TARGET_NAME "${SOURCE}") 174 | message(STATUS "Adding target ${TARGET_NAME} for ${SOURCE}") 175 | add_library(${TARGET_NAME} SHARED src/${PROJECT_NAME}/${SOURCE}) 176 | set_target_properties(${TARGET_NAME} PROPERTIES PREFIX "" SUFFIX ".mexa64") 177 | set_target_properties(${TARGET_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${${PROJECT_NAME}_MEX_DESTINATION}) 178 | target_link_libraries(${TARGET_NAME} ${catkin_LIBRARIES}) 179 | install(TARGETS ${TARGET_NAME} 180 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 182 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 183 | ) 184 | endforeach(SOURCE) 185 | 186 | ## Declare a cpp executable 187 | # add_executable(matlab_ros_bridge_node src/matlab_ros_bridge_node.cpp) 188 | 189 | ## Add cmake target dependencies of the executable/library 190 | ## as an example, message headers may need to be generated before nodes 191 | # add_dependencies(matlab_ros_bridge_node matlab_ros_bridge_generate_messages_cpp) 192 | 193 | ## Specify libraries to link a library or executable target against 194 | # target_link_libraries(matlab_ros_bridge_node 195 | # ${catkin_LIBRARIES} 196 | # ) 197 | 198 | #get_property(MAKE_INFO_SOURCE_PATH GLOBAL PROPERTY MAKE_INFO_SOURCE_PATH) 199 | set(MAKE_INFO_SOURCE_PATH "'${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}'") 200 | 201 | #get_property(MAKE_INFO_INCLUDE_PATH GLOBAL PROPERTY MAKE_INFO_INCLUDE_PATH) 202 | #set(MAKE_INFO_INCLUDE_PATH "'${Boost_INCLUDE_DIRS}' '${catkin_INCLUDE_DIRS}'") 203 | set(MAKE_INFO_INCLUDE_PATH "'${catkin_INCLUDE_DIRS}'") 204 | string(REGEX REPLACE ";" "' '" MAKE_INFO_INCLUDE_PATH "${MAKE_INFO_INCLUDE_PATH}") 205 | 206 | #get_property(MAKE_INFO_LINK_LIBS_OBJS GLOBAL PROPERTY MAKE_INFO_LINK_LIBS_OBJS) 207 | 208 | foreach (catkin_LIBERARY ${catkin_LIBRARIES}) 209 | if(IS_ABSOLUTE ${catkin_LIBERARY}) 210 | set(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 211 | else(IS_ABSOLUTE ${catkin_LIBERARY}) 212 | find_library(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 213 | endif(IS_ABSOLUTE ${catkin_LIBERARY}) 214 | list(APPEND MAKE_INFO_LINK_LIBS_OBJS ${${catkin_LIBERARY}_path}) 215 | endforeach(catkin_LIBERARY) 216 | string(REGEX REPLACE ";" "' '" MAKE_INFO_LINK_LIBS_OBJS "'${MAKE_INFO_LINK_LIBS_OBJS}'") 217 | 218 | configure_file(${RTWMAKECFG_TEMPLATE_PATH} ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m) 219 | 220 | ############# 221 | ## Install ## 222 | ############# 223 | 224 | # all install targets should use catkin DESTINATION variables 225 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 226 | 227 | ## Mark executable scripts (Python etc.) for installation 228 | ## in contrast to setup.py, you can choose the destination 229 | # install(PROGRAMS 230 | # scripts/my_python_script 231 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 232 | # ) 233 | 234 | ## Mark executables and/or libraries for installation 235 | 236 | ## Mark cpp header files for installation 237 | install(DIRECTORY src/ 238 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 239 | FILES_MATCHING PATTERN "*.cpp" 240 | PATTERN ".svn" EXCLUDE 241 | PATTERN "CMakeFiles" EXCLUDE 242 | ) 243 | 244 | ## Mark other files for installation (e.g. launch and bag files, etc.) 245 | install(FILES 246 | ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m 247 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 248 | ) 249 | 250 | #install(FILES 251 | # startup_${PROJECT_NAME}.m 252 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 253 | #) 254 | 255 | ############# 256 | ## Testing ## 257 | ############# 258 | 259 | ## Add gtest based cpp test target and link libraries 260 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_matlab_ros_bridge.cpp) 261 | # if(TARGET ${PROJECT_NAME}-test) 262 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 263 | # endif() 264 | 265 | ## Add folders to be run by python nosetests 266 | # catkin_add_nosetests(test) 267 | -------------------------------------------------------------------------------- /mrb_geometry_msgs/models/geometry_msgs.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/mrb_geometry_msgs/models/geometry_msgs.slx -------------------------------------------------------------------------------- /mrb_geometry_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrb_geometry_msgs 4 | 0.0.0 5 | The matlab_ros_bridge plugin package for sensor_msgs 6 | 7 | Riccardo Spica 8 | Giovanni Claudio 9 | BSD 10 | 11 | http://wiki.ros.org/matlab_ros_bridge 12 | 13 | Riccardo Spica 14 | Martin Riedel 15 | 16 | 17 | catkin 18 | roscpp 19 | geometry_msgs 20 | matlab_ros_bridge 21 | cmake_module_matlab 22 | 23 | roscpp 24 | geometry_msgs 25 | matlab_ros_bridge 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /mrb_geometry_msgs/src/mrb_geometry_msgs/rmsg_pub_PointStamped.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | /* 40 | * You must specify the S_FUNCTION_NAME as the name of your S-function 41 | * (i.e. replace sfuntmpl_basic with the name of your S-function). 42 | */ 43 | 44 | #define S_FUNCTION_NAME rmsg_pub_PointStamped 45 | #define S_FUNCTION_LEVEL 2 46 | 47 | #define STRINGIFY(x) #x 48 | #define TOSTRING(x) STRINGIFY(x) 49 | 50 | 51 | /* 52 | * Need to include simstruc.h for the definition of the SimStruct and 53 | * its associated macro definitions. 54 | */ 55 | #include "simstruc.h" 56 | 57 | #pragma push_macro("RT") 58 | #undef RT 59 | 60 | #include 61 | 62 | // Generic Publisher 63 | #include 64 | 65 | // Message 66 | #include 67 | 68 | #pragma pop_macro("RT") 69 | 70 | #include 71 | 72 | #define MDL_CHECK_PARAMETERS 73 | #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) 74 | /* Function: mdlCheckParameters ============================================= 75 | * Abstract: 76 | * Validate our parameters to verify: 77 | * o The numerator must be of a lower order than the denominator. 78 | * o The sample time must be a real positive nonzero value. 79 | */ 80 | static void mdlCheckParameters(SimStruct *S) 81 | { 82 | // SFUNPRINTF("Calling mdlCheckParameters"); 83 | 84 | // Tsim 85 | if (mxIsEmpty( ssGetSFcnParam(S,0)) || 86 | mxIsSparse( ssGetSFcnParam(S,0)) || 87 | mxIsComplex( ssGetSFcnParam(S,0)) || 88 | mxIsLogical( ssGetSFcnParam(S,0)) || 89 | !mxIsNumeric( ssGetSFcnParam(S,0)) || 90 | !mxIsDouble( ssGetSFcnParam(S,0)) || 91 | mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { 92 | ssSetErrorStatus(S,"Simulation time must be a single double Value"); 93 | return; 94 | } 95 | 96 | // Prefix Topic 97 | if (!mxIsChar( ssGetSFcnParam(S,1)) ) { 98 | ssSetErrorStatus(S,"Prefix value must be char array (string)"); 99 | return; 100 | } 101 | 102 | // Robot Array 103 | if (mxIsEmpty( ssGetSFcnParam(S,2)) || 104 | mxIsSparse( ssGetSFcnParam(S,2)) || 105 | mxIsComplex( ssGetSFcnParam(S,2)) || 106 | mxIsLogical( ssGetSFcnParam(S,2)) || 107 | !mxIsChar( ssGetSFcnParam(S,2)) ) { 108 | ssSetErrorStatus(S,"Robot Vector must be a char vector of robot ids"); 109 | return; 110 | } 111 | 112 | // Postfix Topic 113 | if (!mxIsChar( ssGetSFcnParam(S,3)) ) { 114 | ssSetErrorStatus(S,"Postfix value must be char array (string)"); 115 | return; 116 | } 117 | } 118 | #endif /* MDL_CHECK_PARAMETERS */ 119 | 120 | 121 | 122 | /* Error handling 123 | * -------------- 124 | * 125 | * You should use the following technique to report errors encountered within 126 | * an S-function: 127 | * 128 | * ssSetErrorStatus(S,"Error encountered due to ..."); 129 | * return; 130 | * 131 | * Note that the 2nd argument to ssSetErrorStatus must be persistent memory. 132 | * It cannot be a local variable. For example the following will cause 133 | * unpredictable errors: 134 | * 135 | * mdlOutputs() 136 | * { 137 | * char msg[256]; {ILLEGAL: to fix use "static char msg[256];"} 138 | * sprintf(msg,"Error due to %s", string); 139 | * ssSetErrorStatus(S,msg); 140 | * return; 141 | * } 142 | * 143 | * See matlabroot/simulink/src/sfuntmpl_doc.c for more details. 144 | */ 145 | 146 | /*====================* 147 | * S-function methods * 148 | *====================*/ 149 | 150 | 151 | //double Tsim; 152 | 153 | /* Function: mdlInitializeSizes =============================================== 154 | * Abstract: 155 | * The sizes information is used by Simulink to determine the S-function 156 | * block's characteristics (number of inputs, outputs, states, etc.). 157 | */ 158 | static void mdlInitializeSizes(SimStruct *S) 159 | { 160 | /* See sfuntmpl_doc.c for more details on the macros below */ 161 | 162 | ssSetNumSFcnParams(S, 4); /* Number of expected parameters */ 163 | #if defined(MATLAB_MEX_FILE) 164 | if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { 165 | mdlCheckParameters(S); 166 | if (ssGetErrorStatus(S) != NULL) { 167 | return; 168 | } 169 | } else { 170 | return; /* Parameter mismatch will be reported by Simulink. */ 171 | } 172 | #endif 173 | 174 | int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2)); 175 | 176 | ssSetNumContStates(S, 0); 177 | ssSetNumDiscStates(S, 0); 178 | 179 | if (!ssSetNumInputPorts(S, 1)) return; 180 | ssSetInputPortMatrixDimensions(S, 0, 3, nRobots); // position 181 | 182 | for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) { 183 | /*direct input signal access*/ 184 | ssSetInputPortRequiredContiguous(S, i, true); 185 | 186 | /* 187 | * Set direct feedthrough flag (1=yes, 0=no). 188 | * A port has direct feedthrough if the input is used in either 189 | * the mdlOutputs or mdlGetTimeOfNextVarHit functions. 190 | * See matlabroot/simulink/src/sfuntmpl_directfeed.txt. 191 | */ 192 | ssSetInputPortDirectFeedThrough(S, i, 1); 193 | } 194 | 195 | if (!ssSetNumOutputPorts(S, 0)) return; 196 | 197 | ssSetNumSampleTimes(S, 1); 198 | ssSetNumRWork(S, 0); 199 | ssSetNumIWork(S, 1); // nRobots 200 | ssSetNumPWork(S, nRobots); // nRobots x GenericPub 201 | ssSetNumModes(S, 0); 202 | ssSetNumNonsampledZCs(S, 0); 203 | 204 | /* Specify the sim state compliance to be same as a built-in block */ 205 | ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 206 | 207 | ssSetOptions(S, 0); 208 | } 209 | 210 | 211 | 212 | /* Function: mdlInitializeSampleTimes ========================================= 213 | * Abstract: 214 | * This function is used to specify the sample time(s) for your 215 | * S-function. You must register the same number of sample times as 216 | * specified in ssSetNumSampleTimes. 217 | */ 218 | 219 | static void mdlInitializeSampleTimes(SimStruct *S) 220 | { 221 | real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0)); 222 | ssSetSampleTime(S, 0, Tsim); //DISCRETE_SAMPLE_TIME); 223 | ssSetOffsetTime(S, 0, 0.0); 224 | 225 | } 226 | 227 | 228 | 229 | #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ 230 | #if defined(MDL_INITIALIZE_CONDITIONS) 231 | /* Function: mdlInitializeConditions ======================================== 232 | * Abstract: 233 | * In this function, you should initialize the continuous and discrete 234 | * states for your S-function block. The initial states are placed 235 | * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S). 236 | * You can also perform any other initialization activities that your 237 | * S-function may require. Note, this routine will be called at the 238 | * start of simulation and if it is present in an enabled subsystem 239 | * configured to reset states, it will be call when the enabled subsystem 240 | * restarts execution to reset the states. 241 | */ 242 | static void mdlInitializeConditions(SimStruct *S) 243 | { 244 | } 245 | #endif /* MDL_INITIALIZE_CONDITIONS */ 246 | 247 | #define MDL_START /* Change to #undef to remove function */ 248 | #if defined(MDL_START) 249 | /* Function: mdlStart ======================================================= 250 | * Abstract: 251 | * This function is called once at start of model execution. If you 252 | * have states that should be initialized once, this is the place 253 | * to do it. 254 | */ 255 | 256 | static void mdlStart(SimStruct *S) 257 | { 258 | SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 259 | // init ROS if not yet done. 260 | initROS(S); 261 | 262 | void** vecPWork = ssGetPWork(S); 263 | // save nRobots in IWorkVector 264 | int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2)); 265 | *ssGetIWork(S) = nRobots; 266 | 267 | ros::NodeHandle nodeHandle(ros::this_node::getName()); 268 | 269 | // get Topic Strings 270 | size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1; 271 | size_t postfix_buflen = mxGetN((ssGetSFcnParam(S, 3)))*sizeof(mxChar)+1; 272 | char* prefix_topic = (char*)mxMalloc(prefix_buflen); 273 | char* postfix_topic = (char*)mxMalloc(postfix_buflen); 274 | mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen); 275 | mxGetString((ssGetSFcnParam(S, 3)), postfix_topic, postfix_buflen); 276 | 277 | //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic); 278 | std::stringstream sstream; 279 | mxChar* robotIDs = (mxChar*)mxGetData(ssGetSFcnParam(S, 2)); 280 | for (unsigned int i = 0; i < nRobots; ++i) { 281 | sstream.str(std::string()); 282 | 283 | // build topicstring 284 | sstream << prefix_topic; 285 | sstream << (uint)robotIDs[i]; 286 | sstream << postfix_topic; 287 | 288 | GenericPublisher* pub 289 | = new GenericPublisher(nodeHandle, sstream.str(), 10); 290 | vecPWork[i] = pub; 291 | } 292 | 293 | // free char array 294 | mxFree(prefix_topic); 295 | mxFree(postfix_topic); 296 | } 297 | #endif /* MDL_START */ 298 | 299 | 300 | 301 | /* Function: mdlOutputs ======================================================= 302 | * Abstract: 303 | * In this function, you compute the outputs of your S-function 304 | * block. 305 | */ 306 | static void mdlOutputs(SimStruct *S, int_T tid) 307 | { 308 | // get Objects 309 | void** vecPWork = ssGetPWork(S); 310 | int_T nRobots = *ssGetIWork(S); 311 | 312 | // get Pointers 313 | // accessing inputs 314 | const real_T *position = (const real_T*) ssGetInputPortSignal(S,0); 315 | // const real_T *orientation = (const real_T*) ssGetInputPortSignal(S,1); 316 | 317 | geometry_msgs::PointStamped msg; 318 | for (unsigned int i = 0; i < nRobots; ++i) { 319 | GenericPublisher* pub 320 | = (GenericPublisher*)vecPWork[i]; 321 | 322 | // define send Time. 323 | msg.header.stamp = ros::Time::now(); 324 | 325 | msg.point.x = position[i*3 + 0]; 326 | msg.point.y = position[i*3 + 1]; 327 | msg.point.z = position[i*3 + 2]; 328 | 329 | 330 | pub->publish(msg); 331 | } 332 | 333 | } 334 | 335 | 336 | 337 | #define MDL_UPDATE /* Change to #undef to remove function */ 338 | #if defined(MDL_UPDATE) 339 | /* Function: mdlUpdate ====================================================== 340 | * Abstract: 341 | * This function is called once for every major integration time step. 342 | * Discrete states are typically updated here, but this function is useful 343 | * for performing any tasks that should only take place once per 344 | * integration step. 345 | */ 346 | static void mdlUpdate(SimStruct *S, int_T tid) 347 | { 348 | } 349 | #endif /* MDL_UPDATE */ 350 | 351 | 352 | 353 | #define MDL_DERIVATIVES /* Change to #undef to remove function */ 354 | #if defined(MDL_DERIVATIVES) 355 | /* Function: mdlDerivatives ================================================= 356 | * Abstract: 357 | * In this function, you compute the S-function block's derivatives. 358 | * The derivatives are placed in the derivative vector, ssGetdX(S). 359 | */ 360 | static void mdlDerivatives(SimStruct *S) 361 | { 362 | } 363 | #endif /* MDL_DERIVATIVES */ 364 | 365 | 366 | 367 | /* Function: mdlTerminate ===================================================== 368 | * Abstract: 369 | * In this function, you should perform any actions that are necessary 370 | * at the termination of a simulation. For example, if memory was 371 | * allocated in mdlStart, this is the place to free it. 372 | */ 373 | static void mdlTerminate(SimStruct *S) 374 | { 375 | // get Objects 376 | void** vecPWork = ssGetPWork(S); 377 | 378 | int_T nRobots = *ssGetIWork(S); 379 | for (unsigned int i = 0; i < nRobots; ++i) { 380 | GenericPublisher* pub = (GenericPublisher*)vecPWork[i]; 381 | // cleanup 382 | delete pub; 383 | } 384 | 385 | 386 | SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 387 | } 388 | 389 | 390 | /*======================================================* 391 | * See sfuntmpl_doc.c for the optional S-function methods * 392 | *======================================================*/ 393 | 394 | /*=============================* 395 | * Required S-function trailer * 396 | *=============================*/ 397 | 398 | #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ 399 | #include "simulink.c" /* MEX-file interface mechanism */ 400 | #else 401 | #include "cg_sfun.h" /* Code generation registration function */ 402 | #endif 403 | -------------------------------------------------------------------------------- /mrb_geometry_msgs/src/mrb_geometry_msgs/rmsg_pub_PolygonStamped.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | /* 39 | * You must specify the S_FUNCTION_NAME as the name of your S-function 40 | * (i.e. replace sfuntmpl_basic with the name of your S-function). 41 | */ 42 | 43 | #define S_FUNCTION_NAME rmsg_pub_PolygonStamped 44 | #define S_FUNCTION_LEVEL 2 45 | 46 | #define STRINGIFY(x) #x 47 | #define TOSTRING(x) STRINGIFY(x) 48 | 49 | /* 50 | * Need to include simstruc.h for the definition of the SimStruct and 51 | * its associated macro definitions. 52 | */ 53 | #include "simstruc.h" 54 | 55 | #pragma push_macro("RT") 56 | #undef RT 57 | 58 | #include 59 | 60 | // Generic Publisher 61 | #include 62 | 63 | // Message 64 | #include 65 | 66 | #pragma pop_macro("RT") 67 | 68 | #include 69 | 70 | #define MDL_CHECK_PARAMETERS 71 | #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) 72 | /* Function: mdlCheckParameters ============================================= 73 | * Abstract: 74 | * Validate our parameters to verify: 75 | * o The numerator must be of a lower order than the denominator. 76 | * o The sample time must be a real positive nonzero value. 77 | */ 78 | static void mdlCheckParameters(SimStruct *S) { 79 | // SFUNPRINTF("Calling mdlCheckParameters"); 80 | 81 | // Tsim 82 | if (mxIsEmpty(ssGetSFcnParam(S, 0)) || mxIsSparse(ssGetSFcnParam(S, 0)) 83 | || mxIsComplex(ssGetSFcnParam(S, 0)) 84 | || mxIsLogical(ssGetSFcnParam(S, 0)) 85 | || !mxIsNumeric(ssGetSFcnParam(S, 0)) 86 | || !mxIsDouble(ssGetSFcnParam(S, 0)) 87 | || mxGetNumberOfElements(ssGetSFcnParam(S, 0)) != 1) { 88 | ssSetErrorStatus(S, "Simulation time must be a single double Value"); 89 | return; 90 | } 91 | 92 | // Topic 93 | if (!mxIsChar(ssGetSFcnParam(S, 1))) { 94 | ssSetErrorStatus(S, "Topic value must be char array (string)"); 95 | return; 96 | } 97 | 98 | // Frame ID 99 | if (!mxIsChar(ssGetSFcnParam(S, 2))) { 100 | ssSetErrorStatus(S, "Frame ID value must be char array (string)"); 101 | return; 102 | } 103 | } 104 | #endif /* MDL_CHECK_PARAMETERS */ 105 | 106 | /* Error handling 107 | * -------------- 108 | * 109 | * You should use the following technique to report errors encountered within 110 | * an S-function: 111 | * 112 | * ssSetErrorStatus(S,"Error encountered due to ..."); 113 | * return; 114 | * 115 | * Note that the 2nd argument to ssSetErrorStatus must be persistent memory. 116 | * It cannot be a local variable. For example the following will cause 117 | * unpredictable errors: 118 | * 119 | * mdlOutputs() 120 | * { 121 | * char msg[256]; {ILLEGAL: to fix use "static char msg[256];"} 122 | * sprintf(msg,"Error due to %s", string); 123 | * ssSetErrorStatus(S,msg); 124 | * return; 125 | * } 126 | * 127 | * See matlabroot/simulink/src/sfuntmpl_doc.c for more details. 128 | */ 129 | 130 | /*====================* 131 | * S-function methods * 132 | *====================*/ 133 | 134 | //double Tsim; 135 | /* Function: mdlInitializeSizes =============================================== 136 | * Abstract: 137 | * The sizes information is used by Simulink to determine the S-function 138 | * block's characteristics (number of inputs, outputs, states, etc.). 139 | */ 140 | static void mdlInitializeSizes(SimStruct *S) { 141 | /* See sfuntmpl_doc.c for more details on the macros below */ 142 | 143 | ssSetNumSFcnParams(S, 3); /* Number of expected parameters */ 144 | #if defined(MATLAB_MEX_FILE) 145 | if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { 146 | mdlCheckParameters(S); 147 | if (ssGetErrorStatus(S) != NULL) { 148 | return; 149 | } 150 | } else { 151 | return; /* Parameter mismatch will be reported by Simulink. */ 152 | } 153 | #endif 154 | 155 | ssSetNumContStates(S, 0); 156 | ssSetNumDiscStates(S, 0); 157 | 158 | if (!ssSetNumInputPorts(S, 1)) 159 | return; 160 | 161 | ssSetInputPortMatrixDimensions(S, 0, DYNAMICALLY_SIZED, DYNAMICALLY_SIZED); // position 162 | // ssSetInputPortDimensionsMode(S, 0, INHERIT_DIMS_MODE); 163 | 164 | for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) { 165 | /*direct input signal access*/ 166 | ssSetInputPortRequiredContiguous(S, i, true); 167 | 168 | /* 169 | * Set direct feedthrough flag (1=yes, 0=no). 170 | * A port has direct feedthrough if the input is used in either 171 | * the mdlOutputs or mdlGetTimeOfNextVarHit functions. 172 | * See matlabroot/simulink/src/sfuntmpl_directfeed.txt. 173 | */ 174 | ssSetInputPortDirectFeedThrough(S, i, 1); 175 | } 176 | 177 | if (!ssSetNumOutputPorts(S, 0)) 178 | return; 179 | 180 | ssSetNumSampleTimes(S, 1); 181 | ssSetNumRWork(S, 0); 182 | ssSetNumIWork(S, 0); 183 | ssSetNumPWork(S, 2); //GenericPub and frame id 184 | ssSetNumModes(S, 0); 185 | ssSetNumNonsampledZCs(S, 0); 186 | 187 | /* Specify the sim state compliance to be same as a built-in block */ 188 | ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 189 | 190 | ssSetOptions(S, 0); 191 | } 192 | 193 | #if defined(MATLAB_MEX_FILE) 194 | #define MDL_SET_INPUT_PORT_DIMENSION_INFO 195 | static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, 196 | const DimsInfo_T *dimsInfo) { 197 | if(dimsInfo->numDims != 2 || dimsInfo->dims[0] != 3){ 198 | ssSetErrorStatus(S, "The input should be a 3xN matrix."); 199 | return; 200 | } 201 | if (!ssSetInputPortDimensionInfo(S, port, dimsInfo)) 202 | return; 203 | 204 | } 205 | 206 | # define MDL_SET_DEFAULT_PORT_DIMENSION_INFO 207 | /* Function: mdlSetDefaultPortDimensionInfo =========================================== 208 | * Abstract: 209 | * In case no ports were specified, the default is an input port of width 2 210 | * and an output port of width 1. 211 | */ 212 | static void mdlSetDefaultPortDimensionInfo(SimStruct *S) { 213 | ssSetInputPortMatrixDimensions(S, 0, 3, 1); 214 | } 215 | #endif 216 | 217 | /* Function: mdlInitializeSampleTimes ========================================= 218 | * Abstract: 219 | * This function is used to specify the sample time(s) for your 220 | * S-function. You must register the same number of sample times as 221 | * specified in ssSetNumSampleTimes. 222 | */ 223 | 224 | static void mdlInitializeSampleTimes(SimStruct *S) { 225 | real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0)); 226 | ssSetSampleTime(S, 0, Tsim); //DISCRETE_SAMPLE_TIME); 227 | ssSetOffsetTime(S, 0, 0.0); 228 | 229 | } 230 | 231 | #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ 232 | #if defined(MDL_INITIALIZE_CONDITIONS) 233 | /* Function: mdlInitializeConditions ======================================== 234 | * Abstract: 235 | * In this function, you should initialize the continuous and discrete 236 | * states for your S-function block. The initial states are placed 237 | * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S). 238 | * You can also perform any other initialization activities that your 239 | * S-function may require. Note, this routine will be called at the 240 | * start of simulation and if it is present in an enabled subsystem 241 | * configured to reset states, it will be call when the enabled subsystem 242 | * restarts execution to reset the states. 243 | */ 244 | static void mdlInitializeConditions(SimStruct *S) { 245 | } 246 | #endif /* MDL_INITIALIZE_CONDITIONS */ 247 | 248 | #define MDL_START /* Change to #undef to remove function */ 249 | #if defined(MDL_START) 250 | /* Function: mdlStart ======================================================= 251 | * Abstract: 252 | * This function is called once at start of model execution. If you 253 | * have states that should be initialized once, this is the place 254 | * to do it. 255 | */ 256 | 257 | static void mdlStart(SimStruct *S) { 258 | SFUNPRINTF("Creating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 259 | // init ROS if not yet done. 260 | initROS(S); 261 | 262 | void** vecPWork = ssGetPWork(S); 263 | 264 | ros::NodeHandle nodeHandle(ros::this_node::getName()); 265 | 266 | // get Topic Strings 267 | size_t buflen = mxGetN((ssGetSFcnParam(S, 1))) * sizeof(mxChar) + 1; 268 | char* topic = (char*) mxMalloc(buflen); 269 | mxGetString((ssGetSFcnParam(S, 1)), topic, buflen); 270 | 271 | //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic); 272 | GenericPublisher* pub = new GenericPublisher< 273 | geometry_msgs::PolygonStamped>(nodeHandle, topic, 10); 274 | vecPWork[0] = pub; 275 | 276 | mxFree(topic); 277 | 278 | // get Topic Strings 279 | buflen = mxGetN((ssGetSFcnParam(S, 2))) * sizeof(mxChar) + 1; 280 | topic = (char*)mxMalloc(buflen); 281 | mxGetString((ssGetSFcnParam(S, 2)), topic, buflen); 282 | vecPWork[1] = new std::string(topic); 283 | 284 | mxFree(topic); 285 | 286 | // free char array 287 | 288 | } 289 | #endif /* MDL_START */ 290 | 291 | /* Function: mdlOutputs ======================================================= 292 | * Abstract: 293 | * In this function, you compute the outputs of your S-function 294 | * block. 295 | */ 296 | static void mdlOutputs(SimStruct *S, int_T tid) { 297 | // get Objects 298 | void** vecPWork = ssGetPWork(S); 299 | 300 | // get Pointers 301 | // accessing inputs 302 | const real_T *pts = (const real_T*) ssGetInputPortSignal(S, 0); 303 | const int_T nPts = ssGetCurrentInputPortDimensions(S, 0, 1); 304 | 305 | GenericPublisher* pub = (GenericPublisher< 306 | geometry_msgs::PolygonStamped>*) vecPWork[0]; 307 | 308 | const std::string* topic = (const std::string*)vecPWork[1]; 309 | 310 | geometry_msgs::PolygonStamped msg; 311 | msg.header.stamp = ros::Time::now(); 312 | msg.header.frame_id = *topic; 313 | msg.polygon.points.resize(nPts); 314 | 315 | for (unsigned int i = 0; i < nPts; ++i) { 316 | msg.polygon.points[i].x = pts[3*i+0]; 317 | msg.polygon.points[i].y = pts[3*i+1]; 318 | msg.polygon.points[i].z = pts[3*i+2]; 319 | } 320 | 321 | pub->publish(msg); 322 | 323 | } 324 | 325 | #define MDL_UPDATE /* Change to #undef to remove function */ 326 | #if defined(MDL_UPDATE) 327 | /* Function: mdlUpdate ====================================================== 328 | * Abstract: 329 | * This function is called once for every major integration time step. 330 | * Discrete states are typically updated here, but this function is useful 331 | * for performing any tasks that should only take place once per 332 | * integration step. 333 | */ 334 | static void mdlUpdate(SimStruct *S, int_T tid) { 335 | } 336 | #endif /* MDL_UPDATE */ 337 | 338 | #define MDL_DERIVATIVES /* Change to #undef to remove function */ 339 | #if defined(MDL_DERIVATIVES) 340 | /* Function: mdlDerivatives ================================================= 341 | * Abstract: 342 | * In this function, you compute the S-function block's derivatives. 343 | * The derivatives are placed in the derivative vector, ssGetdX(S). 344 | */ 345 | static void mdlDerivatives(SimStruct *S) { 346 | } 347 | #endif /* MDL_DERIVATIVES */ 348 | 349 | /* Function: mdlTerminate ===================================================== 350 | * Abstract: 351 | * In this function, you should perform any actions that are necessary 352 | * at the termination of a simulation. For example, if memory was 353 | * allocated in mdlStart, this is the place to free it. 354 | */ 355 | static void mdlTerminate(SimStruct *S) { 356 | // get Objects 357 | void** vecPWork = ssGetPWork(S); 358 | 359 | GenericPublisher* pub = (GenericPublisher< 360 | geometry_msgs::PolygonStamped>*) vecPWork[0]; 361 | 362 | const std::string* topic = (const std::string*)vecPWork[1]; 363 | 364 | // cleanup 365 | delete pub; 366 | delete topic; 367 | 368 | SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 369 | } 370 | 371 | /*======================================================* 372 | * See sfuntmpl_doc.c for the optional S-function methods * 373 | *======================================================*/ 374 | 375 | /*=============================* 376 | * Required S-function trailer * 377 | *=============================*/ 378 | 379 | #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ 380 | #include "simulink.c" /* MEX-file interface mechanism */ 381 | #else 382 | #include "cg_sfun.h" /* Code generation registration function */ 383 | #endif 384 | -------------------------------------------------------------------------------- /mrb_geometry_msgs/src/mrb_geometry_msgs/rmsg_pub_Vector3Stamped.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | /* 40 | * You must specify the S_FUNCTION_NAME as the name of your S-function 41 | * (i.e. replace sfuntmpl_basic with the name of your S-function). 42 | */ 43 | 44 | #define S_FUNCTION_NAME rmsg_pub_Vector3Stamped 45 | #define S_FUNCTION_LEVEL 2 46 | 47 | #define STRINGIFY(x) #x 48 | #define TOSTRING(x) STRINGIFY(x) 49 | 50 | 51 | /* 52 | * Need to include simstruc.h for the definition of the SimStruct and 53 | * its associated macro definitions. 54 | */ 55 | #include "simstruc.h" 56 | 57 | #pragma push_macro("RT") 58 | #undef RT 59 | 60 | #include 61 | 62 | // Generic Publisher 63 | #include 64 | 65 | // Message 66 | #include 67 | 68 | #pragma pop_macro("RT") 69 | 70 | #include 71 | 72 | #define MDL_CHECK_PARAMETERS 73 | #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) 74 | /* Function: mdlCheckParameters ============================================= 75 | * Abstract: 76 | * Validate our parameters to verify: 77 | * o The numerator must be of a lower order than the denominator. 78 | * o The sample time must be a real positive nonzero value. 79 | */ 80 | static void mdlCheckParameters(SimStruct *S) 81 | { 82 | // SFUNPRINTF("Calling mdlCheckParameters"); 83 | 84 | // Tsim 85 | if (mxIsEmpty( ssGetSFcnParam(S,0)) || 86 | mxIsSparse( ssGetSFcnParam(S,0)) || 87 | mxIsComplex( ssGetSFcnParam(S,0)) || 88 | mxIsLogical( ssGetSFcnParam(S,0)) || 89 | !mxIsNumeric( ssGetSFcnParam(S,0)) || 90 | !mxIsDouble( ssGetSFcnParam(S,0)) || 91 | mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { 92 | ssSetErrorStatus(S,"Simulation time must be a single double Value"); 93 | return; 94 | } 95 | 96 | // Prefix Topic 97 | if (!mxIsChar( ssGetSFcnParam(S,1)) ) { 98 | ssSetErrorStatus(S,"Prefix value must be char array (string)"); 99 | return; 100 | } 101 | 102 | // Robot Array 103 | if (mxIsEmpty( ssGetSFcnParam(S,2)) || 104 | mxIsSparse( ssGetSFcnParam(S,2)) || 105 | mxIsComplex( ssGetSFcnParam(S,2)) || 106 | mxIsLogical( ssGetSFcnParam(S,2)) || 107 | !mxIsChar( ssGetSFcnParam(S,2)) ) { 108 | ssSetErrorStatus(S,"Robot Vector must be a char vector of robot ids"); 109 | return; 110 | } 111 | 112 | // Postfix Topic 113 | if (!mxIsChar( ssGetSFcnParam(S,3)) ) { 114 | ssSetErrorStatus(S,"Postfix value must be char array (string)"); 115 | return; 116 | } 117 | } 118 | #endif /* MDL_CHECK_PARAMETERS */ 119 | 120 | 121 | 122 | /* Error handling 123 | * -------------- 124 | * 125 | * You should use the following technique to report errors encountered within 126 | * an S-function: 127 | * 128 | * ssSetErrorStatus(S,"Error encountered due to ..."); 129 | * return; 130 | * 131 | * Note that the 2nd argument to ssSetErrorStatus must be persistent memory. 132 | * It cannot be a local variable. For example the following will cause 133 | * unpredictable errors: 134 | * 135 | * mdlOutputs() 136 | * { 137 | * char msg[256]; {ILLEGAL: to fix use "static char msg[256];"} 138 | * sprintf(msg,"Error due to %s", string); 139 | * ssSetErrorStatus(S,msg); 140 | * return; 141 | * } 142 | * 143 | * See matlabroot/simulink/src/sfuntmpl_doc.c for more details. 144 | */ 145 | 146 | /*====================* 147 | * S-function methods * 148 | *====================*/ 149 | 150 | 151 | //double Tsim; 152 | 153 | /* Function: mdlInitializeSizes =============================================== 154 | * Abstract: 155 | * The sizes information is used by Simulink to determine the S-function 156 | * block's characteristics (number of inputs, outputs, states, etc.). 157 | */ 158 | static void mdlInitializeSizes(SimStruct *S) 159 | { 160 | /* See sfuntmpl_doc.c for more details on the macros below */ 161 | 162 | ssSetNumSFcnParams(S, 4); /* Number of expected parameters */ 163 | #if defined(MATLAB_MEX_FILE) 164 | if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { 165 | mdlCheckParameters(S); 166 | if (ssGetErrorStatus(S) != NULL) { 167 | return; 168 | } 169 | } else { 170 | return; /* Parameter mismatch will be reported by Simulink. */ 171 | } 172 | #endif 173 | 174 | int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2)); 175 | 176 | ssSetNumContStates(S, 0); 177 | ssSetNumDiscStates(S, 0); 178 | 179 | if (!ssSetNumInputPorts(S, 1)) return; 180 | ssSetInputPortMatrixDimensions(S, 0, 3, nRobots); // position 181 | 182 | for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) { 183 | /*direct input signal access*/ 184 | ssSetInputPortRequiredContiguous(S, i, true); 185 | 186 | /* 187 | * Set direct feedthrough flag (1=yes, 0=no). 188 | * A port has direct feedthrough if the input is used in either 189 | * the mdlOutputs or mdlGetTimeOfNextVarHit functions. 190 | * See matlabroot/simulink/src/sfuntmpl_directfeed.txt. 191 | */ 192 | ssSetInputPortDirectFeedThrough(S, i, 1); 193 | } 194 | 195 | if (!ssSetNumOutputPorts(S, 0)) return; 196 | 197 | ssSetNumSampleTimes(S, 1); 198 | ssSetNumRWork(S, 0); 199 | ssSetNumIWork(S, 1); // nRobots 200 | ssSetNumPWork(S, nRobots); // nRobots x GenericPub 201 | ssSetNumModes(S, 0); 202 | ssSetNumNonsampledZCs(S, 0); 203 | 204 | /* Specify the sim state compliance to be same as a built-in block */ 205 | ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 206 | 207 | ssSetOptions(S, 0); 208 | } 209 | 210 | 211 | 212 | /* Function: mdlInitializeSampleTimes ========================================= 213 | * Abstract: 214 | * This function is used to specify the sample time(s) for your 215 | * S-function. You must register the same number of sample times as 216 | * specified in ssSetNumSampleTimes. 217 | */ 218 | 219 | static void mdlInitializeSampleTimes(SimStruct *S) 220 | { 221 | real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0)); 222 | ssSetSampleTime(S, 0, Tsim); //DISCRETE_SAMPLE_TIME); 223 | ssSetOffsetTime(S, 0, 0.0); 224 | 225 | } 226 | 227 | 228 | 229 | #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ 230 | #if defined(MDL_INITIALIZE_CONDITIONS) 231 | /* Function: mdlInitializeConditions ======================================== 232 | * Abstract: 233 | * In this function, you should initialize the continuous and discrete 234 | * states for your S-function block. The initial states are placed 235 | * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S). 236 | * You can also perform any other initialization activities that your 237 | * S-function may require. Note, this routine will be called at the 238 | * start of simulation and if it is present in an enabled subsystem 239 | * configured to reset states, it will be call when the enabled subsystem 240 | * restarts execution to reset the states. 241 | */ 242 | static void mdlInitializeConditions(SimStruct *S) 243 | { 244 | } 245 | #endif /* MDL_INITIALIZE_CONDITIONS */ 246 | 247 | #define MDL_START /* Change to #undef to remove function */ 248 | #if defined(MDL_START) 249 | /* Function: mdlStart ======================================================= 250 | * Abstract: 251 | * This function is called once at start of model execution. If you 252 | * have states that should be initialized once, this is the place 253 | * to do it. 254 | */ 255 | 256 | static void mdlStart(SimStruct *S) 257 | { 258 | SFUNPRINTF("Creating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 259 | // init ROS if not yet done. 260 | initROS(S); 261 | 262 | void** vecPWork = ssGetPWork(S); 263 | // save nRobots in IWorkVector 264 | int_T nRobots = mxGetNumberOfElements(ssGetSFcnParam(S,2)); 265 | *ssGetIWork(S) = nRobots; 266 | 267 | ros::NodeHandle nodeHandle(ros::this_node::getName()); 268 | 269 | // get Topic Strings 270 | size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1; 271 | size_t postfix_buflen = mxGetN((ssGetSFcnParam(S, 3)))*sizeof(mxChar)+1; 272 | char* prefix_topic = (char*)mxMalloc(prefix_buflen); 273 | char* postfix_topic = (char*)mxMalloc(postfix_buflen); 274 | mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen); 275 | mxGetString((ssGetSFcnParam(S, 3)), postfix_topic, postfix_buflen); 276 | 277 | //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic); 278 | std::stringstream sstream; 279 | mxChar* robotIDs = (mxChar*)mxGetData(ssGetSFcnParam(S, 2)); 280 | for (unsigned int i = 0; i < nRobots; ++i) { 281 | sstream.str(std::string()); 282 | 283 | // build topicstring 284 | sstream << prefix_topic; 285 | sstream << (uint)robotIDs[i]; 286 | sstream << postfix_topic; 287 | 288 | GenericPublisher* pub 289 | = new GenericPublisher(nodeHandle, sstream.str(), 10); 290 | vecPWork[i] = pub; 291 | } 292 | 293 | // free char array 294 | mxFree(prefix_topic); 295 | mxFree(postfix_topic); 296 | } 297 | #endif /* MDL_START */ 298 | 299 | 300 | 301 | /* Function: mdlOutputs ======================================================= 302 | * Abstract: 303 | * In this function, you compute the outputs of your S-function 304 | * block. 305 | */ 306 | static void mdlOutputs(SimStruct *S, int_T tid) 307 | { 308 | // get Objects 309 | void** vecPWork = ssGetPWork(S); 310 | int_T nRobots = *ssGetIWork(S); 311 | 312 | // get Pointers 313 | // accessing inputs 314 | const real_T *position = (const real_T*) ssGetInputPortSignal(S,0); 315 | // const real_T *orientation = (const real_T*) ssGetInputPortSignal(S,1); 316 | 317 | geometry_msgs::Vector3Stamped msg; 318 | for (unsigned int i = 0; i < nRobots; ++i) { 319 | GenericPublisher* pub 320 | = (GenericPublisher*)vecPWork[i]; 321 | 322 | // define send Time. 323 | msg.header.stamp = ros::Time::now(); 324 | 325 | msg.vector.x = position[i*3 + 0]; 326 | msg.vector.y = position[i*3 + 1]; 327 | msg.vector.z = position[i*3 + 2]; 328 | 329 | 330 | pub->publish(msg); 331 | } 332 | 333 | } 334 | 335 | 336 | 337 | #define MDL_UPDATE /* Change to #undef to remove function */ 338 | #if defined(MDL_UPDATE) 339 | /* Function: mdlUpdate ====================================================== 340 | * Abstract: 341 | * This function is called once for every major integration time step. 342 | * Discrete states are typically updated here, but this function is useful 343 | * for performing any tasks that should only take place once per 344 | * integration step. 345 | */ 346 | static void mdlUpdate(SimStruct *S, int_T tid) 347 | { 348 | } 349 | #endif /* MDL_UPDATE */ 350 | 351 | 352 | 353 | #define MDL_DERIVATIVES /* Change to #undef to remove function */ 354 | #if defined(MDL_DERIVATIVES) 355 | /* Function: mdlDerivatives ================================================= 356 | * Abstract: 357 | * In this function, you compute the S-function block's derivatives. 358 | * The derivatives are placed in the derivative vector, ssGetdX(S). 359 | */ 360 | static void mdlDerivatives(SimStruct *S) 361 | { 362 | } 363 | #endif /* MDL_DERIVATIVES */ 364 | 365 | 366 | 367 | /* Function: mdlTerminate ===================================================== 368 | * Abstract: 369 | * In this function, you should perform any actions that are necessary 370 | * at the termination of a simulation. For example, if memory was 371 | * allocated in mdlStart, this is the place to free it. 372 | */ 373 | static void mdlTerminate(SimStruct *S) 374 | { 375 | // get Objects 376 | void** vecPWork = ssGetPWork(S); 377 | 378 | int_T nRobots = *ssGetIWork(S); 379 | for (unsigned int i = 0; i < nRobots; ++i) { 380 | GenericPublisher* pub = (GenericPublisher*)vecPWork[i]; 381 | // cleanup 382 | delete pub; 383 | } 384 | 385 | 386 | SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 387 | } 388 | 389 | 390 | /*======================================================* 391 | * See sfuntmpl_doc.c for the optional S-function methods * 392 | *======================================================*/ 393 | 394 | /*=============================* 395 | * Required S-function trailer * 396 | *=============================*/ 397 | 398 | #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ 399 | #include "simulink.c" /* MEX-file interface mechanism */ 400 | #else 401 | #include "cg_sfun.h" /* Code generation registration function */ 402 | #endif 403 | -------------------------------------------------------------------------------- /mrb_sensor_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #################################################################### 2 | # 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2012, Max-Planck-Gesellschaft 6 | # Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | #################################################################### 37 | 38 | 39 | cmake_minimum_required(VERSION 2.8.3) 40 | project(mrb_sensor_msgs) 41 | 42 | 43 | #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH}) 44 | 45 | ## Find catkin macros and libraries 46 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 47 | ## is used, also find other catkin packages 48 | find_package(catkin REQUIRED COMPONENTS 49 | matlab_ros_bridge 50 | roscpp 51 | sensor_msgs 52 | cmake_module_matlab 53 | ) 54 | 55 | find_package(Matlab REQUIRED) 56 | if(MATLAB_FOUND) 57 | include_directories(${MATLAB_INCLUDE_DIRS}) 58 | include_directories(${SIMULINK_INCLUDE_DIRS}) 59 | endif() 60 | 61 | add_definitions(-DMATLAB_MEX_FILE) 62 | 63 | ## System dependencies are found with CMake's conventions 64 | # find_package(Boost REQUIRED COMPONENTS system) 65 | 66 | 67 | ## Uncomment this if the package has a setup.py. This macro ensures 68 | ## modules and global scripts declared therein get installed 69 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 70 | # catkin_python_setup() 71 | 72 | ################################################ 73 | ## Declare ROS messages, services and actions ## 74 | ################################################ 75 | 76 | ## To declare and build messages, services or actions from within this 77 | ## package, follow these steps: 78 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 79 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 82 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 83 | ## pulled in transitively but can be declared for certainty nonetheless: 84 | ## * add a build_depend tag for "message_generation" 85 | ## * add a run_depend tag for "message_runtime" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "message_generation" and every package in MSG_DEP_SET to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * add "message_runtime" and every package in MSG_DEP_SET to 90 | ## catkin_package(CATKIN_DEPENDS ...) 91 | ## * uncomment the add_*_files sections below as needed 92 | ## and list every .msg/.srv/.action file to be processed 93 | ## * uncomment the generate_messages entry below 94 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 95 | 96 | ## Generate messages in the 'msg' folder 97 | # add_message_files( 98 | # FILES 99 | # Message1.msg 100 | # Message2.msg 101 | # ) 102 | 103 | ## Generate services in the 'srv' folder 104 | # add_service_files( 105 | # FILES 106 | # Service1.srv 107 | # Service2.srv 108 | # ) 109 | 110 | ## Generate actions in the 'action' folder 111 | # add_action_files( 112 | # FILES 113 | # Action1.action 114 | # Action2.action 115 | # ) 116 | 117 | ## Generate added messages and services with any dependencies listed here 118 | # generate_messages( 119 | # DEPENDENCIES 120 | # active_estimation_msgs# geometry_msgs# sensor_msgs# std_msgs# telekyb_msgs 121 | # ) 122 | 123 | ################################### 124 | ## catkin specific configuration ## 125 | ################################### 126 | ## The catkin_package macro generates cmake config files for your package 127 | ## Declare things to be passed to dependent projects 128 | ## INCLUDE_DIRS: uncomment this if you package contains header files 129 | ## LIBRARIES: libraries you create in this project that dependent projects also need 130 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 131 | ## DEPENDS: system dependencies of this project that dependent projects also need 132 | catkin_package( 133 | # INCLUDE_DIRS include 134 | # LIBRARIES matlab_ros_bridge 135 | # CATKIN_DEPENDS roscpp sensor_msgs matlab_ros_bridge 136 | # DEPENDS 137 | ) 138 | 139 | ########### 140 | ## Build ## 141 | ########### 142 | 143 | ## Specify additional locations of header files 144 | ## Your package locations should be listed before other locations 145 | # include_directories(include) 146 | include_directories( 147 | ${catkin_INCLUDE_DIRS} 148 | ) 149 | 150 | set(SOURCES 151 | rmsg_pub_JointState.cpp 152 | rmsg_pub_Joy.cpp 153 | rmsg_sub_Image.cpp 154 | rmsg_sub_JointState.cpp 155 | rmsg_sub_Joy.cpp 156 | rmsg_pub_PointCloud2_XYZ.cpp 157 | ) 158 | 159 | string(FIND ${CATKIN_DEVEL_PREFIX} ${PROJECT_NAME} FoundPosition REVERSE) 160 | if(FoundPosition EQUAL -1) 161 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${PROJECT_NAME}/${CATKIN_PACKAGE_LIB_DESTINATION}) 162 | else() 163 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) 164 | endif() 165 | 166 | foreach(SOURCE ${SOURCES}) 167 | string(REGEX REPLACE ".cpp" "" TARGET_NAME "${SOURCE}") 168 | message(STATUS "Adding target ${TARGET_NAME} for ${SOURCE}") 169 | add_library(${TARGET_NAME} SHARED src/${PROJECT_NAME}/${SOURCE}) 170 | set_target_properties(${TARGET_NAME} PROPERTIES PREFIX "" SUFFIX ".mexa64") 171 | set_target_properties(${TARGET_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${${PROJECT_NAME}_MEX_DESTINATION}) 172 | target_link_libraries(${TARGET_NAME} ${catkin_LIBRARIES}) 173 | install(TARGETS ${TARGET_NAME} 174 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 177 | ) 178 | endforeach(SOURCE) 179 | 180 | ## Declare a cpp executable 181 | # add_executable(matlab_ros_bridge_node src/matlab_ros_bridge_node.cpp) 182 | 183 | ## Add cmake target dependencies of the executable/library 184 | ## as an example, message headers may need to be generated before nodes 185 | # add_dependencies(matlab_ros_bridge_node matlab_ros_bridge_generate_messages_cpp) 186 | 187 | ## Specify libraries to link a library or executable target against 188 | # target_link_libraries(matlab_ros_bridge_node 189 | # ${catkin_LIBRARIES} 190 | # ) 191 | 192 | #get_property(MAKE_INFO_SOURCE_PATH GLOBAL PROPERTY MAKE_INFO_SOURCE_PATH) 193 | set(MAKE_INFO_SOURCE_PATH "'${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}'") 194 | 195 | #get_property(MAKE_INFO_INCLUDE_PATH GLOBAL PROPERTY MAKE_INFO_INCLUDE_PATH) 196 | #set(MAKE_INFO_INCLUDE_PATH "'${Boost_INCLUDE_DIRS}' '${catkin_INCLUDE_DIRS}'") 197 | set(MAKE_INFO_INCLUDE_PATH "'${catkin_INCLUDE_DIRS}'") 198 | string(REGEX REPLACE ";" "' '" MAKE_INFO_INCLUDE_PATH "${MAKE_INFO_INCLUDE_PATH}") 199 | 200 | #get_property(MAKE_INFO_LINK_LIBS_OBJS GLOBAL PROPERTY MAKE_INFO_LINK_LIBS_OBJS) 201 | 202 | foreach (catkin_LIBERARY ${catkin_LIBRARIES}) 203 | if(IS_ABSOLUTE ${catkin_LIBERARY}) 204 | set(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 205 | else(IS_ABSOLUTE ${catkin_LIBERARY}) 206 | find_library(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 207 | endif(IS_ABSOLUTE ${catkin_LIBERARY}) 208 | list(APPEND MAKE_INFO_LINK_LIBS_OBJS ${${catkin_LIBERARY}_path}) 209 | endforeach(catkin_LIBERARY) 210 | string(REGEX REPLACE ";" "' '" MAKE_INFO_LINK_LIBS_OBJS "'${MAKE_INFO_LINK_LIBS_OBJS}'") 211 | 212 | configure_file(${RTWMAKECFG_TEMPLATE_PATH} ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m) 213 | 214 | ############# 215 | ## Install ## 216 | ############# 217 | 218 | # all install targets should use catkin DESTINATION variables 219 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 220 | 221 | ## Mark executable scripts (Python etc.) for installation 222 | ## in contrast to setup.py, you can choose the destination 223 | # install(PROGRAMS 224 | # scripts/my_python_script 225 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 226 | # ) 227 | 228 | ## Mark executables and/or libraries for installation 229 | 230 | ## Mark cpp header files for installation 231 | install(DIRECTORY src/ 232 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 233 | FILES_MATCHING PATTERN "*.cpp" 234 | PATTERN ".svn" EXCLUDE 235 | PATTERN "CMakeFiles" EXCLUDE 236 | ) 237 | 238 | ## Mark other files for installation (e.g. launch and bag files, etc.) 239 | install(FILES 240 | ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m 241 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 242 | ) 243 | 244 | #install(FILES 245 | # startup_${PROJECT_NAME}.m 246 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 247 | #) 248 | 249 | ############# 250 | ## Testing ## 251 | ############# 252 | 253 | ## Add gtest based cpp test target and link libraries 254 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_matlab_ros_bridge.cpp) 255 | # if(TARGET ${PROJECT_NAME}-test) 256 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 257 | # endif() 258 | 259 | ## Add folders to be run by python nosetests 260 | # catkin_add_nosetests(test) 261 | -------------------------------------------------------------------------------- /mrb_sensor_msgs/models/sensor_msgs.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/mrb_sensor_msgs/models/sensor_msgs.slx -------------------------------------------------------------------------------- /mrb_sensor_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrb_sensor_msgs 4 | 0.0.0 5 | The matlab_ros_bridge plugin package for sensor_msgs 6 | 7 | Riccardo Spica 8 | Giovanni Claudio 9 | BSD 10 | 11 | http://wiki.ros.org/matlab_ros_bridge 12 | 13 | Riccardo Spica 14 | Martin Riedel 15 | 16 | 17 | catkin 18 | roscpp 19 | sensor_msgs 20 | matlab_ros_bridge 21 | cmake_module_matlab 22 | 23 | roscpp 24 | sensor_msgs 25 | matlab_ros_bridge 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /mrb_sensor_msgs/src/mrb_sensor_msgs/rmsg_pub_Joy.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | /* 40 | * You must specify the S_FUNCTION_NAME as the name of your S-function 41 | * (i.e. replace sfuntmpl_basic with the name of your S-function). 42 | */ 43 | 44 | #define S_FUNCTION_NAME rmsg_pub_Joy 45 | #define S_FUNCTION_LEVEL 2 46 | 47 | #define STRINGIFY(x) #x 48 | #define TOSTRING(x) STRINGIFY(x) 49 | 50 | 51 | /* 52 | * Need to include simstruc.h for the definition of the SimStruct and 53 | * its associated macro definitions. 54 | */ 55 | #include "simstruc.h" 56 | 57 | #pragma push_macro("RT") 58 | #undef RT 59 | 60 | #include 61 | 62 | // Generic Publisher 63 | #include 64 | 65 | // Message 66 | #include 67 | 68 | #pragma pop_macro("RT") 69 | 70 | #include 71 | 72 | #define CONSTANT_AXES_SIZE 8 73 | #define CONSTANT_BUTTONS_SIZE 11 74 | 75 | 76 | /* Error handling 77 | * -------------- 78 | * 79 | * You should use the following technique to report errors encountered within 80 | * an S-function: 81 | * 82 | * ssSetErrorStatus(S,"Error encountered due to ..."); 83 | * return; 84 | * 85 | * Note that the 2nd argument to ssSetErrorStatus must be persistent memory. 86 | * It cannot be a local variable. For example the following will cause 87 | * unpredictable errors: 88 | * 89 | * mdlOutputs() 90 | * { 91 | * char msg[256]; {ILLEGAL: to fix use "static char msg[256];"} 92 | * sprintf(msg,"Error due to %s", string); 93 | * ssSetErrorStatus(S,msg); 94 | * return; 95 | * } 96 | * 97 | * See matlabroot/simulink/src/sfuntmpl_doc.c for more details. 98 | */ 99 | 100 | /*====================* 101 | * S-function methods * 102 | *====================*/ 103 | 104 | 105 | //double Tsim; 106 | 107 | /* Function: mdlInitializeSizes =============================================== 108 | * Abstract: 109 | * The sizes information is used by Simulink to determine the S-function 110 | * block's characteristics (number of inputs, outputs, states, etc.). 111 | */ 112 | static void mdlInitializeSizes(SimStruct *S) 113 | { 114 | /* See sfuntmpl_doc.c for more details on the macros below */ 115 | 116 | ssSetNumSFcnParams(S, 2); /* Number of expected parameters */ 117 | if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { 118 | /* Return if number of expected != number of actual parameters */ 119 | return; 120 | } 121 | 122 | ssSetNumContStates(S, 0); 123 | ssSetNumDiscStates(S, 0); 124 | 125 | if (!ssSetNumInputPorts(S, 2)) return; 126 | ssSetInputPortWidth(S, 0, CONSTANT_AXES_SIZE); // axes 127 | ssSetInputPortWidth(S, 1, CONSTANT_BUTTONS_SIZE); // buttons 128 | ssSetInputPortDataType(S, 1, SS_INT32); 129 | 130 | for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) { 131 | /*direct input signal access*/ 132 | ssSetInputPortRequiredContiguous(S, i, true); 133 | 134 | /* 135 | * Set direct feedthrough flag (1=yes, 0=no). 136 | * A port has direct feedthrough if the input is used in either 137 | * the mdlOutputs or mdlGetTimeOfNextVarHit functions. 138 | * See matlabroot/simulink/src/sfuntmpl_directfeed.txt. 139 | */ 140 | ssSetInputPortDirectFeedThrough(S, i, 1); 141 | } 142 | 143 | if (!ssSetNumOutputPorts(S, 0)) return; 144 | 145 | ssSetNumSampleTimes(S, 1); 146 | ssSetNumRWork(S, 0); 147 | ssSetNumIWork(S, 0); 148 | ssSetNumPWork(S, 1); // GenericPub 149 | ssSetNumModes(S, 0); 150 | ssSetNumNonsampledZCs(S, 0); 151 | 152 | /* Specify the sim state compliance to be same as a built-in block */ 153 | ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 154 | 155 | ssSetOptions(S, 0); 156 | } 157 | 158 | 159 | 160 | /* Function: mdlInitializeSampleTimes ========================================= 161 | * Abstract: 162 | * This function is used to specify the sample time(s) for your 163 | * S-function. You must register the same number of sample times as 164 | * specified in ssSetNumSampleTimes. 165 | */ 166 | 167 | static void mdlInitializeSampleTimes(SimStruct *S) 168 | { 169 | real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0)); 170 | ssSetSampleTime(S, 0, Tsim); //DISCRETE_SAMPLE_TIME); 171 | ssSetOffsetTime(S, 0, 0.0); 172 | 173 | } 174 | 175 | 176 | 177 | #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ 178 | #if defined(MDL_INITIALIZE_CONDITIONS) 179 | /* Function: mdlInitializeConditions ======================================== 180 | * Abstract: 181 | * In this function, you should initialize the continuous and discrete 182 | * states for your S-function block. The initial states are placed 183 | * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S). 184 | * You can also perform any other initialization activities that your 185 | * S-function may require. Note, this routine will be called at the 186 | * start of simulation and if it is present in an enabled subsystem 187 | * configured to reset states, it will be call when the enabled subsystem 188 | * restarts execution to reset the states. 189 | */ 190 | static void mdlInitializeConditions(SimStruct *S) 191 | { 192 | } 193 | #endif /* MDL_INITIALIZE_CONDITIONS */ 194 | 195 | 196 | #define MDL_START /* Change to #undef to remove function */ 197 | #if defined(MDL_START) 198 | /* Function: mdlStart ======================================================= 199 | * Abstract: 200 | * This function is called once at start of model execution. If you 201 | * have states that should be initialized once, this is the place 202 | * to do it. 203 | */ 204 | 205 | static void mdlStart(SimStruct *S) 206 | { 207 | SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 208 | // init ROS if not yet done. 209 | initROS(S); 210 | 211 | void** vecPWork = ssGetPWork(S); 212 | 213 | ros::NodeHandle nodeHandle(ros::this_node::getName()); 214 | 215 | // get String 216 | size_t buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1; 217 | char* topic = (char*)mxMalloc(buflen); 218 | size_t status = mxGetString((ssGetSFcnParam(S, 1)), topic, buflen); 219 | 220 | //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic); 221 | 222 | GenericPublisher* pub = new GenericPublisher(nodeHandle, std::string(topic), 10); 223 | vecPWork[0] = pub; 224 | //*sub = n.subscribe(std::string(topic), 1, msgCallback); 225 | // free char array 226 | mxFree(topic); 227 | } 228 | #endif /* MDL_START */ 229 | 230 | 231 | 232 | /* Function: mdlOutputs ======================================================= 233 | * Abstract: 234 | * In this function, you compute the outputs of your S-function 235 | * block. 236 | */ 237 | static void mdlOutputs(SimStruct *S, int_T tid) 238 | { 239 | // get Objects 240 | void** vecPWork = ssGetPWork(S); 241 | GenericPublisher* pub 242 | = (GenericPublisher*)vecPWork[0]; 243 | 244 | // accessing inputs 245 | const real_T *axes = (const real_T*) ssGetInputPortSignal(S,0); 246 | const int32_T *buttons = (const int32_T*) ssGetInputPortSignal(S,1); 247 | 248 | // copy 249 | sensor_msgs::Joy msg; 250 | 251 | // define send Time. 252 | msg.header.stamp = ros::Time::now(); 253 | 254 | msg.axes.resize(CONSTANT_AXES_SIZE); 255 | msg.buttons.resize(CONSTANT_BUTTONS_SIZE); 256 | 257 | // axes 258 | for (unsigned int i=0; i < CONSTANT_AXES_SIZE; i++) 259 | { 260 | msg.axes[i] = axes[i]; 261 | } 262 | 263 | // buttons 264 | for (unsigned int i=0; i < CONSTANT_BUTTONS_SIZE; i++) 265 | { 266 | msg.buttons[i] = buttons[i]; 267 | } 268 | 269 | pub->publish(msg); 270 | } 271 | 272 | 273 | 274 | #define MDL_UPDATE /* Change to #undef to remove function */ 275 | #if defined(MDL_UPDATE) 276 | /* Function: mdlUpdate ====================================================== 277 | * Abstract: 278 | * This function is called once for every major integration time step. 279 | * Discrete states are typically updated here, but this function is useful 280 | * for performing any tasks that should only take place once per 281 | * integration step. 282 | */ 283 | static void mdlUpdate(SimStruct *S, int_T tid) 284 | { 285 | } 286 | #endif /* MDL_UPDATE */ 287 | 288 | 289 | 290 | #define MDL_DERIVATIVES /* Change to #undef to remove function */ 291 | #if defined(MDL_DERIVATIVES) 292 | /* Function: mdlDerivatives ================================================= 293 | * Abstract: 294 | * In this function, you compute the S-function block's derivatives. 295 | * The derivatives are placed in the derivative vector, ssGetdX(S). 296 | */ 297 | static void mdlDerivatives(SimStruct *S) 298 | { 299 | } 300 | #endif /* MDL_DERIVATIVES */ 301 | 302 | 303 | 304 | /* Function: mdlTerminate ===================================================== 305 | * Abstract: 306 | * In this function, you should perform any actions that are necessary 307 | * at the termination of a simulation. For example, if memory was 308 | * allocated in mdlStart, this is the place to free it. 309 | */ 310 | static void mdlTerminate(SimStruct *S) 311 | { 312 | // get Objects 313 | void** vecPWork = ssGetPWork(S); 314 | GenericPublisher* pub = (GenericPublisher*)vecPWork[0]; 315 | 316 | // cleanup 317 | delete pub; 318 | 319 | SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 320 | } 321 | 322 | 323 | /*======================================================* 324 | * See sfuntmpl_doc.c for the optional S-function methods * 325 | *======================================================*/ 326 | 327 | /*=============================* 328 | * Required S-function trailer * 329 | *=============================*/ 330 | 331 | #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ 332 | #include "simulink.c" /* MEX-file interface mechanism */ 333 | #else 334 | #include "cg_sfun.h" /* Code generation registration function */ 335 | #endif 336 | -------------------------------------------------------------------------------- /mrb_sensor_msgs/src/mrb_sensor_msgs/rmsg_sub_Joy.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | /* 40 | * You must specify the S_FUNCTION_NAME as the name of your S-function 41 | * (i.e. replace sfuntmpl_basic with the name of your S-function). 42 | */ 43 | 44 | #define S_FUNCTION_NAME rmsg_sub_Joy 45 | #define S_FUNCTION_LEVEL 2 46 | 47 | #define STRINGIFY(x) #x 48 | #define TOSTRING(x) STRINGIFY(x) 49 | 50 | 51 | /* 52 | * Need to include simstruc.h for the definition of the SimStruct and 53 | * its associated macro definitions. 54 | */ 55 | #include "simstruc.h" 56 | 57 | #pragma push_macro("RT") 58 | #undef RT 59 | 60 | #include 61 | 62 | // Generic Subscriber 63 | #include 64 | 65 | // Message 66 | #include 67 | 68 | #pragma pop_macro("RT") 69 | 70 | #include 71 | 72 | 73 | #define CONSTANT_AXES_SIZE 8 74 | #define CONSTANT_BUTTONS_SIZE 11 75 | 76 | 77 | /* Error handling 78 | * -------------- 79 | * 80 | * You should use the following technique to report errors encountered within 81 | * an S-function: 82 | * 83 | * ssSetErrorStatus(S,"Error encountered due to ..."); 84 | * return; 85 | * 86 | * Note that the 2nd argument to ssSetErrorStatus must be persistent memory. 87 | * It cannot be a local variable. For example the following will cause 88 | * unpredictable errors: 89 | * 90 | * mdlOutputs() 91 | * { 92 | * char msg[256]; {ILLEGAL: to fix use "static char msg[256];"} 93 | * sprintf(msg,"Error due to %s", string); 94 | * ssSetErrorStatus(S,msg); 95 | * return; 96 | * } 97 | * 98 | * See matlabroot/simulink/src/sfuntmpl_doc.c for more details. 99 | */ 100 | 101 | /*====================* 102 | * S-function methods * 103 | *====================*/ 104 | 105 | 106 | //double Tsim; 107 | 108 | /* Function: mdlInitializeSizes =============================================== 109 | * Abstract: 110 | * The sizes information is used by Simulink to determine the S-function 111 | * block's characteristics (number of inputs, outputs, states, etc.). 112 | */ 113 | static void mdlInitializeSizes(SimStruct *S) 114 | { 115 | /* See sfuntmpl_doc.c for more details on the macros below */ 116 | 117 | ssSetNumSFcnParams(S, 2); /* Number of expected parameters */ 118 | if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { 119 | /* Return if number of expected != number of actual parameters */ 120 | return; 121 | } 122 | 123 | ssSetNumContStates(S, 0); 124 | ssSetNumDiscStates(S, 0); 125 | 126 | if (!ssSetNumInputPorts(S, 0)) return; 127 | //ssSetInputPortWidth(S, 0, 1); 128 | //ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/ 129 | /* 130 | * Set direct feedthrough flag (1=yes, 0=no). 131 | * A port has direct feedthrough if the input is used in either 132 | * the mdlOutputs or mdlGetTimeOfNextVarHit functions. 133 | * See matlabroot/simulink/src/sfuntmpl_directfeed.txt. 134 | */ 135 | //ssSetInputPortDirectFeedThrough(S, 0, 1); 136 | 137 | if (!ssSetNumOutputPorts(S, 3)) return; 138 | ssSetOutputPortWidth(S, 0, CONSTANT_AXES_SIZE); // axes 139 | ssSetOutputPortWidth(S, 1, CONSTANT_BUTTONS_SIZE); // buttons 140 | ssSetOutputPortDataType(S, 1, SS_INT32); 141 | ssSetOutputPortWidth(S, 2, 1); // time 142 | 143 | ssSetNumSampleTimes(S, 1); 144 | ssSetNumRWork(S, 0); 145 | ssSetNumIWork(S, 0); 146 | ssSetNumPWork(S, 2); //AsyncSpinner, GenericSub 147 | ssSetNumModes(S, 0); 148 | ssSetNumNonsampledZCs(S, 0); 149 | 150 | /* Specify the sim state compliance to be same as a built-in block */ 151 | ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 152 | 153 | ssSetOptions(S, 0); 154 | } 155 | 156 | 157 | 158 | /* Function: mdlInitializeSampleTimes ========================================= 159 | * Abstract: 160 | * This function is used to specify the sample time(s) for your 161 | * S-function. You must register the same number of sample times as 162 | * specified in ssSetNumSampleTimes. 163 | */ 164 | 165 | static void mdlInitializeSampleTimes(SimStruct *S) 166 | { 167 | real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0)); 168 | ssSetSampleTime(S, 0, Tsim); //DISCRETE_SAMPLE_TIME); 169 | ssSetOffsetTime(S, 0, 0.0); 170 | 171 | } 172 | 173 | 174 | 175 | #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ 176 | #if defined(MDL_INITIALIZE_CONDITIONS) 177 | /* Function: mdlInitializeConditions ======================================== 178 | * Abstract: 179 | * In this function, you should initialize the continuous and discrete 180 | * states for your S-function block. The initial states are placed 181 | * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S). 182 | * You can also perform any other initialization activities that your 183 | * S-function may require. Note, this routine will be called at the 184 | * start of simulation and if it is present in an enabled subsystem 185 | * configured to reset states, it will be call when the enabled subsystem 186 | * restarts execution to reset the states. 187 | */ 188 | static void mdlInitializeConditions(SimStruct *S) 189 | { 190 | } 191 | #endif /* MDL_INITIALIZE_CONDITIONS */ 192 | 193 | #define MDL_START /* Change to #undef to remove function */ 194 | #if defined(MDL_START) 195 | /* Function: mdlStart ======================================================= 196 | * Abstract: 197 | * This function is called once at start of model execution. If you 198 | * have states that should be initialized once, this is the place 199 | * to do it. 200 | */ 201 | 202 | static void mdlStart(SimStruct *S) 203 | { 204 | SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 205 | // init ROS if not yet done. 206 | initROS(S); 207 | 208 | void** vecPWork = ssGetPWork(S); 209 | ros::AsyncSpinner* spinner = new ros::AsyncSpinner(1); // Spinner 210 | spinner->start(); 211 | vecPWork[0] = spinner; 212 | 213 | ros::NodeHandle nodeHandle(ros::this_node::getName()); 214 | 215 | 216 | // get String 217 | size_t buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1; 218 | char* topic = (char*)mxMalloc(buflen); 219 | size_t status = mxGetString((ssGetSFcnParam(S, 1)), topic, buflen); 220 | 221 | //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic); 222 | 223 | GenericSubscriber* sub = new GenericSubscriber(nodeHandle, std::string(topic), 100); 224 | vecPWork[1] = sub; 225 | //*sub = n.subscribe(std::string(topic), 1, msgCallback); 226 | // free char array 227 | mxFree(topic); 228 | } 229 | #endif /* MDL_START */ 230 | 231 | 232 | 233 | /* Function: mdlOutputs ======================================================= 234 | * Abstract: 235 | * In this function, you compute the outputs of your S-function 236 | * block. 237 | */ 238 | static void mdlOutputs(SimStruct *S, int_T tid) 239 | { 240 | // get Objects 241 | void** vecPWork = ssGetPWork(S); 242 | GenericSubscriber* sub = (GenericSubscriber*)vecPWork[1]; 243 | 244 | // Preparing Outputs 245 | real_T* axes = (real_T*)ssGetOutputPortSignal(S,0); 246 | int32_T* buttons = (int32_T*)ssGetOutputPortSignal(S,1); 247 | real_T *time = (real_T*)ssGetOutputPortSignal(S,2); 248 | 249 | sensor_msgs::JoyConstPtr lastMsg = sub->getLastMsg(); 250 | if (lastMsg){ // copy only if a message was actually received 251 | 252 | // axes 253 | for (unsigned int i=0; i < lastMsg->axes.size() && i < CONSTANT_AXES_SIZE; i++) { 254 | axes[i] = lastMsg->axes[i]; 255 | } 256 | // do we have to fill in the rest? 257 | 258 | // buttons 259 | for (unsigned int i=0; i < lastMsg->buttons.size() && i < CONSTANT_BUTTONS_SIZE; i++) { 260 | buttons[i] = lastMsg->buttons[i]; 261 | } 262 | 263 | 264 | time[0] = lastMsg->header.stamp.toSec(); 265 | 266 | } 267 | } 268 | 269 | 270 | 271 | #define MDL_UPDATE /* Change to #undef to remove function */ 272 | #if defined(MDL_UPDATE) 273 | /* Function: mdlUpdate ====================================================== 274 | * Abstract: 275 | * This function is called once for every major integration time step. 276 | * Discrete states are typically updated here, but this function is useful 277 | * for performing any tasks that should only take place once per 278 | * integration step. 279 | */ 280 | static void mdlUpdate(SimStruct *S, int_T tid) 281 | { 282 | } 283 | #endif /* MDL_UPDATE */ 284 | 285 | 286 | 287 | #define MDL_DERIVATIVES /* Change to #undef to remove function */ 288 | #if defined(MDL_DERIVATIVES) 289 | /* Function: mdlDerivatives ================================================= 290 | * Abstract: 291 | * In this function, you compute the S-function block's derivatives. 292 | * The derivatives are placed in the derivative vector, ssGetdX(S). 293 | */ 294 | static void mdlDerivatives(SimStruct *S) 295 | { 296 | } 297 | #endif /* MDL_DERIVATIVES */ 298 | 299 | 300 | 301 | /* Function: mdlTerminate ===================================================== 302 | * Abstract: 303 | * In this function, you should perform any actions that are necessary 304 | * at the termination of a simulation. For example, if memory was 305 | * allocated in mdlStart, this is the place to free it. 306 | */ 307 | static void mdlTerminate(SimStruct *S) 308 | { 309 | // get Objects 310 | void** vecPWork = ssGetPWork(S); 311 | ros::AsyncSpinner* spinner = (ros::AsyncSpinner*)vecPWork[0]; 312 | GenericSubscriber* sub = (GenericSubscriber*)vecPWork[1]; 313 | 314 | // cleanup 315 | delete spinner; 316 | delete sub; 317 | 318 | SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 319 | } 320 | 321 | 322 | /*======================================================* 323 | * See sfuntmpl_doc.c for the optional S-function methods * 324 | *======================================================*/ 325 | 326 | /*=============================* 327 | * Required S-function trailer * 328 | *=============================*/ 329 | 330 | #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ 331 | #include "simulink.c" /* MEX-file interface mechanism */ 332 | #else 333 | #include "cg_sfun.h" /* Code generation registration function */ 334 | #endif 335 | -------------------------------------------------------------------------------- /mrb_shape_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #################################################################### 2 | # 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2012, Max-Planck-Gesellschaft 6 | # Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | #################################################################### 37 | 38 | 39 | cmake_minimum_required(VERSION 2.8.3) 40 | project(mrb_shape_msgs) 41 | 42 | 43 | #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH}) 44 | 45 | ## Find catkin macros and libraries 46 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 47 | ## is used, also find other catkin packages 48 | find_package(catkin REQUIRED COMPONENTS 49 | matlab_ros_bridge 50 | roscpp 51 | shape_msgs 52 | cmake_module_matlab 53 | ) 54 | 55 | find_package(Matlab REQUIRED) 56 | if(MATLAB_FOUND) 57 | include_directories(${MATLAB_INCLUDE_DIRS}) 58 | include_directories(${SIMULINK_INCLUDE_DIRS}) 59 | endif() 60 | 61 | add_definitions(-DMATLAB_MEX_FILE) 62 | 63 | ## System dependencies are found with CMake's conventions 64 | # find_package(Boost REQUIRED COMPONENTS system) 65 | 66 | 67 | ## Uncomment this if the package has a setup.py. This macro ensures 68 | ## modules and global scripts declared therein get installed 69 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 70 | # catkin_python_setup() 71 | 72 | ################################################ 73 | ## Declare ROS messages, services and actions ## 74 | ################################################ 75 | 76 | ## To declare and build messages, services or actions from within this 77 | ## package, follow these steps: 78 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 79 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 82 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 83 | ## pulled in transitively but can be declared for certainty nonetheless: 84 | ## * add a build_depend tag for "message_generation" 85 | ## * add a run_depend tag for "message_runtime" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "message_generation" and every package in MSG_DEP_SET to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * add "message_runtime" and every package in MSG_DEP_SET to 90 | ## catkin_package(CATKIN_DEPENDS ...) 91 | ## * uncomment the add_*_files sections below as needed 92 | ## and list every .msg/.srv/.action file to be processed 93 | ## * uncomment the generate_messages entry below 94 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 95 | 96 | ## Generate messages in the 'msg' folder 97 | # add_message_files( 98 | # FILES 99 | # Message1.msg 100 | # Message2.msg 101 | # ) 102 | 103 | ## Generate services in the 'srv' folder 104 | # add_service_files( 105 | # FILES 106 | # Service1.srv 107 | # Service2.srv 108 | # ) 109 | 110 | ## Generate actions in the 'action' folder 111 | # add_action_files( 112 | # FILES 113 | # Action1.action 114 | # Action2.action 115 | # ) 116 | 117 | ## Generate added messages and services with any dependencies listed here 118 | # generate_messages( 119 | # DEPENDENCIES 120 | # ) 121 | 122 | ################################### 123 | ## catkin specific configuration ## 124 | ################################### 125 | ## The catkin_package macro generates cmake config files for your package 126 | ## Declare things to be passed to dependent projects 127 | ## INCLUDE_DIRS: uncomment this if you package contains header files 128 | ## LIBRARIES: libraries you create in this project that dependent projects also need 129 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 130 | ## DEPENDS: system dependencies of this project that dependent projects also need 131 | catkin_package( 132 | # INCLUDE_DIRS include 133 | # LIBRARIES matlab_ros_bridge 134 | # CATKIN_DEPENDS roscpp shape_msgs matlab_ros_bridge 135 | # DEPENDS boost Matlab 136 | ) 137 | 138 | ########### 139 | ## Build ## 140 | ########### 141 | 142 | ## Specify additional locations of header files 143 | ## Your package locations should be listed before other locations 144 | # include_directories(include) 145 | include_directories( 146 | ${catkin_INCLUDE_DIRS} 147 | ) 148 | 149 | set(SOURCES 150 | rmsg_pub_Mesh.cpp 151 | ) 152 | 153 | string(FIND ${CATKIN_DEVEL_PREFIX} ${PROJECT_NAME} FoundPosition REVERSE) 154 | if(FoundPosition EQUAL -1) 155 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${PROJECT_NAME}/${CATKIN_PACKAGE_LIB_DESTINATION}) 156 | else() 157 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) 158 | endif() 159 | 160 | foreach(SOURCE ${SOURCES}) 161 | string(REGEX REPLACE ".cpp" "" TARGET_NAME "${SOURCE}") 162 | message(STATUS "Adding target ${TARGET_NAME} for ${SOURCE}") 163 | add_library(${TARGET_NAME} SHARED src/${PROJECT_NAME}/${SOURCE}) 164 | set_target_properties(${TARGET_NAME} PROPERTIES PREFIX "" SUFFIX ".mexa64") 165 | set_target_properties(${TARGET_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${${PROJECT_NAME}_MEX_DESTINATION}) 166 | target_link_libraries(${TARGET_NAME} ${catkin_LIBRARIES}) 167 | install(TARGETS ${TARGET_NAME} 168 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | ) 172 | endforeach(SOURCE) 173 | 174 | ## Declare a cpp executable 175 | # add_executable(matlab_ros_bridge_node src/matlab_ros_bridge_node.cpp) 176 | 177 | ## Add cmake target dependencies of the executable/library 178 | ## as an example, message headers may need to be generated before nodes 179 | # add_dependencies(matlab_ros_bridge_node matlab_ros_bridge_generate_messages_cpp) 180 | 181 | ## Specify libraries to link a library or executable target against 182 | # target_link_libraries(matlab_ros_bridge_node 183 | # ${catkin_LIBRARIES} 184 | # ) 185 | 186 | #get_property(MAKE_INFO_SOURCE_PATH GLOBAL PROPERTY MAKE_INFO_SOURCE_PATH) 187 | set(MAKE_INFO_SOURCE_PATH "'${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}'") 188 | 189 | #get_property(MAKE_INFO_INCLUDE_PATH GLOBAL PROPERTY MAKE_INFO_INCLUDE_PATH) 190 | #set(MAKE_INFO_INCLUDE_PATH "'${Boost_INCLUDE_DIRS}' '${catkin_INCLUDE_DIRS}'") 191 | set(MAKE_INFO_INCLUDE_PATH "'${catkin_INCLUDE_DIRS}'") 192 | string(REGEX REPLACE ";" "' '" MAKE_INFO_INCLUDE_PATH "${MAKE_INFO_INCLUDE_PATH}") 193 | 194 | #get_property(MAKE_INFO_LINK_LIBS_OBJS GLOBAL PROPERTY MAKE_INFO_LINK_LIBS_OBJS) 195 | 196 | foreach (catkin_LIBERARY ${catkin_LIBRARIES}) 197 | if(IS_ABSOLUTE ${catkin_LIBERARY}) 198 | set(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 199 | else(IS_ABSOLUTE ${catkin_LIBERARY}) 200 | find_library(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 201 | endif(IS_ABSOLUTE ${catkin_LIBERARY}) 202 | list(APPEND MAKE_INFO_LINK_LIBS_OBJS ${${catkin_LIBERARY}_path}) 203 | endforeach(catkin_LIBERARY) 204 | string(REGEX REPLACE ";" "' '" MAKE_INFO_LINK_LIBS_OBJS "'${MAKE_INFO_LINK_LIBS_OBJS}'") 205 | 206 | configure_file(${RTWMAKECFG_TEMPLATE_PATH} ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m) 207 | 208 | ############# 209 | ## Install ## 210 | ############# 211 | 212 | # all install targets should use catkin DESTINATION variables 213 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 214 | 215 | ## Mark executable scripts (Python etc.) for installation 216 | ## in contrast to setup.py, you can choose the destination 217 | # install(PROGRAMS 218 | # scripts/my_python_script 219 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 220 | # ) 221 | 222 | ## Mark executables and/or libraries for installation 223 | 224 | ## Mark cpp header files for installation 225 | install(DIRECTORY src/ 226 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 227 | FILES_MATCHING PATTERN "*.cpp" 228 | PATTERN ".svn" EXCLUDE 229 | PATTERN "CMakeFiles" EXCLUDE 230 | ) 231 | 232 | ## Mark other files for installation (e.g. launch and bag files, etc.) 233 | install(FILES 234 | ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m 235 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 236 | ) 237 | 238 | #install(FILES 239 | # startup_${PROJECT_NAME}.m 240 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 241 | #) 242 | 243 | ############# 244 | ## Testing ## 245 | ############# 246 | 247 | ## Add gtest based cpp test target and link libraries 248 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_matlab_ros_bridge.cpp) 249 | # if(TARGET ${PROJECT_NAME}-test) 250 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 251 | # endif() 252 | 253 | ## Add folders to be run by python nosetests 254 | # catkin_add_nosetests(test) 255 | -------------------------------------------------------------------------------- /mrb_shape_msgs/models/shape_msgs.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/mrb_shape_msgs/models/shape_msgs.slx -------------------------------------------------------------------------------- /mrb_shape_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrb_shape_msgs 4 | 0.0.0 5 | The matlab_ros_bridge plugin package for shape_msgs 6 | 7 | Riccardo Spica 8 | Giovanni Claudio 9 | BSD 10 | 11 | http://wiki.ros.org/matlab_ros_bridge 12 | 13 | Riccardo Spica 14 | 15 | catkin 16 | roscpp 17 | shape_msgs 18 | matlab_ros_bridge 19 | cmake_module_matlab 20 | 21 | roscpp 22 | shape_msgs 23 | matlab_ros_bridge 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /mrb_shape_msgs/src/mrb_shape_msgs/rmsg_pub_Mesh.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | /* 39 | * You must specify the S_FUNCTION_NAME as the name of your S-function 40 | * (i.e. replace sfuntmpl_basic with the name of your S-function). 41 | */ 42 | 43 | #define S_FUNCTION_NAME rmsg_pub_Mesh 44 | #define S_FUNCTION_LEVEL 2 45 | 46 | #define STRINGIFY(x) #x 47 | #define TOSTRING(x) STRINGIFY(x) 48 | 49 | /* 50 | * Need to include simstruc.h for the definition of the SimStruct and 51 | * its associated macro definitions. 52 | */ 53 | #include "simstruc.h" 54 | 55 | #pragma push_macro("RT") 56 | #undef RT 57 | 58 | #include 59 | 60 | // Generic Publisher 61 | #include 62 | 63 | // Message 64 | #include 65 | 66 | #pragma pop_macro("RT") 67 | 68 | #include 69 | 70 | #define MDL_CHECK_PARAMETERS 71 | #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) 72 | /* Function: mdlCheckParameters ============================================= 73 | * Abstract: 74 | * Validate our parameters to verify: 75 | * o The numerator must be of a lower order than the denominator. 76 | * o The sample time must be a real positive nonzero value. 77 | */ 78 | static void mdlCheckParameters(SimStruct *S) { 79 | // SFUNPRINTF("Calling mdlCheckParameters"); 80 | 81 | // Tsim 82 | if (mxIsEmpty(ssGetSFcnParam(S, 0)) || mxIsSparse(ssGetSFcnParam(S, 0)) 83 | || mxIsComplex(ssGetSFcnParam(S, 0)) 84 | || mxIsLogical(ssGetSFcnParam(S, 0)) 85 | || !mxIsNumeric(ssGetSFcnParam(S, 0)) 86 | || !mxIsDouble(ssGetSFcnParam(S, 0)) 87 | || mxGetNumberOfElements(ssGetSFcnParam(S, 0)) != 1) { 88 | ssSetErrorStatus(S, "Simulation time must be a single double Value"); 89 | return; 90 | } 91 | 92 | // Topic 93 | if (!mxIsChar(ssGetSFcnParam(S, 1))) { 94 | ssSetErrorStatus(S, "Topic value must be char array (string)"); 95 | return; 96 | } 97 | 98 | } 99 | #endif /* MDL_CHECK_PARAMETERS */ 100 | 101 | /* Error handling 102 | * -------------- 103 | * 104 | * You should use the following technique to report errors encountered within 105 | * an S-function: 106 | * 107 | * ssSetErrorStatus(S,"Error encountered due to ..."); 108 | * return; 109 | * 110 | * Note that the 2nd argument to ssSetErrorStatus must be persistent memory. 111 | * It cannot be a local variable. For example the following will cause 112 | * unpredictable errors: 113 | * 114 | * mdlOutputs() 115 | * { 116 | * char msg[256]; {ILLEGAL: to fix use "static char msg[256];"} 117 | * sprintf(msg,"Error due to %s", string); 118 | * ssSetErrorStatus(S,msg); 119 | * return; 120 | * } 121 | * 122 | * See matlabroot/simulink/src/sfuntmpl_doc.c for more details. 123 | */ 124 | 125 | /*====================* 126 | * S-function methods * 127 | *====================*/ 128 | 129 | //double Tsim; 130 | /* Function: mdlInitializeSizes =============================================== 131 | * Abstract: 132 | * The sizes information is used by Simulink to determine the S-function 133 | * block's characteristics (number of inputs, outputs, states, etc.). 134 | */ 135 | static void mdlInitializeSizes(SimStruct *S) { 136 | /* See sfuntmpl_doc.c for more details on the macros below */ 137 | 138 | ssSetNumSFcnParams(S, 2); /* Number of expected parameters */ 139 | #if defined(MATLAB_MEX_FILE) 140 | if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { 141 | mdlCheckParameters(S); 142 | if (ssGetErrorStatus(S) != NULL) { 143 | return; 144 | } 145 | } else { 146 | return; /* Parameter mismatch will be reported by Simulink. */ 147 | } 148 | #endif 149 | 150 | ssSetNumContStates(S, 0); 151 | ssSetNumDiscStates(S, 0); 152 | 153 | if (!ssSetNumInputPorts(S, 2)) 154 | return; 155 | 156 | ssSetInputPortMatrixDimensions(S, 0, DYNAMICALLY_SIZED, DYNAMICALLY_SIZED); // triangles 157 | ssSetInputPortDataType(S, 0, SS_UINT32); 158 | ssSetInputPortMatrixDimensions(S, 1, DYNAMICALLY_SIZED, DYNAMICALLY_SIZED); // vertices 159 | 160 | for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) { 161 | /*direct input signal access*/ 162 | ssSetInputPortRequiredContiguous(S, i, true); 163 | 164 | /* 165 | * Set direct feedthrough flag (1=yes, 0=no). 166 | * A port has direct feedthrough if the input is used in either 167 | * the mdlOutputs or mdlGetTimeOfNextVarHit functions. 168 | * See matlabroot/simulink/src/sfuntmpl_directfeed.txt. 169 | */ 170 | ssSetInputPortDirectFeedThrough(S, i, 1); 171 | } 172 | 173 | if (!ssSetNumOutputPorts(S, 0)) 174 | return; 175 | 176 | ssSetNumSampleTimes(S, 1); 177 | ssSetNumRWork(S, 0); 178 | ssSetNumIWork(S, 0); 179 | ssSetNumPWork(S, 2); //GenericPub and frame id 180 | ssSetNumModes(S, 0); 181 | ssSetNumNonsampledZCs(S, 0); 182 | 183 | /* Specify the sim state compliance to be same as a built-in block */ 184 | ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 185 | 186 | ssSetOptions(S, 0); 187 | } 188 | 189 | #if defined(MATLAB_MEX_FILE) 190 | #define MDL_SET_INPUT_PORT_DIMENSION_INFO 191 | static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, 192 | const DimsInfo_T *dimsInfo) { 193 | 194 | if (dimsInfo->numDims != 2 || dimsInfo->dims[0] != 3) { 195 | std::stringstream ss; 196 | ss << "Input port " << port << " should be a 3xN matrix."; 197 | ssSetErrorStatus(S, ss.str().c_str()); 198 | return; 199 | } 200 | 201 | if (!ssSetInputPortDimensionInfo(S, port, dimsInfo)) 202 | return; 203 | 204 | } 205 | 206 | # define MDL_SET_DEFAULT_PORT_DIMENSION_INFO 207 | /* Function: mdlSetDefaultPortDimensionInfo =========================================== 208 | * Abstract: 209 | * In case no ports were specified, the default is an input port of width 2 210 | * and an output port of width 1. 211 | */ 212 | static void mdlSetDefaultPortDimensionInfo(SimStruct *S) { 213 | ssSetInputPortMatrixDimensions(S, 0, 3, 1); 214 | ssSetInputPortMatrixDimensions(S, 1, 3, 3); 215 | } 216 | #endif 217 | 218 | /* Function: mdlInitializeSampleTimes ========================================= 219 | * Abstract: 220 | * This function is used to specify the sample time(s) for your 221 | * S-function. You must register the same number of sample times as 222 | * specified in ssSetNumSampleTimes. 223 | */ 224 | 225 | static void mdlInitializeSampleTimes(SimStruct *S) { 226 | real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0)); 227 | ssSetSampleTime(S, 0, Tsim); //DISCRETE_SAMPLE_TIME); 228 | ssSetOffsetTime(S, 0, 0.0); 229 | } 230 | 231 | #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ 232 | #if defined(MDL_INITIALIZE_CONDITIONS) 233 | /* Function: mdlInitializeConditions ======================================== 234 | * Abstract: 235 | * In this function, you should initialize the continuous and discrete 236 | * states for your S-function block. The initial states are placed 237 | * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S). 238 | * You can also perform any other initialization activities that your 239 | * S-function may require. Note, this routine will be called at the 240 | * start of simulation and if it is present in an enabled subsystem 241 | * configured to reset states, it will be call when the enabled subsystem 242 | * restarts execution to reset the states. 243 | */ 244 | static void mdlInitializeConditions(SimStruct *S) { 245 | } 246 | #endif /* MDL_INITIALIZE_CONDITIONS */ 247 | 248 | #define MDL_START /* Change to #undef to remove function */ 249 | #if defined(MDL_START) 250 | /* Function: mdlStart ======================================================= 251 | * Abstract: 252 | * This function is called once at start of model execution. If you 253 | * have states that should be initialized once, this is the place 254 | * to do it. 255 | */ 256 | 257 | static void mdlStart(SimStruct *S) { 258 | SFUNPRINTF("Creating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 259 | // init ROS if not yet done. 260 | initROS(S); 261 | 262 | void** vecPWork = ssGetPWork(S); 263 | 264 | ros::NodeHandle nodeHandle(ros::this_node::getName()); 265 | 266 | // get Topic Strings 267 | size_t buflen = mxGetN((ssGetSFcnParam(S, 1))) * sizeof(mxChar) + 1; 268 | char* topic = (char*) mxMalloc(buflen); 269 | mxGetString((ssGetSFcnParam(S, 1)), topic, buflen); 270 | 271 | //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic); 272 | GenericPublisher* pub = new GenericPublisher< 273 | shape_msgs::Mesh>(nodeHandle, topic, 10); 274 | vecPWork[0] = pub; 275 | 276 | mxFree(topic); 277 | 278 | } 279 | 280 | #endif /* MDL_START */ 281 | 282 | /* Function: mdlOutputs ======================================================= 283 | * Abstract: 284 | * In this function, you compute the outputs of your S-function 285 | * block. 286 | */ 287 | static void mdlOutputs(SimStruct *S, int_T tid) { 288 | // get Objects 289 | void** vecPWork = ssGetPWork(S); 290 | 291 | // get Pointers 292 | // accessing inputs 293 | const uint32_T *idx = (const uint32_T*) ssGetInputPortSignal(S, 0); 294 | const real_T *pts = (const real_T*) ssGetInputPortSignal(S, 1); 295 | 296 | if (ssGetInputPortNumDimensions(S, 0) != 2 || ssGetInputPortNumDimensions(S, 1) != 2) { 297 | ssSetErrorStatus(S, 298 | "Wrong number of dimensions. This should never happen!."); 299 | return; 300 | } 301 | 302 | #if defined(MATLAB_MEX_FILE) 303 | const int_T nTriangles = ssGetCurrentInputPortDimensions(S, 0, 1); 304 | const int_T nPts = ssGetCurrentInputPortDimensions(S, 1, 1); 305 | #else 306 | const int_T nTriangles = ssGetInputPortDimensions(S, 0)[1]; 307 | const int_T nPts = ssGetInputPortDimensions(S, 1)[1]; 308 | #endif 309 | 310 | GenericPublisher* pub = 311 | (GenericPublisher*) vecPWork[0]; 312 | 313 | const std::string* topic = (const std::string*) vecPWork[1]; 314 | 315 | shape_msgs::Mesh msg; 316 | msg.triangles.resize(nTriangles); 317 | msg.vertices.resize(nPts); 318 | 319 | for (int_T i = 0; i < 3; ++i) { 320 | for (int_T j = 0; j < nTriangles; ++j) { 321 | msg.triangles[j].vertex_indices[i] = idx[3 * j + i]; 322 | } 323 | } 324 | 325 | for (int_T j = 0; j < nPts; ++j) { 326 | msg.vertices[j].x = pts[3 * j + 0]; 327 | msg.vertices[j].y = pts[3 * j + 1]; 328 | msg.vertices[j].z = pts[3 * j + 2]; 329 | } 330 | 331 | pub->publish(msg); 332 | 333 | } 334 | 335 | #define MDL_UPDATE /* Change to #undef to remove function */ 336 | #if defined(MDL_UPDATE) 337 | /* Function: mdlUpdate ====================================================== 338 | * Abstract: 339 | * This function is called once for every major integration time step. 340 | * Discrete states are typically updated here, but this function is useful 341 | * for performing any tasks that should only take place once per 342 | * integration step. 343 | */ 344 | static void mdlUpdate(SimStruct *S, int_T tid) { 345 | } 346 | #endif /* MDL_UPDATE */ 347 | 348 | #define MDL_DERIVATIVES /* Change to #undef to remove function */ 349 | #if defined(MDL_DERIVATIVES) 350 | /* Function: mdlDerivatives ================================================= 351 | * Abstract: 352 | * In this function, you compute the S-function block's derivatives. 353 | * The derivatives are placed in the derivative vector, ssGetdX(S). 354 | */ 355 | static void mdlDerivatives(SimStruct *S) { 356 | } 357 | #endif /* MDL_DERIVATIVES */ 358 | 359 | /* Function: mdlTerminate ===================================================== 360 | * Abstract: 361 | * In this function, you should perform any actions that are necessary 362 | * at the termination of a simulation. For example, if memory was 363 | * allocated in mdlStart, this is the place to free it. 364 | */ 365 | static void mdlTerminate(SimStruct *S) { 366 | // get Objects 367 | void** vecPWork = ssGetPWork(S); 368 | 369 | GenericPublisher* pub = 370 | (GenericPublisher*) vecPWork[0]; 371 | 372 | const std::string* topic = (const std::string*) vecPWork[1]; 373 | 374 | // cleanup 375 | delete pub; 376 | delete topic; 377 | 378 | SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 379 | } 380 | 381 | /*======================================================* 382 | * See sfuntmpl_doc.c for the optional S-function methods * 383 | *======================================================*/ 384 | 385 | /*=============================* 386 | * Required S-function trailer * 387 | *=============================*/ 388 | 389 | #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ 390 | #include "simulink.c" /* MEX-file interface mechanism */ 391 | #else 392 | #include "cg_sfun.h" /* Code generation registration function */ 393 | #endif 394 | -------------------------------------------------------------------------------- /mrb_std_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #################################################################### 2 | # 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2012, Max-Planck-Gesellschaft 6 | # Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | #################################################################### 37 | 38 | 39 | cmake_minimum_required(VERSION 2.8.3) 40 | project(mrb_std_msgs) 41 | 42 | 43 | #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH}) 44 | 45 | ## Find catkin macros and libraries 46 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 47 | ## is used, also find other catkin packages 48 | find_package(catkin REQUIRED COMPONENTS 49 | matlab_ros_bridge 50 | roscpp 51 | std_msgs 52 | cmake_module_matlab 53 | ) 54 | 55 | find_package(Matlab REQUIRED) 56 | if(MATLAB_FOUND) 57 | include_directories(${MATLAB_INCLUDE_DIRS}) 58 | include_directories(${SIMULINK_INCLUDE_DIRS}) 59 | endif() 60 | 61 | add_definitions(-DMATLAB_MEX_FILE) 62 | 63 | ## System dependencies are found with CMake's conventions 64 | # find_package(Boost REQUIRED COMPONENTS system) 65 | 66 | 67 | ## Uncomment this if the package has a setup.py. This macro ensures 68 | ## modules and global scripts declared therein get installed 69 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 70 | # catkin_python_setup() 71 | 72 | ################################################ 73 | ## Declare ROS messages, services and actions ## 74 | ################################################ 75 | 76 | ## To declare and build messages, services or actions from within this 77 | ## package, follow these steps: 78 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 79 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 82 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 83 | ## pulled in transitively but can be declared for certainty nonetheless: 84 | ## * add a build_depend tag for "message_generation" 85 | ## * add a run_depend tag for "message_runtime" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "message_generation" and every package in MSG_DEP_SET to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * add "message_runtime" and every package in MSG_DEP_SET to 90 | ## catkin_package(CATKIN_DEPENDS ...) 91 | ## * uncomment the add_*_files sections below as needed 92 | ## and list every .msg/.srv/.action file to be processed 93 | ## * uncomment the generate_messages entry below 94 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 95 | 96 | ## Generate messages in the 'msg' folder 97 | # add_message_files( 98 | # FILES 99 | # Message1.msg 100 | # Message2.msg 101 | # ) 102 | 103 | ## Generate services in the 'srv' folder 104 | # add_service_files( 105 | # FILES 106 | # Service1.srv 107 | # Service2.srv 108 | # ) 109 | 110 | ## Generate actions in the 'action' folder 111 | # add_action_files( 112 | # FILES 113 | # Action1.action 114 | # Action2.action 115 | # ) 116 | 117 | ## Generate added messages and services with any dependencies listed here 118 | # generate_messages( 119 | # DEPENDENCIES 120 | # active_estimation_msgs# geometry_msgs# std_msgs# std_msgs# telekyb_msgs 121 | # ) 122 | 123 | ################################### 124 | ## catkin specific configuration ## 125 | ################################### 126 | ## The catkin_package macro generates cmake config files for your package 127 | ## Declare things to be passed to dependent projects 128 | ## INCLUDE_DIRS: uncomment this if you package contains header files 129 | ## LIBRARIES: libraries you create in this project that dependent projects also need 130 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 131 | ## DEPENDS: system dependencies of this project that dependent projects also need 132 | catkin_package( 133 | # INCLUDE_DIRS include 134 | # LIBRARIES matlab_ros_bridge 135 | # CATKIN_DEPENDS roscpp std_msgs matlab_ros_bridge 136 | # DEPENDS boost Matlab 137 | ) 138 | 139 | ########### 140 | ## Build ## 141 | ########### 142 | 143 | ## Specify additional locations of header files 144 | ## Your package locations should be listed before other locations 145 | # include_directories(include) 146 | include_directories( 147 | ${catkin_INCLUDE_DIRS} 148 | ) 149 | 150 | set(SOURCES 151 | rmsg_pub_String.cpp 152 | ) 153 | 154 | string(FIND ${CATKIN_DEVEL_PREFIX} ${PROJECT_NAME} FoundPosition REVERSE) 155 | if(FoundPosition EQUAL -1) 156 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${PROJECT_NAME}/${CATKIN_PACKAGE_LIB_DESTINATION}) 157 | else() 158 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) 159 | endif() 160 | 161 | foreach(SOURCE ${SOURCES}) 162 | string(REGEX REPLACE ".cpp" "" TARGET_NAME "${SOURCE}") 163 | message(STATUS "Adding target ${TARGET_NAME} for ${SOURCE}") 164 | add_library(${TARGET_NAME} SHARED src/${PROJECT_NAME}/${SOURCE}) 165 | set_target_properties(${TARGET_NAME} PROPERTIES PREFIX "" SUFFIX ".mexa64") 166 | set_target_properties(${TARGET_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${${PROJECT_NAME}_MEX_DESTINATION}) 167 | target_link_libraries(${TARGET_NAME} ${catkin_LIBRARIES}) 168 | install(TARGETS ${TARGET_NAME} 169 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | ) 173 | endforeach(SOURCE) 174 | 175 | ## Declare a cpp executable 176 | # add_executable(matlab_ros_bridge_node src/matlab_ros_bridge_node.cpp) 177 | 178 | ## Add cmake target dependencies of the executable/library 179 | ## as an example, message headers may need to be generated before nodes 180 | # add_dependencies(matlab_ros_bridge_node matlab_ros_bridge_generate_messages_cpp) 181 | 182 | ## Specify libraries to link a library or executable target against 183 | # target_link_libraries(matlab_ros_bridge_node 184 | # ${catkin_LIBRARIES} 185 | # ) 186 | 187 | #get_property(MAKE_INFO_SOURCE_PATH GLOBAL PROPERTY MAKE_INFO_SOURCE_PATH) 188 | set(MAKE_INFO_SOURCE_PATH "'${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}'") 189 | 190 | #get_property(MAKE_INFO_INCLUDE_PATH GLOBAL PROPERTY MAKE_INFO_INCLUDE_PATH) 191 | #set(MAKE_INFO_INCLUDE_PATH "'${Boost_INCLUDE_DIRS}' '${catkin_INCLUDE_DIRS}'") 192 | set(MAKE_INFO_INCLUDE_PATH "'${catkin_INCLUDE_DIRS}'") 193 | string(REGEX REPLACE ";" "' '" MAKE_INFO_INCLUDE_PATH "${MAKE_INFO_INCLUDE_PATH}") 194 | 195 | #get_property(MAKE_INFO_LINK_LIBS_OBJS GLOBAL PROPERTY MAKE_INFO_LINK_LIBS_OBJS) 196 | 197 | foreach (catkin_LIBERARY ${catkin_LIBRARIES}) 198 | if(IS_ABSOLUTE ${catkin_LIBERARY}) 199 | set(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 200 | else(IS_ABSOLUTE ${catkin_LIBERARY}) 201 | find_library(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 202 | endif(IS_ABSOLUTE ${catkin_LIBERARY}) 203 | list(APPEND MAKE_INFO_LINK_LIBS_OBJS ${${catkin_LIBERARY}_path}) 204 | endforeach(catkin_LIBERARY) 205 | string(REGEX REPLACE ";" "' '" MAKE_INFO_LINK_LIBS_OBJS "'${MAKE_INFO_LINK_LIBS_OBJS}'") 206 | 207 | configure_file(${RTWMAKECFG_TEMPLATE_PATH} ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m) 208 | 209 | ############# 210 | ## Install ## 211 | ############# 212 | 213 | # all install targets should use catkin DESTINATION variables 214 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 215 | 216 | ## Mark executable scripts (Python etc.) for installation 217 | ## in contrast to setup.py, you can choose the destination 218 | # install(PROGRAMS 219 | # scripts/my_python_script 220 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 221 | # ) 222 | 223 | ## Mark executables and/or libraries for installation 224 | 225 | ## Mark cpp header files for installation 226 | install(DIRECTORY src/ 227 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 228 | FILES_MATCHING PATTERN "*.cpp" 229 | PATTERN ".svn" EXCLUDE 230 | PATTERN "CMakeFiles" EXCLUDE 231 | ) 232 | 233 | ## Mark other files for installation (e.g. launch and bag files, etc.) 234 | install(FILES 235 | ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m 236 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 237 | ) 238 | 239 | #install(FILES 240 | # startup_${PROJECT_NAME}.m 241 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 242 | #) 243 | 244 | ############# 245 | ## Testing ## 246 | ############# 247 | 248 | ## Add gtest based cpp test target and link libraries 249 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_matlab_ros_bridge.cpp) 250 | # if(TARGET ${PROJECT_NAME}-test) 251 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 252 | # endif() 253 | 254 | ## Add folders to be run by python nosetests 255 | # catkin_add_nosetests(test) 256 | -------------------------------------------------------------------------------- /mrb_std_msgs/models/std_msgs.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/mrb_std_msgs/models/std_msgs.slx -------------------------------------------------------------------------------- /mrb_std_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrb_std_msgs 4 | 0.0.0 5 | The matlab_ros_bridge plugin package for std_msgs 6 | 7 | Riccardo Spica 8 | Giovanni Claudio 9 | BSD 10 | 11 | http://wiki.ros.org/matlab_ros_bridge 12 | 13 | Riccardo Spica 14 | 15 | 16 | catkin 17 | roscpp 18 | std_msgs 19 | matlab_ros_bridge 20 | cmake_module_matlab 21 | 22 | roscpp 23 | std_msgs 24 | matlab_ros_bridge 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /mrb_std_msgs/src/mrb_std_msgs/rmsg_pub_String.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | /* 40 | * You must specify the S_FUNCTION_NAME as the name of your S-function 41 | * (i.e. replace sfuntmpl_basic with the name of your S-function). 42 | */ 43 | 44 | #define S_FUNCTION_NAME rmsg_pub_String 45 | #define S_FUNCTION_LEVEL 2 46 | 47 | #define STRINGIFY(x) #x 48 | #define TOSTRING(x) STRINGIFY(x) 49 | 50 | 51 | /* 52 | * Need to include simstruc.h for the definition of the SimStruct and 53 | * its associated macro definitions. 54 | */ 55 | #include "simstruc.h" 56 | 57 | #pragma push_macro("RT") 58 | #undef RT 59 | 60 | #include 61 | 62 | // Generic Publisher 63 | #include 64 | 65 | // Message 66 | #include 67 | 68 | #pragma pop_macro("RT") 69 | 70 | #include 71 | 72 | #define MDL_CHECK_PARAMETERS 73 | #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) 74 | /* Function: mdlCheckParameters ============================================= 75 | * Abstract: 76 | * Validate our parameters to verify: 77 | * o The numerator must be of a lower order than the denominator. 78 | * o The sample time must be a real positive nonzero value. 79 | */ 80 | static void mdlCheckParameters(SimStruct *S) 81 | { 82 | 83 | // Tsim 84 | if (mxIsEmpty( ssGetSFcnParam(S,0)) || 85 | mxIsSparse( ssGetSFcnParam(S,0)) || 86 | mxIsComplex( ssGetSFcnParam(S,0)) || 87 | mxIsLogical( ssGetSFcnParam(S,0)) || 88 | !mxIsNumeric( ssGetSFcnParam(S,0)) || 89 | !mxIsDouble( ssGetSFcnParam(S,0)) || 90 | mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { 91 | ssSetErrorStatus(S,"Simulation time must be a single double Value"); 92 | return; 93 | } 94 | 95 | // Prefix Topic 96 | if (!mxIsChar( ssGetSFcnParam(S,1)) ) { 97 | ssSetErrorStatus(S,"Prefix value must be char array (string)"); 98 | return; 99 | } 100 | 101 | // String length 102 | if (mxIsEmpty( ssGetSFcnParam(S,2)) || 103 | mxIsSparse( ssGetSFcnParam(S,2)) || 104 | mxIsComplex( ssGetSFcnParam(S,2)) || 105 | mxIsLogical( ssGetSFcnParam(S,2)) || 106 | !mxIsChar(ssGetSFcnParam(S,2)) || //ssGetDTypeIdFromMxArray(ssGetSFcnParam(S,2))!=SS_UINT32 || 107 | mxGetNumberOfElements(ssGetSFcnParam(S,2)) != 1) { 108 | ssSetErrorStatus(S,"String length must be a char"); 109 | return; 110 | } 111 | 112 | } 113 | #endif /* MDL_CHECK_PARAMETERS */ 114 | 115 | 116 | 117 | /* Error handling 118 | * -------------- 119 | * 120 | * You should use the following technique to report errors encountered within 121 | * an S-function: 122 | * 123 | * ssSetErrorStatus(S,"Error encountered due to ..."); 124 | * return; 125 | * 126 | * Note that the 2nd argument to ssSetErrorStatus must be persistent memory. 127 | * It cannot be a local variable. For example the following will cause 128 | * unpredictable errors: 129 | * 130 | * mdlOutputs() 131 | * { 132 | * char msg[256]; {ILLEGAL: to fix use "static char msg[256];"} 133 | * sprintf(msg,"Error due to %s", string); 134 | * ssSetErrorStatus(S,msg); 135 | * return; 136 | * } 137 | * 138 | * See matlabroot/simulink/src/sfuntmpl_doc.c for more details. 139 | */ 140 | 141 | /*====================* 142 | * S-function methods * 143 | *====================*/ 144 | 145 | 146 | //double Tsim; 147 | 148 | /* Function: mdlInitializeSizes =============================================== 149 | * Abstract: 150 | * The sizes information is used by Simulink to determine the S-function 151 | * block's characteristics (number of inputs, outputs, states, etc.). 152 | */ 153 | static void mdlInitializeSizes(SimStruct *S) 154 | { 155 | /* See sfuntmpl_doc.c for more details on the macros below */ 156 | 157 | ssSetNumSFcnParams(S, 3); /* Number of expected parameters */ 158 | #if defined(MATLAB_MEX_FILE) 159 | if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { 160 | mdlCheckParameters(S); 161 | if (ssGetErrorStatus(S) != NULL) { 162 | return; 163 | } 164 | } else { 165 | return; /* Parameter mismatch will be reported by Simulink. */ 166 | } 167 | #endif 168 | 169 | ssSetNumContStates(S, 0); 170 | ssSetNumDiscStates(S, 0); 171 | 172 | if (!ssSetNumInputPorts(S, 1)) return; 173 | 174 | //const uint32_T strLength = *((uint32_T*)mxGetData(ssGetSFcnParam(S, 2))); 175 | const mxChar strLength = *((mxChar*)mxGetData(ssGetSFcnParam(S, 2))); 176 | ssSetInputPortMatrixDimensions(S, 0, 1, strLength); // string 177 | ssSetInputPortDataType(S, 0, SS_UINT8); 178 | 179 | for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) { 180 | /*direct input signal access*/ 181 | ssSetInputPortRequiredContiguous(S, i, true); 182 | /* 183 | * Set direct feedthrough flag (1=yes, 0=no). 184 | * A port has direct feedthrough if the input is used in either 185 | * the mdlOutputs or mdlGetTimeOfNextVarHit functions. 186 | * See matlabroot/simulink/src/sfuntmpl_directfeed.txt. 187 | */ 188 | ssSetInputPortDirectFeedThrough(S, i, 1); 189 | } 190 | 191 | if (!ssSetNumOutputPorts(S, 0)) return; 192 | 193 | ssSetNumSampleTimes(S, 1); 194 | ssSetNumRWork(S, 0); 195 | ssSetNumIWork(S, 1); // strLength 196 | ssSetNumPWork(S, 1); // GenericPub 197 | ssSetNumModes(S, 0); 198 | ssSetNumNonsampledZCs(S, 0); 199 | 200 | /* Specify the sim state compliance to be same as a built-in block */ 201 | ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 202 | 203 | ssSetOptions(S, 0); 204 | } 205 | 206 | 207 | 208 | /* Function: mdlInitializeSampleTimes ========================================= 209 | * Abstract: 210 | * This function is used to specify the sample time(s) for your 211 | * S-function. You must register the same number of sample times as 212 | * specified in ssSetNumSampleTimes. 213 | */ 214 | 215 | static void mdlInitializeSampleTimes(SimStruct *S) 216 | { 217 | real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0)); 218 | ssSetSampleTime(S, 0, Tsim); //DISCRETE_SAMPLE_TIME); 219 | ssSetOffsetTime(S, 0, 0.0); 220 | 221 | } 222 | 223 | 224 | 225 | #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ 226 | #if defined(MDL_INITIALIZE_CONDITIONS) 227 | /* Function: mdlInitializeConditions ======================================== 228 | * Abstract: 229 | * In this function, you should initialize the continuous and discrete 230 | * states for your S-function block. The initial states are placed 231 | * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S). 232 | * You can also perform any other initialization activities that your 233 | * S-function may require. Note, this routine will be called at the 234 | * start of simulation and if it is present in an enabled subsystem 235 | * configured to reset states, it will be call when the enabled subsystem 236 | * restarts execution to reset the states. 237 | */ 238 | static void mdlInitializeConditions(SimStruct *S) 239 | { 240 | } 241 | #endif /* MDL_INITIALIZE_CONDITIONS */ 242 | 243 | #define MDL_START /* Change to #undef to remove function */ 244 | #if defined(MDL_START) 245 | /* Function: mdlStart ======================================================= 246 | * Abstract: 247 | * This function is called once at start of model execution. If you 248 | * have states that should be initialized once, this is the place 249 | * to do it. 250 | */ 251 | 252 | static void mdlStart(SimStruct *S) 253 | { 254 | SFUNPRINTF("Creating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 255 | // init ROS if not yet done. 256 | initROS(S); 257 | 258 | void** vecPWork = ssGetPWork(S); 259 | 260 | // ssGetIWork(S)[0] = *((uint32_T*)mxGetData(ssGetSFcnParam(S, 2))); 261 | ssGetIWork(S)[0] = *((mxChar*)mxGetData(ssGetSFcnParam(S, 2))); 262 | 263 | ros::NodeHandle nodeHandle(ros::this_node::getName()); 264 | 265 | // get Topic Strings 266 | size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1; 267 | char* prefix_topic = (char*)mxMalloc(prefix_buflen); 268 | mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen); 269 | 270 | //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic); 271 | GenericPublisher* pub 272 | = new GenericPublisher(nodeHandle, prefix_topic, 10); 273 | vecPWork[0] = pub; 274 | 275 | // free char array 276 | mxFree(prefix_topic); 277 | 278 | } 279 | #endif /* MDL_START */ 280 | 281 | 282 | 283 | /* Function: mdlOutputs ======================================================= 284 | * Abstract: 285 | * In this function, you compute the outputs of your S-function 286 | * block. 287 | */ 288 | static void mdlOutputs(SimStruct *S, int_T tid) 289 | { 290 | 291 | // get Objects 292 | void** vecPWork = ssGetPWork(S); 293 | //const uint32_T strLength = ssGetIWork(S)[0]; 294 | const mxChar strLength = ssGetIWork(S)[0]; 295 | 296 | // get Pointers 297 | // accessing inputs 298 | uint8_T* txt = (uint8_T*)ssGetInputPortSignal(S,0); 299 | 300 | std_msgs::String msg; 301 | msg.data.resize(strLength); 302 | memcpy(&msg.data[0], (char*)txt, strLength*sizeof(char)); 303 | // for (unsigned int i=0; i < strLength; ++i) { 304 | // msg.data[i] = (char)txt[i]; 305 | // } 306 | 307 | GenericPublisher* pub 308 | = (GenericPublisher*)vecPWork[0]; 309 | 310 | pub->publish(msg); 311 | 312 | } 313 | 314 | 315 | 316 | #define MDL_UPDATE /* Change to #undef to remove function */ 317 | #if defined(MDL_UPDATE) 318 | /* Function: mdlUpdate ====================================================== 319 | * Abstract: 320 | * This function is called once for every major integration time step. 321 | * Discrete states are typically updated here, but this function is useful 322 | * for performing any tasks that should only take place once per 323 | * integration step. 324 | */ 325 | static void mdlUpdate(SimStruct *S, int_T tid) 326 | { 327 | } 328 | #endif /* MDL_UPDATE */ 329 | 330 | 331 | 332 | #define MDL_DERIVATIVES /* Change to #undef to remove function */ 333 | #if defined(MDL_DERIVATIVES) 334 | /* Function: mdlDerivatives ================================================= 335 | * Abstract: 336 | * In this function, you compute the S-function block's derivatives. 337 | * The derivatives are placed in the derivative vector, ssGetdX(S). 338 | */ 339 | static void mdlDerivatives(SimStruct *S) 340 | { 341 | } 342 | #endif /* MDL_DERIVATIVES */ 343 | 344 | 345 | 346 | /* Function: mdlTerminate ===================================================== 347 | * Abstract: 348 | * In this function, you should perform any actions that are necessary 349 | * at the termination of a simulation. For example, if memory was 350 | * allocated in mdlStart, this is the place to free it. 351 | */ 352 | static void mdlTerminate(SimStruct *S) 353 | { 354 | // get Objects 355 | void** vecPWork = ssGetPWork(S); 356 | GenericPublisher* pub = (GenericPublisher*)vecPWork[0]; 357 | delete pub; 358 | 359 | SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 360 | } 361 | 362 | 363 | 364 | /*=============================* 365 | * Required S-function trailer * 366 | *=============================*/ 367 | 368 | #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ 369 | #include "simulink.c" /* MEX-file interface mechanism */ 370 | #else 371 | #include "cg_sfun.h" /* Code generation registration function */ 372 | #endif 373 | -------------------------------------------------------------------------------- /mrb_tf/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/mrb_tf/CATKIN_IGNORE -------------------------------------------------------------------------------- /mrb_tf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #################################################################### 2 | # 3 | # Software License Agreement (BSD License) 4 | # 5 | # Copyright (c) 2012, Max-Planck-Gesellschaft 6 | # Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | #################################################################### 37 | 38 | 39 | cmake_minimum_required(VERSION 2.8.3) 40 | project(mrb_tf) 41 | 42 | 43 | #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/modules ${CMAKE_MODULE_PATH}) 44 | 45 | ## Find catkin macros and libraries 46 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 47 | ## is used, also find other catkin packages 48 | find_package(catkin REQUIRED COMPONENTS 49 | matlab_ros_bridge 50 | roscpp 51 | tf 52 | cmake_module_matlab 53 | ) 54 | 55 | find_package(Matlab REQUIRED) 56 | if(MATLAB_FOUND) 57 | include_directories(${MATLAB_INCLUDE_DIRS}) 58 | include_directories(${SIMULINK_INCLUDE_DIRS}) 59 | endif() 60 | 61 | add_definitions(-DMATLAB_MEX_FILE) 62 | 63 | ## System dependencies are found with CMake's conventions 64 | # find_package(Boost REQUIRED COMPONENTS system) 65 | 66 | 67 | ## Uncomment this if the package has a setup.py. This macro ensures 68 | ## modules and global scripts declared therein get installed 69 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 70 | # catkin_python_setup() 71 | 72 | ################################################ 73 | ## Declare ROS messages, services and actions ## 74 | ################################################ 75 | 76 | ## To declare and build messages, services or actions from within this 77 | ## package, follow these steps: 78 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 79 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 82 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 83 | ## pulled in transitively but can be declared for certainty nonetheless: 84 | ## * add a build_depend tag for "message_generation" 85 | ## * add a run_depend tag for "message_runtime" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "message_generation" and every package in MSG_DEP_SET to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * add "message_runtime" and every package in MSG_DEP_SET to 90 | ## catkin_package(CATKIN_DEPENDS ...) 91 | ## * uncomment the add_*_files sections below as needed 92 | ## and list every .msg/.srv/.action file to be processed 93 | ## * uncomment the generate_messages entry below 94 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 95 | 96 | ## Generate messages in the 'msg' folder 97 | # add_message_files( 98 | # FILES 99 | # Message1.msg 100 | # Message2.msg 101 | # ) 102 | 103 | ## Generate services in the 'srv' folder 104 | # add_service_files( 105 | # FILES 106 | # Service1.srv 107 | # Service2.srv 108 | # ) 109 | 110 | ## Generate actions in the 'action' folder 111 | # add_action_files( 112 | # FILES 113 | # Action1.action 114 | # Action2.action 115 | # ) 116 | 117 | ## Generate added messages and services with any dependencies listed here 118 | # generate_messages( 119 | # DEPENDENCIES 120 | # ) 121 | 122 | ################################### 123 | ## catkin specific configuration ## 124 | ################################### 125 | ## The catkin_package macro generates cmake config files for your package 126 | ## Declare things to be passed to dependent projects 127 | ## INCLUDE_DIRS: uncomment this if you package contains header files 128 | ## LIBRARIES: libraries you create in this project that dependent projects also need 129 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 130 | ## DEPENDS: system dependencies of this project that dependent projects also need 131 | catkin_package( 132 | # INCLUDE_DIRS include 133 | # LIBRARIES matlab_ros_bridge 134 | # CATKIN_DEPENDS roscpp tf matlab_ros_bridge 135 | # DEPENDS boost Matlab 136 | ) 137 | 138 | ########### 139 | ## Build ## 140 | ########### 141 | 142 | ## Specify additional locations of header files 143 | ## Your package locations should be listed before other locations 144 | # include_directories(include) 145 | include_directories( 146 | ${catkin_INCLUDE_DIRS} 147 | ) 148 | 149 | set(SOURCES 150 | listen_tf.cpp 151 | ) 152 | 153 | string(FIND ${CATKIN_DEVEL_PREFIX} ${PROJECT_NAME} FoundPosition REVERSE) 154 | if(FoundPosition EQUAL -1) 155 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${PROJECT_NAME}/${CATKIN_PACKAGE_LIB_DESTINATION}) 156 | else() 157 | set(${PROJECT_NAME}_MEX_DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}) 158 | endif() 159 | 160 | foreach(SOURCE ${SOURCES}) 161 | string(REGEX REPLACE ".cpp" "" TARGET_NAME "${SOURCE}") 162 | message(STATUS "Adding target ${TARGET_NAME} for ${SOURCE}") 163 | add_library(${TARGET_NAME} SHARED src/${PROJECT_NAME}/${SOURCE}) 164 | set_target_properties(${TARGET_NAME} PROPERTIES PREFIX "" SUFFIX ".mexa64") 165 | set_target_properties(${TARGET_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${${PROJECT_NAME}_MEX_DESTINATION}) 166 | target_link_libraries(${TARGET_NAME} ${catkin_LIBRARIES}) 167 | install(TARGETS ${TARGET_NAME} 168 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | ) 172 | endforeach(SOURCE) 173 | 174 | ## Declare a cpp executable 175 | # add_executable(matlab_ros_bridge_node src/matlab_ros_bridge_node.cpp) 176 | 177 | ## Add cmake target dependencies of the executable/library 178 | ## as an example, message headers may need to be generated before nodes 179 | # add_dependencies(matlab_ros_bridge_node matlab_ros_bridge_generate_messages_cpp) 180 | 181 | ## Specify libraries to link a library or executable target against 182 | # target_link_libraries(matlab_ros_bridge_node 183 | # ${catkin_LIBRARIES} 184 | # ) 185 | 186 | #get_property(MAKE_INFO_SOURCE_PATH GLOBAL PROPERTY MAKE_INFO_SOURCE_PATH) 187 | set(MAKE_INFO_SOURCE_PATH "'${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}'") 188 | 189 | #get_property(MAKE_INFO_INCLUDE_PATH GLOBAL PROPERTY MAKE_INFO_INCLUDE_PATH) 190 | #set(MAKE_INFO_INCLUDE_PATH "'${Boost_INCLUDE_DIRS}' '${catkin_INCLUDE_DIRS}'") 191 | set(MAKE_INFO_INCLUDE_PATH "'${catkin_INCLUDE_DIRS}'") 192 | string(REGEX REPLACE ";" "' '" MAKE_INFO_INCLUDE_PATH "${MAKE_INFO_INCLUDE_PATH}") 193 | 194 | #get_property(MAKE_INFO_LINK_LIBS_OBJS GLOBAL PROPERTY MAKE_INFO_LINK_LIBS_OBJS) 195 | 196 | foreach (catkin_LIBERARY ${catkin_LIBRARIES}) 197 | if(IS_ABSOLUTE ${catkin_LIBERARY}) 198 | set(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 199 | else(IS_ABSOLUTE ${catkin_LIBERARY}) 200 | find_library(${catkin_LIBERARY}_path ${catkin_LIBERARY}) 201 | endif(IS_ABSOLUTE ${catkin_LIBERARY}) 202 | list(APPEND MAKE_INFO_LINK_LIBS_OBJS ${${catkin_LIBERARY}_path}) 203 | endforeach(catkin_LIBERARY) 204 | string(REGEX REPLACE ";" "' '" MAKE_INFO_LINK_LIBS_OBJS "'${MAKE_INFO_LINK_LIBS_OBJS}'") 205 | 206 | configure_file(${RTWMAKECFG_TEMPLATE_PATH} ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m) 207 | 208 | ############# 209 | ## Install ## 210 | ############# 211 | 212 | # all install targets should use catkin DESTINATION variables 213 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 214 | 215 | ## Mark executable scripts (Python etc.) for installation 216 | ## in contrast to setup.py, you can choose the destination 217 | # install(PROGRAMS 218 | # scripts/my_python_script 219 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 220 | # ) 221 | 222 | ## Mark executables and/or libraries for installation 223 | 224 | ## Mark cpp header files for installation 225 | install(DIRECTORY src/ 226 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src 227 | FILES_MATCHING PATTERN "*.cpp" 228 | PATTERN ".svn" EXCLUDE 229 | PATTERN "CMakeFiles" EXCLUDE 230 | ) 231 | 232 | ## Mark other files for installation (e.g. launch and bag files, etc.) 233 | install(FILES 234 | ${${PROJECT_NAME}_MEX_DESTINATION}/rtwmakecfg.m 235 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 236 | ) 237 | 238 | #install(FILES 239 | # startup_${PROJECT_NAME}.m 240 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 241 | #) 242 | 243 | ############# 244 | ## Testing ## 245 | ############# 246 | 247 | ## Add gtest based cpp test target and link libraries 248 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_matlab_ros_bridge.cpp) 249 | # if(TARGET ${PROJECT_NAME}-test) 250 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 251 | # endif() 252 | 253 | ## Add folders to be run by python nosetests 254 | # catkin_add_nosetests(test) 255 | -------------------------------------------------------------------------------- /mrb_tf/models/tf_lib.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/matlab_ros_bridge/4c194c476befbb62c2020d653a758d607200a0c4/mrb_tf/models/tf_lib.slx -------------------------------------------------------------------------------- /mrb_tf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mrb_tf 4 | 0.0.0 5 | The matlab_ros_bridge plugin package for tf 6 | 7 | Riccardo Spica 8 | Giovanni Claudio 9 | BSD 10 | 11 | http://wiki.ros.org/matlab_ros_bridge 12 | 13 | Riccardo Spica 14 | 15 | 16 | catkin 17 | matlab_ros_bridge 18 | roscpp 19 | tf 20 | cmake_module_matlab 21 | 22 | matlab_ros_bridge 23 | roscpp 24 | tf 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mrb_tf/src/mrb_tf/listen_tf.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2012, Max-Planck-Gesellschaft 6 | * Copyright (c) 2012-2015, Inria 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 the copyright holder 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 | ********************************************************************/ 37 | 38 | 39 | /* 40 | * You must specify the S_FUNCTION_NAME as the name of your S-function 41 | * (i.e. replace sfuntmpl_basic with the name of your S-function). 42 | */ 43 | 44 | #define S_FUNCTION_NAME listen_tf 45 | #define S_FUNCTION_LEVEL 2 46 | 47 | #define STRINGIFY(x) #x 48 | #define TOSTRING(x) STRINGIFY(x) 49 | 50 | 51 | /* 52 | * Need to include simstruc.h for the definition of the SimStruct and 53 | * its associated macro definitions. 54 | */ 55 | #include "simstruc.h" 56 | 57 | #pragma push_macro("RT") 58 | #undef RT 59 | 60 | #include 61 | 62 | // TF 63 | #include 64 | #include 65 | 66 | #pragma pop_macro("RT") 67 | 68 | #include 69 | 70 | #define MDL_CHECK_PARAMETERS 71 | #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) 72 | /* Function: mdlCheckParameters ============================================= 73 | * Abstract: 74 | * Validate our parameters to verify: 75 | * o The numerator must be of a lower order than the denominator. 76 | * o The sample time must be a real positive nonzero value. 77 | */ 78 | static void mdlCheckParameters(SimStruct *S) 79 | { 80 | // SFUNPRINTF("Calling mdlCheckParameters"); 81 | 82 | // Tsim 83 | if (mxIsEmpty( ssGetSFcnParam(S,0)) || 84 | mxIsSparse( ssGetSFcnParam(S,0)) || 85 | mxIsComplex( ssGetSFcnParam(S,0)) || 86 | mxIsLogical( ssGetSFcnParam(S,0)) || 87 | !mxIsNumeric( ssGetSFcnParam(S,0)) || 88 | !mxIsDouble( ssGetSFcnParam(S,0)) || 89 | mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { 90 | ssSetErrorStatus(S,"Simulation time must be a single double Value"); 91 | return; 92 | } 93 | 94 | // Base Frame 95 | if (!mxIsChar( ssGetSFcnParam(S,1)) ) { 96 | ssSetErrorStatus(S,"Source Frame value must be char array (string)"); 97 | return; 98 | } 99 | 100 | // End Frame 101 | if (!mxIsChar( ssGetSFcnParam(S,2)) ) { 102 | ssSetErrorStatus(S,"Target Frame value must be char array (string)"); 103 | return; 104 | } 105 | } 106 | #endif /* MDL_CHECK_PARAMETERS */ 107 | 108 | 109 | 110 | /* Error handling 111 | * -------------- 112 | * 113 | * You should use the following technique to report errors encountered within 114 | * an S-function: 115 | * 116 | * ssSetErrorStatus(S,"Error encountered due to ..."); 117 | * return; 118 | * 119 | * Note that the 2nd argument to ssSetErrorStatus must be persistent memory. 120 | * It cannot be a local variable. For example the following will cause 121 | * unpredictable errors: 122 | * 123 | * mdlOutputs() 124 | * { 125 | * char msg[256]; {ILLEGAL: to fix use "static char msg[256];"} 126 | * sprintf(msg,"Error due to %s", string); 127 | * ssSetErrorStatus(S,msg); 128 | * return; 129 | * } 130 | * 131 | * See matlabroot/simulink/src/sfuntmpl_doc.c for more details. 132 | */ 133 | 134 | /*====================* 135 | * S-function methods * 136 | *====================*/ 137 | 138 | 139 | //double Tsim; 140 | 141 | /* Function: mdlInitializeSizes =============================================== 142 | * Abstract: 143 | * The sizes information is used by Simulink to determine the S-function 144 | * block's characteristics (number of inputs, outputs, states, etc.). 145 | */ 146 | static void mdlInitializeSizes(SimStruct *S) 147 | { 148 | /* See sfuntmpl_doc.c for more details on the macros below */ 149 | 150 | ssSetNumSFcnParams(S, 3); /* Number of expected parameters */ 151 | #if defined(MATLAB_MEX_FILE) 152 | if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { 153 | mdlCheckParameters(S); 154 | if (ssGetErrorStatus(S) != NULL) { 155 | return; 156 | } 157 | } else { 158 | return; /* Parameter mismatch will be reported by Simulink. */ 159 | } 160 | #endif 161 | 162 | 163 | ssSetNumContStates(S, 0); 164 | ssSetNumDiscStates(S, 0); 165 | 166 | if (!ssSetNumInputPorts(S, 0)) return; 167 | //ssSetInputPortWidth(S, 0, 1); 168 | //ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/ 169 | /* 170 | * Set direct feedthrough flag (1=yes, 0=no). 171 | * A port has direct feedthrough if the input is used in either 172 | * the mdlOutputs or mdlGetTimeOfNextVarHit functions. 173 | * See matlabroot/simulink/src/sfuntmpl_directfeed.txt. 174 | */ 175 | //ssSetInputPortDirectFeedThrough(S, 0, 1); 176 | 177 | if (!ssSetNumOutputPorts(S, 3)) return; 178 | ssSetOutputPortMatrixDimensions(S, 0, 3, 1); // position 179 | ssSetOutputPortMatrixDimensions(S, 1, 4, 1); // orientation 180 | ssSetOutputPortMatrixDimensions(S, 2, 1, 1); // SentTime in s; 181 | 182 | ssSetNumSampleTimes(S, 1); 183 | ssSetNumRWork(S, 0); 184 | ssSetNumIWork(S, 0); 185 | ssSetNumPWork(S, 3); 186 | ssSetNumModes(S, 0); 187 | ssSetNumNonsampledZCs(S, 0); 188 | 189 | /* Specify the sim state compliance to be same as a built-in block */ 190 | ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); 191 | 192 | ssSetOptions(S, 0); 193 | } 194 | 195 | 196 | 197 | /* Function: mdlInitializeSampleTimes ========================================= 198 | * Abstract: 199 | * This function is used to specify the sample time(s) for your 200 | * S-function. You must register the same number of sample times as 201 | * specified in ssSetNumSampleTimes. 202 | */ 203 | 204 | static void mdlInitializeSampleTimes(SimStruct *S) 205 | { 206 | real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0)); 207 | ssSetSampleTime(S, 0, Tsim); //DISCRETE_SAMPLE_TIME); 208 | ssSetOffsetTime(S, 0, 0.0); 209 | 210 | } 211 | 212 | 213 | 214 | #define MDL_INITIALIZE_CONDITIONS /* Change to #undef to remove function */ 215 | #if defined(MDL_INITIALIZE_CONDITIONS) 216 | /* Function: mdlInitializeConditions ======================================== 217 | * Abstract: 218 | * In this function, you should initialize the continuous and discrete 219 | * states for your S-function block. The initial states are placed 220 | * in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S). 221 | * You can also perform any other initialization activities that your 222 | * S-function may require. Note, this routine will be called at the 223 | * start of simulation and if it is present in an enabled subsystem 224 | * configured to reset states, it will be call when the enabled subsystem 225 | * restarts execution to reset the states. 226 | */ 227 | static void mdlInitializeConditions(SimStruct *S) 228 | { 229 | } 230 | #endif /* MDL_INITIALIZE_CONDITIONS */ 231 | 232 | 233 | #define MDL_START /* Change to #undef to remove function */ 234 | #if defined(MDL_START) 235 | /* Function: mdlStart ======================================================= 236 | * Abstract: 237 | * This function is called once at start of model execution. If you 238 | * have states that should be initialized once, this is the place 239 | * to do it. 240 | */ 241 | 242 | static void mdlStart(SimStruct *S) 243 | { 244 | SFUNPRINTF("Starting Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 245 | // init ROS if not yet done. 246 | initROS(S); 247 | 248 | void** vecPWork = ssGetPWork(S); 249 | 250 | ros::NodeHandle nodeHandle(ros::this_node::getName()); 251 | 252 | // get Topic Strings 253 | size_t source_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1; 254 | size_t target_buflen = mxGetN((ssGetSFcnParam(S, 2)))*sizeof(mxChar)+1; 255 | char* source_frame = (char*)mxMalloc(source_buflen); 256 | char* target_frame = (char*)mxMalloc(target_buflen); 257 | mxGetString((ssGetSFcnParam(S, 1)), source_frame, source_buflen); 258 | mxGetString((ssGetSFcnParam(S, 2)), target_frame, target_buflen); 259 | 260 | // vecPWork[0] = new tf::TransformListener; 261 | vecPWork[1] = new std::string(source_frame); 262 | vecPWork[2] = new std::string(target_frame); 263 | 264 | mxFree(source_frame); 265 | mxFree(target_frame); 266 | } 267 | #endif /* MDL_START */ 268 | 269 | 270 | 271 | /* Function: mdlOutputs ======================================================= 272 | * Abstract: 273 | * In this function, you compute the outputs of your S-function 274 | * block. 275 | */ 276 | static void mdlOutputs(SimStruct *S, int_T tid) 277 | { 278 | // get Objects 279 | void** vecPWork = ssGetPWork(S); 280 | 281 | // Preparing Outputs 282 | real_T *position = (real_T*)ssGetOutputPortSignal(S,0); 283 | real_T *quaternion = (real_T*)ssGetOutputPortSignal(S,1); 284 | real_T *time = (real_T*)ssGetOutputPortSignal(S,2); 285 | 286 | // tf::TransformListener* listener = static_cast(vecPWork[0]); 287 | std::string* source_frame = static_cast(vecPWork[1]); 288 | std::string* target_frame = static_cast(vecPWork[2]); 289 | 290 | tf::TransformListener listener; 291 | tf::StampedTransform transform; 292 | try{ 293 | listener.lookupTransform(*target_frame, *source_frame, ros::Time(0), transform); 294 | position[0] = transform.getOrigin().getX(); 295 | position[1] = transform.getOrigin().getY(); 296 | position[2] = transform.getOrigin().getZ(); 297 | 298 | // w, x, y, z 299 | quaternion[0] = transform.getRotation().getW(); 300 | quaternion[1] = transform.getRotation().getX(); 301 | quaternion[2] = transform.getRotation().getY(); 302 | quaternion[3] = transform.getRotation().getZ(); 303 | } catch (tf::TransformException &ex) { 304 | ssWarning(S,ex.what()); 305 | return; 306 | } 307 | 308 | 309 | //time[0] = lastMsg->header.stamp.toSec(); 310 | 311 | } 312 | 313 | 314 | 315 | #define MDL_UPDATE /* Change to #undef to remove function */ 316 | #if defined(MDL_UPDATE) 317 | /* Function: mdlUpdate ====================================================== 318 | * Abstract: 319 | * This function is called once for every major integration time step. 320 | * Discrete states are typically updated here, but this function is useful 321 | * for performing any tasks that should only take place once per 322 | * integration step. 323 | */ 324 | static void mdlUpdate(SimStruct *S, int_T tid) 325 | { 326 | } 327 | #endif /* MDL_UPDATE */ 328 | 329 | 330 | 331 | #define MDL_DERIVATIVES /* Change to #undef to remove function */ 332 | #if defined(MDL_DERIVATIVES) 333 | /* Function: mdlDerivatives ================================================= 334 | * Abstract: 335 | * In this function, you compute the S-function block's derivatives. 336 | * The derivatives are placed in the derivative vector, ssGetdX(S). 337 | */ 338 | static void mdlDerivatives(SimStruct *S) 339 | { 340 | } 341 | #endif /* MDL_DERIVATIVES */ 342 | 343 | 344 | 345 | /* Function: mdlTerminate ===================================================== 346 | * Abstract: 347 | * In this function, you should perform any actions that are necessary 348 | * at the termination of a simulation. For example, if memory was 349 | * allocated in mdlStart, this is the place to free it. 350 | */ 351 | static void mdlTerminate(SimStruct *S) 352 | { 353 | // get Objects 354 | void** vecPWork = ssGetPWork(S); 355 | 356 | 357 | // delete static_cast(vecPWork[0]); 358 | // mxFree(vecPWork[1]); 359 | // mxFree(vecPWork[2]); 360 | // mxFree(prefix_topic); 361 | // mxFree(postfix_topic); 362 | 363 | SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME)); 364 | } 365 | 366 | 367 | /*======================================================* 368 | * See sfuntmpl_doc.c for the optional S-function methods * 369 | *======================================================*/ 370 | 371 | /*=============================* 372 | * Required S-function trailer * 373 | *=============================*/ 374 | 375 | #ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ 376 | #include "simulink.c" /* MEX-file interface mechanism */ 377 | #else 378 | #include "cg_sfun.h" /* Code generation registration function */ 379 | #endif 380 | --------------------------------------------------------------------------------