├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── asp_mbed └── cfg_binary │ ├── README.md │ ├── cfg-linux-static-1_9_6 │ ├── cfg-mingw-static-1_9_6.exe │ └── cfg-osx-static-1_9_5 ├── doc ├── document.txt └── mROS.pdf ├── mros-lib ├── Makefile.m ├── mros-msgs │ ├── sensor_msgs │ │ └── Image.h │ └── std_msgs │ │ ├── Header.h │ │ ├── Int16.h │ │ ├── Int16MultiArray.h │ │ ├── Int32.h │ │ ├── Int32MultiArray.h │ │ ├── Int64MultiArray.h │ │ ├── Int8.h │ │ ├── Int8MultiArray.h │ │ ├── String.h │ │ ├── Time.h │ │ ├── UInt16.h │ │ ├── UInt16MultiArray.h │ │ ├── UInt32.h │ │ ├── UInt32MultiArray.h │ │ ├── UInt64.h │ │ ├── UInt64MultiArray.h │ │ ├── UInt8.h │ │ └── UInt8MultiArray.h ├── mros-src │ ├── api │ │ ├── mros_topic_callback.h │ │ ├── ros.cpp │ │ └── ros.h │ ├── comm │ │ └── cimpl │ │ │ ├── mros_comm_cimpl.h │ │ │ ├── mros_comm_socket_cimpl.c │ │ │ ├── mros_comm_socket_cimpl.h │ │ │ ├── mros_comm_tcp_client_cimpl.c │ │ │ ├── mros_comm_tcp_client_cimpl.h │ │ │ ├── mros_comm_tcp_client_factory_cimpl.c │ │ │ ├── mros_comm_tcp_client_factory_cimpl.h │ │ │ ├── mros_comm_tcp_server_cimpl.c │ │ │ ├── mros_comm_tcp_server_cimpl.h │ │ │ └── target │ │ │ └── lwip │ │ │ ├── mros_comm_cimpl.c │ │ │ └── mros_comm_target.h │ ├── config │ │ ├── mros_sys_config.c │ │ ├── mros_sys_config.h │ │ ├── mros_usr_config.c │ │ ├── mros_usr_config.h │ │ └── os │ │ │ └── target │ │ │ └── os_asp │ │ │ ├── mros_os_config.c │ │ │ └── mros_os_config.h │ ├── inc │ │ ├── mros_array_container.h │ │ ├── mros_exclusive_area.h │ │ ├── mros_exclusive_ops.h │ │ ├── mros_list.h │ │ ├── mros_memory.h │ │ ├── mros_name.h │ │ ├── mros_os.h │ │ ├── mros_types.h │ │ └── mros_wait_queue.h │ ├── lib │ │ ├── mros_memory.c │ │ ├── mros_name.c │ │ └── mros_wait_queue.c │ ├── node │ │ └── cimpl │ │ │ ├── mros_node_cimpl.c │ │ │ └── mros_node_cimpl.h │ ├── os │ │ ├── mros_exclusive_area.c │ │ ├── mros_exclusive_ops.c │ │ └── target │ │ │ └── os_asp │ │ │ ├── mros_log.h │ │ │ ├── mros_os.c │ │ │ ├── mros_os_target.h │ │ │ ├── mros_test.c │ │ │ └── mros_test.h │ ├── packet │ │ ├── cimpl │ │ │ ├── mros_packet_cimpl.h │ │ │ ├── mros_packet_decoder_cimpl.h │ │ │ ├── mros_packet_encoder_cimpl.h │ │ │ └── version │ │ │ │ └── kinetic │ │ │ │ ├── mros_packet_config.h │ │ │ │ ├── mros_packet_decoder_cimpl.c │ │ │ │ └── mros_packet_encoder_cimpl.c │ │ └── template │ │ │ └── version │ │ │ └── kinetic │ │ │ ├── mros_packet_fmt_http.h │ │ │ ├── mros_packet_fmt_tcpros.h │ │ │ └── mros_packet_fmt_xml.h │ ├── protocol │ │ └── cimpl │ │ │ ├── mros_protocol_client_rpc_cimpl.c │ │ │ ├── mros_protocol_client_rpc_cimpl.h │ │ │ ├── mros_protocol_master_cimpl.c │ │ │ ├── mros_protocol_master_cimpl.h │ │ │ ├── mros_protocol_operation_cimpl.c │ │ │ ├── mros_protocol_operation_cimpl.h │ │ │ ├── mros_protocol_publish_cimpl.c │ │ │ ├── mros_protocol_publish_cimpl.h │ │ │ ├── mros_protocol_server_proc_cimpl.c │ │ │ ├── mros_protocol_server_proc_cimpl.h │ │ │ ├── mros_protocol_slave_cimpl.c │ │ │ ├── mros_protocol_slave_cimpl.h │ │ │ ├── mros_protocol_subscribe_cimpl.c │ │ │ └── mros_protocol_subscribe_cimpl.h │ ├── topic │ │ └── cimpl │ │ │ ├── mros_topic_cimpl.c │ │ │ ├── mros_topic_cimpl.h │ │ │ ├── mros_topic_connector_cimpl.c │ │ │ ├── mros_topic_connector_cimpl.h │ │ │ ├── mros_topic_connector_factory_cimpl.c │ │ │ ├── mros_topic_connector_factory_cimpl.h │ │ │ └── mros_topic_runner_cimpl.h │ └── transfer │ │ └── cimpl │ │ ├── mros_topic_data_publisher_cimpl.c │ │ ├── mros_topic_data_publisher_cimpl.h │ │ ├── mros_topic_data_subscriber_cimpl.c │ │ └── mros_topic_data_subscriber_cimpl.h └── mros.cfg ├── mros_msg-gen ├── including_msgs.json ├── mros_msg-gen.py ├── msg_header_template.tpl ├── msg_headers_includer.tpl ├── msg_headers_spetializer.tpl ├── msg_max_size.h └── msg_max_size.tpl ├── mros_ws ├── Makefile.asp ├── Makefile.cv ├── Makefile.mbd ├── Makefile.mros ├── custom_pubsub │ ├── .code_review_properties │ ├── .cproject │ ├── .project │ ├── .settings │ │ ├── org.eclipse.core.resources.prefs │ │ └── org.eclipse.core.runtime.prefs │ ├── Makefile │ ├── app.cfg │ ├── app.cpp │ ├── app.h │ ├── custom_pubsub_debug.launch │ ├── include │ │ ├── custom_pubsub │ │ │ ├── LEDValues.h │ │ │ ├── PersonName.h │ │ │ └── UserTypeTest.h │ │ ├── message_class_specialization.h │ │ ├── message_headers.h │ │ └── msg_max_size.h │ ├── mros_config │ │ ├── mros_sys_config.c │ │ ├── mros_sys_config.h │ │ ├── mros_usr_config.c │ │ ├── mros_usr_config.h │ │ └── os │ │ │ └── target │ │ │ └── os_asp │ │ │ ├── mros_os_config.c │ │ │ └── mros_os_config.h │ └── msg_app.json ├── image_publisher │ ├── .code_review_properties │ ├── .cproject │ ├── .project │ ├── .settings │ │ ├── org.eclipse.core.resources.prefs │ │ └── org.eclipse.core.runtime.prefs │ ├── Makefile │ ├── app.cfg │ ├── app.cpp │ ├── app.h │ ├── camera_app.json │ ├── image_publisher_debug.launch │ ├── include │ │ ├── message_class_specialization.h │ │ ├── message_headers.h │ │ └── msg_max_size.h │ └── mros_config │ │ ├── mros_sys_config.c │ │ ├── mros_sys_config.h │ │ ├── mros_usr_config.c │ │ ├── mros_usr_config.h │ │ └── os │ │ └── target │ │ └── os_asp │ │ ├── mros_os_config.c │ │ └── mros_os_config.h └── string_pubsub │ ├── .code_review_properties │ ├── .cproject │ ├── .project │ ├── .settings │ ├── com.atollic.truestudio.debug.hardware_device.prefs │ ├── org.eclipse.core.resources.prefs │ └── org.eclipse.core.runtime.prefs │ ├── Makefile │ ├── app.cfg │ ├── app.cpp │ ├── app.h │ ├── app.json │ ├── include │ ├── message_class_specialization.h │ ├── message_headers.h │ └── msg_max_size.h │ ├── mros_config │ ├── mros_sys_config.c │ ├── mros_sys_config.h │ ├── mros_usr_config.c │ ├── mros_usr_config.h │ └── os │ │ └── target │ │ └── os_asp │ │ ├── mros_os_config.c │ │ └── mros_os_config.h │ └── string_pubsub_debug.launch └── ros_catkin_ws └── src ├── custom_pubsub ├── CMakeLists.txt ├── msg │ ├── LEDValues.msg │ ├── PersonName.msg │ └── UserTypeTest.msg ├── package.xml └── src │ ├── custom_publisher.cpp │ └── custom_subscriber.cpp └── string_pubsub ├── CMakeLists.txt ├── package.xml └── src ├── led_switch_publisher.cpp └── ultrasonic_subscriber.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *.d 3 | *.a 4 | *.bin 5 | *.history 6 | 7 | # for ROS catkin_ws 8 | ros_catkin_ws/.catkin_workspace 9 | ros_catkin_ws/build 10 | ros_catkin_ws/devel 11 | ros_catkin_ws/src/CMakeLists.txt 12 | 13 | # for TrueSTUDIO 14 | mros_ws/.metadata/ 15 | mros_ws/*/.metadata/ 16 | mros_ws/*/.settings/language.settings.xml 17 | 18 | # for ASP 19 | asp 20 | asp.* 21 | cfg1_out* 22 | kernel_cfg* 23 | offset.h 24 | GR-PEACH_mbed.map 25 | Makefile.depend 26 | 27 | # etc.,,, 28 | *.swp 29 | .DS_Store 30 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "opencv-lib"] 2 | path = opencv-lib 3 | url = https://github.com/d-kato/opencv-lib.git 4 | ignore = dirty 5 | [submodule "asp_mbed/asp-gr_peach_gcc-mbed"] 6 | path = asp_mbed/asp-gr_peach_gcc-mbed 7 | url = https://github.com/tlk-emb/asp-gr_peach_gcc-mbed.git 8 | ignore = dirty 9 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | mROS 2 | A light-weighted runtime environment for ROS nodes onto embedded micro-controller 3 | 4 | Copyright (C) 2017- by EMB Group in Takagi Lab., Kyoto University 5 | 6 | The above copyright holders grant permission gratis to use, 7 | duplicate, modify, or redistribute (hereafter called use) this 8 | software (including the one made by modifying this software), 9 | provided that the following four conditions (1) through (4) are 10 | satisfied. 11 | 12 | (1) When this software is used in the form of source code, the above 13 | copyright notice, this use conditions, and the disclaimer shown 14 | below must be retained in the source code without modification. 15 | 16 | (2) When this software is redistributed in the forms usable for the 17 | development of other software, such as in library form, the above 18 | copyright notice, this use conditions, and the disclaimer shown 19 | below must be shown without modification in the document provided 20 | with the redistributed software, such as the user manual. 21 | 22 | (3) When this software is redistributed in the forms unusable for the 23 | development of other software, such as the case when the software 24 | is embedded in a piece of equipment, either of the following two 25 | conditions must be satisfied: 26 | 27 | (a) The above copyright notice, this use conditions, and the 28 | disclaimer shown below must be shown without modification in 29 | the document provided with the redistributed software, such as 30 | the user manual. 31 | 32 | (b) How the software is to be redistributed must be reported to the 33 | TOPPERS Project according to the procedure described 34 | separately. 35 | 36 | (4) The above copyright holders and the TOPPERS Project are exempt 37 | from responsibility for any type of damage directly or indirectly 38 | caused from the use of this software and are indemnified by any 39 | users or end users of this software from any and all causes of 40 | action whatsoever. 41 | 42 | THIS SOFTWARE IS PROVIDED "AS IS." THE ABOVE COPYRIGHT HOLDERS AND 43 | THE TOPPERS PROJECT DISCLAIM ANY EXPRESS OR IMPLIED WARRANTIES, 44 | INCLUDING, BUT NOT LIMITED TO, ITS APPLICABILITY TO A PARTICULAR 45 | PURPOSE. IN NO EVENT SHALL THE ABOVE COPYRIGHT HOLDERS AND THE 46 | TOPPERS PROJECT BE LIABLE FOR ANY TYPE OF DAMAGE DIRECTLY OR 47 | INDIRECTLY CAUSED FROM THE USE OF THIS SOFTWARE. 48 | 49 | --- 50 | 51 | mROS 52 | 組込みデバイス向けROS 1ノードの軽量実行環境 53 | 54 | Copyright (C) 2017- by 京都大学高木研究室組込みシステム研究グループ 55 | 56 | 上記著作権者は，以下の (1)〜(4) の条件を満たす場合に限り，本ソフトウェ 57 | ア（本ソフトウェアを改変したものを含む．以下同じ）を使用・複製・改変・ 58 | 再配布（以下，利用と呼ぶ）することを無償で許諾する． 59 | (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作権 60 | 表示，この利用条件および下記の無保証規定が，そのままの形でソース 61 | コード中に含まれていること． 62 | (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使用 63 | できる形で再配布する場合には，再配布に伴うドキュメント（利用者マ 64 | ニュアルなど）に，上記の著作権表示，この利用条件および下記の無保 65 | 証規定を掲載すること． 66 | (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使用 67 | できない形で再配布する場合には，次のいずれかの条件を満たすこと． 68 | (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著作 69 | 権表示，この利用条件および下記の無保証規定を掲載すること． 70 | (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに報 71 | 告すること． 72 | (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損害 73 | からも，上記著作権者およびTOPPERSプロジェクトを免責すること．また， 74 | 本ソフトウェアのユーザまたはエンドユーザからのいかなる理由に基づ 75 | く請求からも，上記著作権者およびTOPPERSプロジェクトを免責すること． 76 | 77 | 本ソフトウェアは，無保証で提供されているものである．上記著作権者およ 78 | びTOPPERSプロジェクトは，本ソフトウェアに関して，特定の使用目的に対す 79 | る適合性も含めて，いかなる保証も行わない．また，本ソフトウェアの利用 80 | により直接的または間接的に生じたいかなる損害に関しても，その責任を負 81 | わない． 82 | -------------------------------------------------------------------------------- /asp_mbed/cfg_binary/README.md: -------------------------------------------------------------------------------- 1 | NOTE: executables in this directory are downloaded from following link 2 | https://www.toppers.jp/cfg-download.html 3 | -------------------------------------------------------------------------------- /asp_mbed/cfg_binary/cfg-linux-static-1_9_6: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mROS-base/mROS/741b8840b8a0394233942086688e1a98ec267bc6/asp_mbed/cfg_binary/cfg-linux-static-1_9_6 -------------------------------------------------------------------------------- /asp_mbed/cfg_binary/cfg-mingw-static-1_9_6.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mROS-base/mROS/741b8840b8a0394233942086688e1a98ec267bc6/asp_mbed/cfg_binary/cfg-mingw-static-1_9_6.exe -------------------------------------------------------------------------------- /asp_mbed/cfg_binary/cfg-osx-static-1_9_5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mROS-base/mROS/741b8840b8a0394233942086688e1a98ec267bc6/asp_mbed/cfg_binary/cfg-osx-static-1_9_5 -------------------------------------------------------------------------------- /doc/document.txt: -------------------------------------------------------------------------------- 1 | 組込みマイコン向けROSノード軽量実行環境 2 | 作成者：京都大学　森　智也 3 | 作成日時：2017/8/29 4 | 5 | 概要： 6 | TOPPERS/ASPカーネルとmbedライブラリを使用してROSノード軽量実行環境を実現する． 7 | 8 | ディレクトリ構成： 9 | mROS 10 | |-.git 11 | |-cfg-mingw-static-1_9_6 12 | |-doc //ドキュメント 13 | |-mros_test //サンプル用ROSソースコード 14 | |-ros_emb //mROSソースコード 15 | |-truestudio //truestudioワークスペース 16 | 17 | サンプル実行手順： 18 | 超音波センサ(HC-SR04)を使用したサンプル 19 | ピン接続は/doc/mROS.pdf 7p 20 | 21 | ホストPC上の作業 22 | １，mros_testディレクトリをROSのcatkinワークスペースのsrcディレクトリにコピー 23 | ２，catkin_makeでmros_listenerとmros_talkerを実行可能にする 24 | 25 | mROS上の作業 26 | １，ROSマスタIPの設定 27 | ros_emb.cpp:19　m_ipにIPを設定 28 | ２，truestudioで再ビルド 29 | ３，asp.binをgr-peachに書き込む 30 | 31 | ホストPC上のターミナルを3画面開き，それぞれ 32 | $roscore 33 | $rosrun mros_test mros_listener 34 | $rosrun mros_test mros_talker 35 | を実行のち，mROSノードを実行 36 | 37 | usr_tsk1がサブスクライバ，usr_tsk2がパブリッシャを記述したユーザタスクとなっている 38 | 両者とも使用するメッセージ型はstd_msg/String 39 | 40 | To Do： 41 | ・sub_task内におけるサブスクライブループの実装 42 | 周期ハンドラで回転させた際に，初期化のループを抜けてsocketが接続できていない 43 | 初期化でパブリッシャとの接続を確立させ，一定周期でサブスクライブの実行が行えるようにする． 44 | ・tcp_rosでTCPROSコネクションヘッダに引数としてノード情報等を渡せるようにする 45 | 現在決め打ちでやってるからあんまりよくない 46 | ・ユーザタスクの初期化(ノード情報登録)に原子性を持たせる 47 | registerpublisherなどが同時に行われるとrequesttopicなどの受付がうまくいかない 48 | セマフォでxml-masタスクやxml-slvタスクを資源として管理して接続が完了しなければ次のユーザタスクが動けないようにする 49 | ・メッセージタイプの追加 50 | 対応するメッセージタイプの追加 51 | 標準メッセージくらいはサポートしたい　 52 | ・ROSAPIの対応 53 | templateを使ったsubscriberやadvertiseがうまくできていない 54 | ・ 55 | -------------------------------------------------------------------------------- /doc/mROS.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mROS-base/mROS/741b8840b8a0394233942086688e1a98ec267bc6/doc/mROS.pdf -------------------------------------------------------------------------------- /mros-lib/Makefile.m: -------------------------------------------------------------------------------- 1 | ROS_VERSION := kinetic 2 | OS_NAME := os_asp 3 | 4 | MROS_SRC_DIR := $(MROS_DIR)/mros-src 5 | MROS_MSG_DIR := $(MROS_DIR)/mros-msg 6 | 7 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/inc 8 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/api 9 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/protocol/cimpl 10 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/node/cimpl 11 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/topic/cimpl 12 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/os/target/$(OS_NAME) 13 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/comm/cimpl/target/lwip 14 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/comm/cimpl 15 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/packet/cimpl 16 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/packet/template/version/$(ROS_VERSION) 17 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/packet/cimpl/version/$(ROS_VERSION) 18 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/protocol/cimpl 19 | MROS_IFLAGS += -I$(MROS_SRC_DIR)/transfer/cimpl 20 | MROS_IFLAGS += -I$(MROS_MSG_DIR) 21 | 22 | 23 | VPATH := $(MROS_SRC_DIR)/api 24 | VPATH += $(MROS_SRC_DIR)/comm/cimpl/target/lwip 25 | VPATH += $(MROS_SRC_DIR)/comm/cimpl/target/lwip 26 | VPATH += $(MROS_SRC_DIR)/comm/cimpl 27 | VPATH += $(MROS_SRC_DIR)/node/cimpl 28 | VPATH += $(MROS_SRC_DIR)/lib 29 | VPATH += $(MROS_SRC_DIR)/os 30 | VPATH += $(MROS_SRC_DIR)/os/target/$(OS_NAME) 31 | VPATH += $(MROS_SRC_DIR)/packet/cimpl/version/$(ROS_VERSION) 32 | VPATH += $(MROS_SRC_DIR)/protocol/cimpl 33 | VPATH += $(MROS_SRC_DIR)/topic/cimpl 34 | VPATH += $(MROS_SRC_DIR)/transfer/cimpl 35 | VPATH += $(MROS_SRC_DIR)/config 36 | 37 | MROS_CXX_OBJ := ros.o 38 | MROS_C_OBJ += mros_comm_cimpl.o 39 | MROS_C_OBJ += mros_comm_socket_cimpl.o 40 | MROS_C_OBJ += mros_comm_tcp_client_cimpl.o 41 | MROS_C_OBJ += mros_comm_tcp_client_factory_cimpl.o 42 | MROS_C_OBJ += mros_comm_tcp_server_cimpl.o 43 | MROS_C_OBJ += mros_memory.o 44 | MROS_C_OBJ += mros_wait_queue.o 45 | MROS_C_OBJ += mros_node_cimpl.o 46 | MROS_C_OBJ += mros_exclusive_area.o 47 | MROS_C_OBJ += mros_exclusive_ops.o 48 | MROS_C_OBJ += mros_os.o 49 | MROS_C_OBJ += mros_packet_decoder_cimpl.o 50 | MROS_C_OBJ += mros_packet_encoder_cimpl.o 51 | MROS_C_OBJ += mros_protocol_client_rpc_cimpl.o 52 | MROS_C_OBJ += mros_protocol_master_cimpl.o 53 | MROS_C_OBJ += mros_protocol_operation_cimpl.o 54 | MROS_C_OBJ += mros_protocol_publish_cimpl.o 55 | MROS_C_OBJ += mros_protocol_server_proc_cimpl.o 56 | MROS_C_OBJ += mros_protocol_slave_cimpl.o 57 | MROS_C_OBJ += mros_protocol_subscribe_cimpl.o 58 | MROS_C_OBJ += mros_topic_data_publisher_cimpl.o 59 | MROS_C_OBJ += mros_topic_data_subscriber_cimpl.o 60 | MROS_C_OBJ += mros_topic_cimpl.o 61 | MROS_C_OBJ += mros_topic_connector_cimpl.o 62 | MROS_C_OBJ += mros_topic_connector_factory_cimpl.o 63 | MROS_C_OBJ += mros_name.o 64 | CDEFS += -DMROS_LOG_DISABLE_WARN 65 | 66 | MROS_INCLUDE_PATHS += $(MROS_IFLAGS) 67 | 68 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/sensor_msgs/Image.h: -------------------------------------------------------------------------------- 1 | #ifndef _SENSOR_MSGS_IMAGE_H 2 | #define _SENSOR_MSGS_IMAGE_H 3 | 4 | #include "std_msgs/Header.h" 5 | #include 6 | 7 | 8 | static const int IMAGE_MSG_ID = 20; 9 | 10 | namespace sensor_msgs{ 11 | class Image{ 12 | public: 13 | std_msgs::Header header; 14 | uint32_t height; 15 | uint32_t width; 16 | std::string encoding; 17 | uint8_t is_bigendian; 18 | uint32_t step; 19 | std::vector data; 20 | 21 | int dataSize(){ 22 | return header.dataSize() + 4 + 4 + encoding.size() + 1 + 4 + data.size()*1 + 4 + 4*4; 23 | } 24 | 25 | void memCopy(char*& addrPtr){ 26 | int size; 27 | 28 | header.memCopy(addrPtr); 29 | 30 | memcpy(addrPtr,&height,4); 31 | addrPtr += 4; 32 | 33 | memcpy(addrPtr,&width,4); 34 | addrPtr += 4; 35 | 36 | size = encoding.size(); 37 | memcpy(addrPtr,&size,4); 38 | addrPtr += 4; 39 | memcpy(addrPtr, encoding.c_str(),size); 40 | addrPtr += size; 41 | 42 | memcpy(addrPtr,&is_bigendian,1); 43 | addrPtr += 1; 44 | 45 | memcpy(addrPtr,&step,4); 46 | addrPtr += 4; 47 | { 48 | size = data.size(); 49 | memcpy(addrPtr,&size,4); 50 | addrPtr += 4; 51 | const uint8_t* ptr = data.data(); 52 | for(int i=0; i 109 | struct MD5Sum 110 | { 111 | static const char* value() 112 | { 113 | return "060021388200f6f0f447d0fcd9c64743"; 114 | } 115 | 116 | }; 117 | 118 | template<> 119 | struct DataType 120 | { 121 | static const char* value() 122 | { 123 | return "sensor_msgs/Image"; 124 | } 125 | 126 | }; 127 | 128 | template<> 129 | struct DataTypeId 130 | { 131 | static int value() 132 | { 133 | return (int)IMAGE_MSG_ID; 134 | } 135 | 136 | }; 137 | 138 | template<> 139 | struct Definition 140 | { 141 | static const char* value() 142 | { 143 | return "std_msgs header\n\ 144 | uint32 height\n\ 145 | uint32 width\n\ 146 | string encoding\n\ 147 | uint8 is_bigendian\n\ 148 | uint32 step\n\ 149 | uint8[] data\n\ 150 | "; 151 | } 152 | }; 153 | } 154 | 155 | namespace subtask_methods 156 | { 157 | template<> 158 | struct CallCallbackFuncs{ 159 | static void call(void (*fp)(void *), char *rbuf) 160 | { 161 | sensor_msgs::Image msg; 162 | rbuf += 4; 163 | msg.deserialize(rbuf); 164 | fp(&msg); 165 | } 166 | }; 167 | } 168 | 169 | #endif 170 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/Header.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_HEADER_H 2 | #define _STD_MSGS_HEADER_H 3 | 4 | #include "std_msgs/Time.h" 5 | 6 | 7 | 8 | static const int HEADER_MSG_ID = 104; 9 | 10 | namespace std_msgs{ 11 | class Header{ 12 | public: 13 | uint32_t seq; 14 | std_msgs::Time stamp; 15 | std::string frame_id; 16 | 17 | int dataSize(){ 18 | return 4 + stamp.dataSize() + frame_id.size() + 4*3; 19 | } 20 | 21 | void memCopy(char*& addrPtr){ 22 | int size; 23 | 24 | memcpy(addrPtr,&seq,4); 25 | addrPtr += 4; 26 | 27 | stamp.memCopy(addrPtr); 28 | 29 | size = frame_id.size(); 30 | memcpy(addrPtr,&size,4); 31 | addrPtr += 4; 32 | memcpy(addrPtr, frame_id.c_str(),size); 33 | addrPtr += size; 34 | 35 | } 36 | 37 | void deserialize(char*& rbuf){ 38 | uint32_t size; 39 | memcpy(&seq,rbuf,4); 40 | rbuf += 4; 41 | 42 | 43 | stamp.deserialize(rbuf); 44 | 45 | { 46 | memcpy(&size,rbuf,4); 47 | rbuf += 4; 48 | char buf_char[size+1]; 49 | memcpy(&buf_char,rbuf,size); 50 | buf_char[size] = '\0'; 51 | frame_id = buf_char; 52 | rbuf += size; 53 | } 54 | 55 | 56 | } 57 | }; 58 | 59 | } 60 | 61 | namespace message_traits 62 | { 63 | template<> 64 | struct MD5Sum 65 | { 66 | static const char* value() 67 | { 68 | return "3695c7678a2b8f86015eccf2f844688c"; 69 | } 70 | 71 | }; 72 | 73 | template<> 74 | struct DataType 75 | { 76 | static const char* value() 77 | { 78 | return "std_msgs/Header"; 79 | } 80 | 81 | }; 82 | 83 | template<> 84 | struct DataTypeId 85 | { 86 | static int value() 87 | { 88 | return (int)HEADER_MSG_ID; 89 | } 90 | 91 | }; 92 | 93 | template<> 94 | struct Definition 95 | { 96 | static const char* value() 97 | { 98 | return "uint32 seq\n\ 99 | time stamp\n\ 100 | string frame_id\n\ 101 | "; 102 | } 103 | }; 104 | } 105 | 106 | namespace subtask_methods 107 | { 108 | template<> 109 | struct CallCallbackFuncs{ 110 | static void call(void (*fp)(void *), char *rbuf) 111 | { 112 | std_msgs::Header msg; 113 | rbuf += 4; 114 | msg.deserialize(rbuf); 115 | fp(&msg); 116 | } 117 | }; 118 | } 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/Int16.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_INT16_ 2 | #define _STD_MSGS_INT16_ 3 | 4 | #include 5 | 6 | static const int INT16_MSG_ID = 6; 7 | 8 | namespace std_msgs{ 9 | class Int16{ 10 | public: 11 | int16_t data; 12 | static const int id = INT16_MSG_ID; 13 | int dataSize(){return 2;} 14 | 15 | void memCopy(char *addrPtr){ 16 | memcpy(addrPtr,&data,2); 17 | } 18 | }; 19 | } 20 | 21 | namespace message_traits 22 | { 23 | 24 | template<> 25 | struct MD5Sum 26 | { 27 | static const char* value() 28 | { 29 | return "8524586e34fbd7cb1c08c5f5f1ca0e57"; 30 | } 31 | 32 | }; 33 | 34 | template<> 35 | struct DataType 36 | { 37 | static const char* value() 38 | { 39 | return "std_msgs/UInt16"; 40 | } 41 | 42 | }; 43 | 44 | template<> 45 | struct DataTypeId 46 | { 47 | static int value() 48 | { 49 | return (int)INT16_MSG_ID; 50 | } 51 | 52 | }; 53 | 54 | template<> 55 | struct Definition 56 | { 57 | static const char* value() 58 | { 59 | return "int16 data\n\ 60 | "; 61 | } 62 | }; 63 | } 64 | /* 65 | namespace subtask_methods 66 | { 67 | template<> 68 | struct CallCallbackFuncs{ 69 | static void call(void (*fp)(), char *rbuf) 70 | { 71 | std_msgs::UInt16 msg; 72 | msg.data = (int)rbuf[4] + (int)rbuf[5]*256; 73 | } 74 | }; 75 | } 76 | */ 77 | 78 | namespace subtask_methods 79 | { 80 | template<> 81 | struct CallCallbackFuncs{ 82 | static void call(void (*fp)(void *), char *rbuf) 83 | { 84 | std_msgs::Int16 msg; 85 | rbuf += 4; 86 | memcpy(&msg.data,rbuf,2); 87 | //msg.data = (int)rbuf[4] + (int)rbuf[5]*256; 88 | fp(&msg); 89 | } 90 | }; 91 | } 92 | #endif 93 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/Int16MultiArray.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | static const int INT16MULTIARRAY_MSG_ID = 12; 6 | 7 | namespace std_msgs{ 8 | class Int16MultiArray{ 9 | public: 10 | std::vector data; 11 | int dataSize(){return data.size() * 2+ 12;} 12 | void memCopy(char *addrPtr){ 13 | addrPtr += 8; 14 | uint32_t size = data.size(); 15 | memcpy(addrPtr,&size,4); 16 | addrPtr += 4; 17 | const int16_t* ptr = data.data(); 18 | for(int i=0; i 29 | struct MD5Sum 30 | { 31 | static const char* value() 32 | { 33 | return "d9338d7f523fcb692fae9d0a0e9f067c"; 34 | } 35 | 36 | }; 37 | 38 | template<> 39 | struct DataType 40 | { 41 | static const char* value() 42 | { 43 | return "std_msgs/Int16MultiArray"; 44 | } 45 | 46 | }; 47 | 48 | template<> 49 | struct DataTypeId 50 | { 51 | static int value() 52 | { 53 | return (int)INT16MULTIARRAY_MSG_ID; 54 | } 55 | 56 | }; 57 | 58 | template<> 59 | struct Definition 60 | { 61 | static const char* value() 62 | { 63 | return "int16[] data\n\\n\ 64 | "; 65 | } 66 | }; 67 | } 68 | 69 | namespace subtask_methods 70 | { 71 | template<> 72 | struct CallCallbackFuncs{ 73 | static void call(void (*fp)(void *), char *rbuf) 74 | { 75 | std_msgs::Int16MultiArray msg; 76 | rbuf += 4; 77 | rbuf += 8; 78 | int16_t buf_int; 79 | uint32_t arr_size; 80 | memcpy(&arr_size,rbuf,4 ); 81 | rbuf += 4; 82 | msg.data.reserve(arr_size); 83 | for(int i=0 ; i < arr_size ; i++){ 84 | memcpy(&buf_int,rbuf, 2); 85 | msg.data.push_back(buf_int); 86 | rbuf += 2; 87 | } 88 | fp(&msg); 89 | } 90 | }; 91 | } 92 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/Int32.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_INT32_H 2 | #define _STD_MSGS_INT32_H 3 | 4 | #include 5 | 6 | static const int INT32_MSG_ID = 7; 7 | 8 | namespace std_msgs{ 9 | class Int32{ 10 | public: 11 | int32_t data; 12 | int dataSize(){return 4;} 13 | void memCopy(char *addrPtr){ 14 | memcpy(addrPtr,&data,4); 15 | } 16 | }; 17 | } 18 | 19 | namespace message_traits 20 | { 21 | template<> 22 | struct MD5Sum 23 | { 24 | static const char* value() 25 | { 26 | return "da5909fbe378aeaf85e547e830cc1bb7"; 27 | } 28 | 29 | }; 30 | 31 | template<> 32 | struct DataType 33 | { 34 | static const char* value() 35 | { 36 | return "std_msgs/Int32"; 37 | } 38 | 39 | }; 40 | 41 | template<> 42 | struct DataTypeId 43 | { 44 | static int value() 45 | { 46 | return (int)INT32_MSG_ID; 47 | } 48 | 49 | }; 50 | 51 | template<> 52 | struct Definition 53 | { 54 | static const char* value() 55 | { 56 | return "int32 data\n\\n\ 57 | "; 58 | } 59 | }; 60 | } 61 | 62 | namespace subtask_methods 63 | { 64 | template<> 65 | struct CallCallbackFuncs{ 66 | static void call(void (*fp)(void *), char *rbuf) 67 | { 68 | std_msgs::Int32 msg; 69 | rbuf += 4; 70 | memcpy(&msg.data,rbuf,4); 71 | //msg.data = (unsigned int)rbuf[4] + (unsigned int)rbuf[5]*256 + (unsigned int)rbuf[6]*65536 + (int)rbuf[7]*16777216; 72 | fp(&msg); 73 | } 74 | }; 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/Int32MultiArray.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | static const int INT32MULTIARRAY_MSG_ID = 13; 5 | 6 | namespace std_msgs{ 7 | class Int32MultiArray{ 8 | public: 9 | std::vector data; 10 | int dataSize(){return data.size() * 4 + 12;} 11 | void memCopy(char *addrPtr){ 12 | addrPtr += 8; 13 | uint32_t size = data.size(); 14 | memcpy(addrPtr,&size,4); 15 | addrPtr += 4; 16 | const int32_t* ptr = data.data(); 17 | for(int i=0; i 28 | struct MD5Sum 29 | { 30 | static const char* value() 31 | { 32 | return "1d99f79f8b325b44fee908053e9c945b"; 33 | } 34 | 35 | }; 36 | 37 | template<> 38 | struct DataType 39 | { 40 | static const char* value() 41 | { 42 | return "std_msgs/Int32MultiArray"; 43 | } 44 | 45 | }; 46 | 47 | template<> 48 | struct DataTypeId 49 | { 50 | static int value() 51 | { 52 | return (int)INT32MULTIARRAY_MSG_ID; 53 | } 54 | 55 | }; 56 | 57 | template<> 58 | struct Definition 59 | { 60 | static const char* value() 61 | { 62 | return "int32 data\n\\n\ 63 | "; 64 | } 65 | }; 66 | } 67 | 68 | namespace subtask_methods 69 | { 70 | template<> 71 | struct CallCallbackFuncs{ 72 | static void call(void (*fp)(void *), char *rbuf) 73 | { 74 | std_msgs::Int32MultiArray msg; 75 | rbuf += 4; 76 | rbuf += 8; 77 | int32_t buf_int; 78 | uint32_t arr_size; 79 | memcpy(&arr_size,rbuf,4 ); 80 | rbuf += 4; 81 | msg.data.reserve(arr_size); 82 | for(int i=0 ; i < arr_size ; i++){ 83 | memcpy(&buf_int,rbuf, 4); 84 | msg.data.push_back(buf_int); 85 | rbuf += 4; 86 | } 87 | fp(&msg); 88 | } 89 | }; 90 | } 91 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/Int64MultiArray.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | static const int INT64MULTIARRAY_MSG_ID = 14; 5 | 6 | namespace std_msgs{ 7 | class Int64MultiArray{ 8 | public: 9 | std::vector data; 10 | int dataSize(){return data.size() * 8+ 12;} 11 | void memCopy(char *addrPtr){ 12 | addrPtr += 8; 13 | uint32_t size = data.size(); 14 | memcpy(addrPtr,&size,4); 15 | addrPtr += 4; 16 | const int64_t* ptr = data.data(); 17 | for(int i=0; i 28 | struct MD5Sum 29 | { 30 | static const char* value() 31 | { 32 | return "54865aa6c65be0448113a2afc6a49270"; 33 | } 34 | 35 | }; 36 | 37 | template<> 38 | struct DataType 39 | { 40 | static const char* value() 41 | { 42 | return "std_msgs/Int64MultiArray"; 43 | } 44 | 45 | }; 46 | 47 | template<> 48 | struct DataTypeId 49 | { 50 | static int value() 51 | { 52 | return (int)INT64MULTIARRAY_MSG_ID; 53 | } 54 | 55 | }; 56 | 57 | template<> 58 | struct Definition 59 | { 60 | static const char* value() 61 | { 62 | return "int64[] data\n\\n\ 63 | "; 64 | } 65 | }; 66 | } 67 | 68 | namespace subtask_methods 69 | { 70 | template<> 71 | struct CallCallbackFuncs{ 72 | static void call(void (*fp)(void *), char *rbuf) 73 | { 74 | std_msgs::Int64MultiArray msg; 75 | rbuf += 4; 76 | rbuf += 8; 77 | int64_t buf_int; 78 | uint32_t arr_size; 79 | memcpy(&arr_size,rbuf,4 ); 80 | rbuf += 4; 81 | msg.data.reserve(arr_size); 82 | for(int i=0 ; i < arr_size ; i++){ 83 | memcpy(&buf_int,rbuf, 8); 84 | msg.data.push_back(buf_int); 85 | rbuf += 8; 86 | } 87 | fp(&msg); 88 | } 89 | }; 90 | } 91 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/Int8.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_INT8_H 2 | #define _STD_MSGS_INT8_H 3 | 4 | #include 5 | 6 | static const int INT8_MSG_ID = 1; 7 | 8 | namespace std_msgs{ 9 | class Int8{ 10 | public: 11 | int8_t data; 12 | int dataSize(){return 1;} 13 | void memCopy(char *addrPtr){ 14 | memcpy(addrPtr,&data,1); 15 | } 16 | }; 17 | } 18 | 19 | namespace message_traits 20 | { 21 | template<> 22 | struct MD5Sum 23 | { 24 | static const char* value() 25 | { 26 | return "27ffa0c9c4b8fb8492252bcad9e5c57b"; 27 | } 28 | 29 | }; 30 | 31 | template<> 32 | struct DataType 33 | { 34 | static const char* value() 35 | { 36 | return "std_msgs/UInt8"; 37 | } 38 | 39 | }; 40 | 41 | template<> 42 | struct DataTypeId 43 | { 44 | static int value() 45 | { 46 | return (int)INT8_MSG_ID; 47 | } 48 | 49 | }; 50 | 51 | template<> 52 | struct Definition 53 | { 54 | static const char* value() 55 | { 56 | return "int8 data\n\\n\ 57 | "; 58 | } 59 | }; 60 | } 61 | 62 | namespace subtask_methods 63 | { 64 | template<> 65 | struct CallCallbackFuncs{ 66 | static void call(void (*fp)(void *), char *rbuf) 67 | { 68 | std_msgs::Int8 msg; 69 | rbuf += 4; 70 | memcpy(&msg.data,rbuf,1); 71 | //msg.data = (int)rbuf[4]; 72 | fp(&msg); 73 | } 74 | }; 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/Int8MultiArray.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | static const int INT8MULTIARRAY_MSG_ID = 11; 5 | 6 | namespace std_msgs{ 7 | class Int8MultiArray{ 8 | public: 9 | std::vector data; 10 | int dataSize(){return data.size() * 1 + 12;} 11 | void memCopy(char *addrPtr){ 12 | addrPtr += 8; 13 | uint32_t size = data.size(); 14 | memcpy(addrPtr,&size,4); 15 | addrPtr += 4; 16 | const int8_t* ptr = data.data(); 17 | for(int i=0; i 28 | struct MD5Sum 29 | { 30 | static const char* value() 31 | { 32 | return "d7c1af35a1b4781bbe79e03dd94b7c13"; 33 | } 34 | 35 | }; 36 | 37 | template<> 38 | struct DataType 39 | { 40 | static const char* value() 41 | { 42 | return "std_msgs/Int8MultiArray"; 43 | } 44 | 45 | }; 46 | 47 | template<> 48 | struct DataTypeId 49 | { 50 | static int value() 51 | { 52 | return (int)INT8MULTIARRAY_MSG_ID; 53 | } 54 | 55 | }; 56 | 57 | template<> 58 | struct Definition 59 | { 60 | static const char* value() 61 | { 62 | return "int8[] data\n\\n\ 63 | "; 64 | } 65 | }; 66 | } 67 | 68 | namespace subtask_methods 69 | { 70 | template<> 71 | struct CallCallbackFuncs{ 72 | static void call(void (*fp)(void *), char *rbuf) 73 | { 74 | std_msgs::Int8MultiArray msg; 75 | rbuf += 4; 76 | rbuf += 8; 77 | int8_t buf_int; 78 | uint32_t arr_size; 79 | memcpy(&arr_size,rbuf,4 ); 80 | rbuf += 4; 81 | msg.data.reserve(arr_size); 82 | for(int i=0 ; i < arr_size ; i++){ 83 | memcpy(&buf_int,rbuf, 1); 84 | msg.data.push_back(buf_int); 85 | rbuf += 1; 86 | } 87 | fp(&msg); 88 | } 89 | }; 90 | } 91 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/String.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_STRING_ 2 | #define _STD_MSGS_STRING_ 3 | 4 | #include 5 | 6 | static const int STRING_MSG_ID = 9; 7 | 8 | namespace std_msgs{ 9 | class String{ 10 | public: 11 | std::string data; 12 | int dataSize(){return data.size() + 4;} 13 | 14 | void memCopy(char *addrPtr){ 15 | int size; 16 | size = data.size(); 17 | memcpy(addrPtr, &size, 4); 18 | addrPtr += 4; 19 | memcpy(addrPtr, data.c_str(), size); 20 | } 21 | }; 22 | } 23 | 24 | namespace message_traits 25 | { 26 | template<> 27 | struct MD5Sum 28 | { 29 | static const char* value() 30 | { 31 | return "992ce8a1687cec8c8bd883ec73ca41d1"; 32 | } 33 | 34 | }; 35 | 36 | template<> 37 | struct DataType 38 | { 39 | static const char* value() 40 | { 41 | return "std_msgs/String"; 42 | } 43 | 44 | }; 45 | 46 | template<> 47 | struct DataTypeId 48 | { 49 | static int value() 50 | { 51 | return (int)STRING_MSG_ID; 52 | } 53 | 54 | }; 55 | 56 | template<> 57 | struct Definition 58 | { 59 | static const char* value() 60 | { 61 | return "string data\n\ 62 | "; 63 | } 64 | }; 65 | 66 | } 67 | 68 | namespace subtask_methods 69 | { 70 | template<> 71 | struct CallCallbackFuncs{ 72 | static void call(void (*fp)(void *), char *rbuf) 73 | { 74 | std_msgs::String msg; 75 | int size; 76 | memcpy((char*)&size, &rbuf[4], 4); 77 | std::string str_msg((const char*)&rbuf[8], size); 78 | 79 | msg.data = str_msg; 80 | fp(&msg); 81 | } 82 | }; 83 | } 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/Time.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_TIME_H 2 | #define _STD_MSGS_TIME_H 3 | 4 | #include 5 | 6 | 7 | 8 | static const int TIME_MSG_ID = 103; 9 | 10 | namespace std_msgs{ 11 | class Time{ 12 | public: 13 | uint32_t sec; 14 | uint32_t nsec; 15 | 16 | int dataSize(){ 17 | return 4 + 4 + 4*2; 18 | } 19 | 20 | void memCopy(char*& addrPtr){ 21 | int size; 22 | 23 | memcpy(addrPtr,&sec,4); 24 | addrPtr += 4; 25 | 26 | memcpy(addrPtr,&nsec,4); 27 | addrPtr += 4; 28 | 29 | } 30 | 31 | void deserialize(char*& rbuf){ 32 | uint32_t size; 33 | memcpy(&sec,rbuf,4); 34 | rbuf += 4; 35 | 36 | memcpy(&nsec,rbuf,4); 37 | rbuf += 4; 38 | 39 | 40 | } 41 | }; 42 | 43 | } 44 | 45 | namespace message_traits 46 | { 47 | template<> 48 | struct MD5Sum 49 | { 50 | static const char* value() 51 | { 52 | return "4771ad66fef816d2e4bead2f45a1cde6"; 53 | } 54 | 55 | }; 56 | 57 | template<> 58 | struct DataType 59 | { 60 | static const char* value() 61 | { 62 | return "std_msgs/Time"; 63 | } 64 | 65 | }; 66 | 67 | template<> 68 | struct DataTypeId 69 | { 70 | static int value() 71 | { 72 | return (int)TIME_MSG_ID; 73 | } 74 | 75 | }; 76 | 77 | template<> 78 | struct Definition 79 | { 80 | static const char* value() 81 | { 82 | return "uint32 sec\n\ 83 | uint32 nsec\n\ 84 | "; 85 | } 86 | }; 87 | } 88 | 89 | namespace subtask_methods 90 | { 91 | template<> 92 | struct CallCallbackFuncs{ 93 | static void call(void (*fp)(void *), char *rbuf) 94 | { 95 | std_msgs::Time msg; 96 | rbuf += 4; 97 | msg.deserialize(rbuf); 98 | fp(&msg); 99 | } 100 | }; 101 | } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/UInt16.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_UINT16_ 2 | #define _STD_MSGS_UINT16_ 3 | 4 | #include 5 | 6 | static const int UINT16_MSG_ID = 6; 7 | 8 | namespace std_msgs{ 9 | class UInt16{ 10 | public: 11 | uint16_t data; 12 | static const int id = UINT16_MSG_ID; 13 | int dataSize(){return 2;} 14 | 15 | void memCopy(char *addrPtr){ 16 | memcpy(addrPtr,&data,2); 17 | } 18 | }; 19 | } 20 | 21 | namespace message_traits 22 | { 23 | 24 | template<> 25 | struct MD5Sum 26 | { 27 | static const char* value() 28 | { 29 | return "1df79edf208b629fe6b81923a544552d"; 30 | } 31 | 32 | }; 33 | 34 | template<> 35 | struct DataType 36 | { 37 | static const char* value() 38 | { 39 | return "std_msgs/UInt16"; 40 | } 41 | 42 | }; 43 | 44 | template<> 45 | struct DataTypeId 46 | { 47 | static int value() 48 | { 49 | return (int)UINT16_MSG_ID; 50 | } 51 | 52 | }; 53 | 54 | template<> 55 | struct Definition 56 | { 57 | static const char* value() 58 | { 59 | return "uint16 data\n\ 60 | "; 61 | } 62 | }; 63 | } 64 | /* 65 | namespace subtask_methods 66 | { 67 | template<> 68 | struct CallCallbackFuncs{ 69 | static void call(void (*fp)(), char *rbuf) 70 | { 71 | std_msgs::UInt16 msg; 72 | msg.data = (int)rbuf[4] + (int)rbuf[5]*256; 73 | } 74 | }; 75 | } 76 | */ 77 | 78 | namespace subtask_methods 79 | { 80 | template<> 81 | struct CallCallbackFuncs{ 82 | static void call(void (*fp)(void *), char *rbuf) 83 | { 84 | std_msgs::UInt16 msg; 85 | rbuf += 4; 86 | memcpy(&msg.data,rbuf,2); 87 | //msg.data = (int)rbuf[4] + (int)rbuf[5]*256; 88 | fp(&msg); 89 | } 90 | }; 91 | } 92 | #endif 93 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/UInt16MultiArray.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | static const int UINT16MULTIARRAY_MSG_ID = 16; 5 | 6 | namespace std_msgs{ 7 | class UInt16MultiArray{ 8 | public: 9 | std::vector data; 10 | int dataSize(){return data.size() * 2+ 12;} 11 | void memCopy(char *addrPtr){ 12 | addrPtr += 8; 13 | uint32_t size = data.size(); 14 | memcpy(addrPtr,&size,4); 15 | addrPtr += 4; 16 | const uint16_t* ptr = data.data(); 17 | for(int i=0; i 28 | struct MD5Sum 29 | { 30 | static const char* value() 31 | { 32 | return "52f264f1c973c4b73790d384c6cb4484"; 33 | } 34 | 35 | }; 36 | 37 | template<> 38 | struct DataType 39 | { 40 | static const char* value() 41 | { 42 | return "std_msgs/UInt16MultiArray"; 43 | } 44 | 45 | }; 46 | 47 | template<> 48 | struct DataTypeId 49 | { 50 | static int value() 51 | { 52 | return (int)UINT16MULTIARRAY_MSG_ID; 53 | } 54 | 55 | }; 56 | 57 | template<> 58 | struct Definition 59 | { 60 | static const char* value() 61 | { 62 | return "uint16[] data\n\\n\ 63 | "; 64 | } 65 | }; 66 | } 67 | 68 | namespace subtask_methods 69 | { 70 | template<> 71 | struct CallCallbackFuncs{ 72 | static void call(void (*fp)(void *), char *rbuf) 73 | { 74 | std_msgs::UInt16MultiArray msg; 75 | rbuf += 4; 76 | rbuf += 8; 77 | uint16_t buf_int; 78 | uint32_t arr_size; 79 | memcpy(&arr_size,rbuf,4 ); 80 | rbuf += 4; 81 | msg.data.reserve(arr_size); 82 | for(int i=0 ; i < arr_size ; i++){ 83 | memcpy(&buf_int,rbuf, 2); 84 | msg.data.push_back(buf_int); 85 | rbuf += 2; 86 | } 87 | fp(&msg); 88 | } 89 | }; 90 | } 91 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/UInt32.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_UINT32_H 2 | #define _STD_MSGS_UINT32_H 3 | 4 | #include 5 | 6 | static const int UINT32_MSG_ID = 7; 7 | 8 | namespace std_msgs{ 9 | class UInt32{ 10 | public: 11 | uint32_t data; 12 | int dataSize(){return 4;} 13 | void memCopy(char *addrPtr){ 14 | memcpy(addrPtr,&data,4); 15 | } 16 | }; 17 | } 18 | 19 | namespace message_traits 20 | { 21 | template<> 22 | struct MD5Sum 23 | { 24 | static const char* value() 25 | { 26 | return "304a39449588c7f8ce2df6e8001c5fce"; 27 | } 28 | 29 | }; 30 | 31 | template<> 32 | struct DataType 33 | { 34 | static const char* value() 35 | { 36 | return "std_msgs/UInt32"; 37 | } 38 | 39 | }; 40 | 41 | template<> 42 | struct DataTypeId 43 | { 44 | static int value() 45 | { 46 | return (int)UINT32_MSG_ID; 47 | } 48 | 49 | }; 50 | 51 | template<> 52 | struct Definition 53 | { 54 | static const char* value() 55 | { 56 | return "uint32 data\n\\n\ 57 | "; 58 | } 59 | }; 60 | } 61 | 62 | namespace subtask_methods 63 | { 64 | template<> 65 | struct CallCallbackFuncs{ 66 | static void call(void (*fp)(void *), char *rbuf) 67 | { 68 | std_msgs::UInt32 msg; 69 | rbuf += 4; 70 | memcpy(&msg.data,rbuf,4); 71 | //msg.data = (unsigned int)rbuf[4] + (unsigned int)rbuf[5]*256 + (unsigned int)rbuf[6]*65536 + (int)rbuf[7]*16777216; 72 | fp(&msg); 73 | } 74 | }; 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/UInt32MultiArray.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | static const int UINT32MULTIARRAY_MSG_ID = 17; 5 | 6 | namespace std_msgs{ 7 | class UInt32MultiArray{ 8 | public: 9 | std::vector data; 10 | int dataSize(){return data.size() * 4 + 12;} 11 | void memCopy(char *addrPtr){ 12 | addrPtr += 8; 13 | uint32_t size = data.size(); 14 | memcpy(addrPtr,&size,4); 15 | addrPtr += 4; 16 | const uint32_t* ptr = data.data(); 17 | for(int i=0; i 28 | struct MD5Sum 29 | { 30 | static const char* value() 31 | { 32 | return "4d6a180abc9be191b96a7eda6c8a233d"; 33 | } 34 | 35 | }; 36 | 37 | template<> 38 | struct DataType 39 | { 40 | static const char* value() 41 | { 42 | return "std_msgs/UInt32MultiArray"; 43 | } 44 | 45 | }; 46 | 47 | template<> 48 | struct DataTypeId 49 | { 50 | static int value() 51 | { 52 | return (int)UINT32MULTIARRAY_MSG_ID; 53 | } 54 | 55 | }; 56 | 57 | template<> 58 | struct Definition 59 | { 60 | static const char* value() 61 | { 62 | return "int32 data\n\\n\ 63 | "; 64 | } 65 | }; 66 | } 67 | 68 | namespace subtask_methods 69 | { 70 | template<> 71 | struct CallCallbackFuncs{ 72 | static void call(void (*fp)(void *), char *rbuf) 73 | { 74 | std_msgs::UInt32MultiArray msg; 75 | rbuf += 4; 76 | rbuf += 8; 77 | uint32_t buf_int; 78 | uint32_t arr_size; 79 | memcpy(&arr_size,rbuf,4 ); 80 | rbuf += 4; 81 | msg.data.reserve(arr_size); 82 | for(int i=0 ; i < arr_size ; i++){ 83 | memcpy(&buf_int,rbuf, 4); 84 | msg.data.push_back(buf_int); 85 | rbuf += 4; 86 | } 87 | fp(&msg); 88 | } 89 | }; 90 | } 91 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/UInt64.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_UINT64_H 2 | #define _STD_MSGS_UINT64_H 3 | 4 | #include 5 | 6 | static const int UINT64_MSG_ID = 8; 7 | 8 | namespace std_msgs{ 9 | class UInt64{ 10 | public: 11 | uint64_t data; 12 | int dataSize(){return 8;} 13 | void memCopy(char *addrPtr){ 14 | memcpy(addrPtr,&data,8); 15 | } 16 | }; 17 | } 18 | 19 | namespace message_traits 20 | { 21 | template<> 22 | struct MD5Sum 23 | { 24 | static const char* value() 25 | { 26 | return "1b2a79973e8bf53d7b53acb71299cb57"; 27 | } 28 | 29 | }; 30 | 31 | template<> 32 | struct DataType 33 | { 34 | static const char* value() 35 | { 36 | return "std_msgs/UInt64"; 37 | } 38 | 39 | }; 40 | 41 | template<> 42 | struct DataTypeId 43 | { 44 | static int value() 45 | { 46 | return (int)UINT64_MSG_ID; 47 | } 48 | 49 | }; 50 | 51 | template<> 52 | struct Definition 53 | { 54 | static const char* value() 55 | { 56 | return "int64 data\n\\n\ 57 | "; 58 | } 59 | }; 60 | } 61 | 62 | namespace subtask_methods 63 | { 64 | template<> 65 | struct CallCallbackFuncs{ 66 | static void call(void (*fp)(void *), char *rbuf) 67 | { 68 | std_msgs::UInt64 msg; 69 | rbuf += 4; 70 | memcpy(&msg.data,rbuf,8); 71 | //msg.data = &rbuf[8]; 72 | fp(&msg); 73 | } 74 | }; 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/UInt64MultiArray.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | static const int UINT64MULTIARRAY_MSG_ID = 18; 5 | 6 | namespace std_msgs{ 7 | class UInt64MultiArray{ 8 | public: 9 | std::vector data; 10 | int dataSize(){return data.size() * 8+ 12;} 11 | void memCopy(char *addrPtr){ 12 | addrPtr += 8; 13 | uint32_t size = data.size(); 14 | memcpy(addrPtr,&size,4); 15 | addrPtr += 4; 16 | const uint64_t* ptr = data.data(); 17 | for(int i=0; i 28 | struct MD5Sum 29 | { 30 | static const char* value() 31 | { 32 | return "6088f127afb1d6c72927aa1247e945af"; 33 | } 34 | 35 | }; 36 | 37 | template<> 38 | struct DataType 39 | { 40 | static const char* value() 41 | { 42 | return "std_msgs/UInt64MultiArray"; 43 | } 44 | 45 | }; 46 | 47 | template<> 48 | struct DataTypeId 49 | { 50 | static int value() 51 | { 52 | return (int)UINT64MULTIARRAY_MSG_ID; 53 | } 54 | 55 | }; 56 | 57 | template<> 58 | struct Definition 59 | { 60 | static const char* value() 61 | { 62 | return "uint64[] data\n\\n\ 63 | "; 64 | } 65 | }; 66 | } 67 | 68 | namespace subtask_methods 69 | { 70 | template<> 71 | struct CallCallbackFuncs{ 72 | static void call(void (*fp)(void *), char *rbuf) 73 | { 74 | std_msgs::UInt64MultiArray msg; 75 | rbuf += 4; 76 | rbuf += 8; 77 | uint64_t buf_int; 78 | uint32_t arr_size; 79 | memcpy(&arr_size,rbuf,4 ); 80 | rbuf += 4; 81 | msg.data.reserve(arr_size); 82 | for(int i=0 ; i < arr_size ; i++){ 83 | memcpy(&buf_int,rbuf, 8); 84 | msg.data.push_back(buf_int); 85 | rbuf += 8; 86 | } 87 | fp(&msg); 88 | } 89 | }; 90 | } 91 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/UInt8.h: -------------------------------------------------------------------------------- 1 | #ifndef _STD_MSGS_UINT8_H 2 | #define _STD_MSGS_UINT8_H 3 | 4 | #include 5 | 6 | static const int UINT8_MSG_ID = 5; 7 | 8 | namespace std_msgs{ 9 | class UInt8{ 10 | public: 11 | uint8_t data; 12 | int dataSize(){return 1;} 13 | void memCopy(char *addrPtr){ 14 | memcpy(addrPtr,&data,1); 15 | } 16 | }; 17 | } 18 | 19 | namespace message_traits 20 | { 21 | template<> 22 | struct MD5Sum 23 | { 24 | static const char* value() 25 | { 26 | return "7c8164229e7d2c17eb95e9231617fdee"; 27 | } 28 | 29 | }; 30 | 31 | template<> 32 | struct DataType 33 | { 34 | static const char* value() 35 | { 36 | return "std_msgs/UInt8"; 37 | } 38 | 39 | }; 40 | 41 | template<> 42 | struct DataTypeId 43 | { 44 | static int value() 45 | { 46 | return (int)UINT8_MSG_ID; 47 | } 48 | 49 | }; 50 | 51 | template<> 52 | struct Definition 53 | { 54 | static const char* value() 55 | { 56 | return "int8 data\n\\n\ 57 | "; 58 | } 59 | }; 60 | } 61 | 62 | namespace subtask_methods 63 | { 64 | template<> 65 | struct CallCallbackFuncs{ 66 | static void call(void (*fp)(void *), char *rbuf) 67 | { 68 | std_msgs::UInt8 msg; 69 | rbuf += 4; 70 | memcpy(&msg.data,rbuf,1); 71 | //msg.data = (int)rbuf[4]; 72 | fp(&msg); 73 | } 74 | }; 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /mros-lib/mros-msgs/std_msgs/UInt8MultiArray.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | static const int UINT8MULTIARRAY_MSG_ID = 15; 5 | 6 | namespace std_msgs{ 7 | class UInt8MultiArray{ 8 | public: 9 | std::vector data; 10 | int dataSize(){return data.size() * 1 + 12;} 11 | void memCopy(char *addrPtr){ 12 | addrPtr += 8; 13 | uint32_t size = data.size(); 14 | memcpy(addrPtr,&size,4); 15 | addrPtr += 4; 16 | const uint8_t* ptr = data.data(); 17 | for(int i=0; i 28 | struct MD5Sum 29 | { 30 | static const char* value() 31 | { 32 | return "82373f1612381bb6ee473b5cd6f5d89c"; 33 | } 34 | 35 | }; 36 | 37 | template<> 38 | struct DataType 39 | { 40 | static const char* value() 41 | { 42 | return "std_msgs/UInt8MultiArray"; 43 | } 44 | 45 | }; 46 | 47 | template<> 48 | struct DataTypeId 49 | { 50 | static int value() 51 | { 52 | return (int)UINT8MULTIARRAY_MSG_ID; 53 | } 54 | 55 | }; 56 | 57 | template<> 58 | struct Definition 59 | { 60 | static const char* value() 61 | { 62 | return "uint8[] data\n\\n\ 63 | "; 64 | } 65 | }; 66 | } 67 | 68 | namespace subtask_methods 69 | { 70 | template<> 71 | struct CallCallbackFuncs{ 72 | static void call(void (*fp)(void *), char *rbuf) 73 | { 74 | std_msgs::UInt8MultiArray msg; 75 | rbuf += 4; 76 | rbuf += 8; 77 | uint8_t buf_int; 78 | uint32_t arr_size; 79 | memcpy(&arr_size,rbuf,4 ); 80 | rbuf += 4; 81 | msg.data.reserve(arr_size); 82 | for(int i=0 ; i < arr_size ; i++){ 83 | memcpy(&buf_int,rbuf, 1); 84 | msg.data.push_back(buf_int); 85 | rbuf += 1; 86 | } 87 | fp(&msg); 88 | } 89 | }; 90 | } 91 | -------------------------------------------------------------------------------- /mros-lib/mros-src/api/mros_topic_callback.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_INTEGRATION_H_ 2 | #define _MROS_INTEGRATION_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | extern void mros_topic_callback(mros_uint32 topic_id, mros_uint32 type_id, mRosFuncIdType func_id, const char *data, mros_uint32 len); 11 | 12 | 13 | #ifdef __cplusplus 14 | } 15 | #endif 16 | 17 | 18 | #endif /* _MROS_INTEGRATION_H_ */ 19 | -------------------------------------------------------------------------------- /mros-lib/mros-src/api/ros.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROS_H_ 2 | #define _ROS_H_ 3 | 4 | #include 5 | 6 | namespace ros { 7 | 8 | void init(int argc, char *argv, std::string node_name); 9 | 10 | 11 | class Publisher { 12 | public: 13 | template 14 | void publish(T& data); 15 | void set(void *cobj) 16 | { 17 | this->cobj = cobj; 18 | } 19 | void* get() 20 | { 21 | return this->cobj; 22 | } 23 | private: 24 | void *cobj; 25 | }; 26 | 27 | class Subscriber { 28 | public: 29 | void set(void *cobj) 30 | { 31 | this->cobj = cobj; 32 | } 33 | void* get() 34 | { 35 | return this->cobj; 36 | } 37 | private: 38 | void *cobj; 39 | }; 40 | 41 | 42 | class NodeHandle { 43 | public: 44 | template 45 | Subscriber subscribe(std::string topic, int queue_size, void (*fp) (T)); 46 | template 47 | Publisher advertise(std::string topic, int queue_size); 48 | }; 49 | 50 | #define ROS_RATE_RATE_SEC_UNIT 1000 51 | class Rate{ 52 | public: 53 | Rate(int rate) 54 | { 55 | this->rate = rate; 56 | }; 57 | void sleep(void); 58 | private: 59 | int rate; 60 | }; 61 | 62 | void spin(void); 63 | 64 | } 65 | 66 | namespace message_traits 67 | { 68 | template 69 | struct MD5Sum{static const char* value();}; 70 | 71 | template 72 | struct DataType{static const char* value();}; 73 | 74 | template 75 | struct DataTypeId{static int value(void);}; 76 | 77 | template 78 | struct Definition{static const char* value();}; 79 | } 80 | 81 | namespace subtask_methods 82 | { 83 | template 84 | struct CallCallbackFuncs{static void call(void (*fp)(void *), char *rbuf);}; 85 | } 86 | 87 | #include "mros_log.h" 88 | 89 | #endif /* _ROS_H_ */ 90 | -------------------------------------------------------------------------------- /mros-lib/mros-src/comm/cimpl/mros_comm_socket_cimpl.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 mbed.org, MIT License 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 4 | * and associated documentation files (the "Software"), to deal in the Software without restriction, 5 | * including without limitation the rights to use, copy, modify, merge, publish, distribute, 6 | * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 7 | * furnished to do so, subject to the following conditions: 8 | * 9 | * The above copyright notice and this permission notice shall be included in all copies or 10 | * substantial portions of the Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 13 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 14 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 17 | */ 18 | #ifndef _MROS_COMM_SOCKET_CIMPL_H_ 19 | #define _MROS_COMM_SOCKET_CIMPL_H_ 20 | 21 | #ifdef __cplusplus 22 | extern "C" { 23 | #endif 24 | 25 | #include "mros_comm_cimpl.h" 26 | 27 | typedef struct { 28 | mros_int32 sock_fd; 29 | mros_uint32 timeout; 30 | mros_boolean blocking; 31 | mros_int32 comm_type; 32 | } mRosCommSocketType; 33 | 34 | static inline void mros_comm_set_timeval(mros_uint32 tmo_msec, mRosTimeValType *tv) 35 | { 36 | mros_uint32 tv_sec = tmo_msec / 1000; 37 | mros_uint32 tv_usec = (tmo_msec - (tv_sec * 1000)) * 1000; 38 | mros_comm_timeval_set(tv_sec, tv_usec, tv); 39 | return; 40 | } 41 | 42 | typedef enum { 43 | MROS_COMM_SOCKET_TYPE_TCP = 0, 44 | MROS_COMM_SOCKET_TYPE_UDP, 45 | } mRosCommSocketEnumType; 46 | 47 | #define MROS_COMM_DEFAULT_TIMEOUT 1500 48 | 49 | extern mRosReturnType mros_comm_socket_init(mRosCommSocketType *socket, mRosCommSocketEnumType type); 50 | extern mRosReturnType mros_comm_socket_open(mRosCommSocketType *socket); 51 | extern void mros_comm_socket_close(mRosCommSocketType *socket); 52 | extern mRosReturnType mros_comm_socket_set_blocking(mRosCommSocketType *socket, mros_boolean blocking, mros_uint32 timeout); 53 | 54 | extern mRosReturnType mros_comm_socket_wait_readable(mRosCommSocketType *socket, mros_uint32 timeout); 55 | extern mRosReturnType mros_comm_socket_wait_writable(mRosCommSocketType *socket, mros_uint32 timeout); 56 | 57 | #ifdef __cplusplus 58 | } 59 | #endif 60 | 61 | #endif /* _MROS_COMM_SOCKET_CIMPL_H_ */ 62 | -------------------------------------------------------------------------------- /mros-lib/mros-src/comm/cimpl/mros_comm_tcp_client_cimpl.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 mbed.org, MIT License 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 4 | * and associated documentation files (the "Software"), to deal in the Software without restriction, 5 | * including without limitation the rights to use, copy, modify, merge, publish, distribute, 6 | * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 7 | * furnished to do so, subject to the following conditions: 8 | * 9 | * The above copyright notice and this permission notice shall be included in all copies or 10 | * substantial portions of the Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 13 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 14 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 17 | */ 18 | #ifndef _MROS_COMM_TCP_CLIENT_CIMPL_H_ 19 | #define _MROS_COMM_TCP_CLIENT_CIMPL_H_ 20 | 21 | #ifdef __cplusplus 22 | extern "C" { 23 | #endif 24 | 25 | #include "mros_comm_socket_cimpl.h" 26 | 27 | typedef struct { 28 | mRosCommSocketType socket; 29 | mRosSockAddrInType remote; 30 | mros_boolean connected; 31 | } mRosCommTcpClientType; 32 | 33 | 34 | extern mRosReturnType mros_comm_tcp_client_init(mRosCommTcpClientType *client, const char* host, mros_int32 port); 35 | extern mRosReturnType mros_comm_tcp_client_ip32_init(mRosCommTcpClientType *client, mros_uint32 ipaddr, mros_int32 port); 36 | 37 | extern void mros_comm_tcp_client_close(mRosCommTcpClientType *client); 38 | extern mRosReturnType mros_comm_tcp_client_connect(mRosCommTcpClientType *client); 39 | extern mRosReturnType mros_comm_tcp_client_connect_ip32(mRosCommTcpClientType *client, mros_uint32 ipaddr, mros_int32 port); 40 | extern mros_boolean mros_comm_tcp_client_is_connected(mRosCommTcpClientType *client); 41 | extern mRosReturnType mros_comm_tcp_client_send(mRosCommTcpClientType *client, const char* data, mRosSizeType length, mRosSizeType *res); 42 | extern mRosReturnType mros_comm_tcp_client_send_all(mRosCommTcpClientType *client, const char* data, mRosSizeType length, mRosSizeType *res); 43 | extern mRosReturnType mros_comm_tcp_client_receive(mRosCommTcpClientType *client, char* data, mRosSizeType length, mRosSizeType *res); 44 | extern mRosReturnType mros_comm_tcp_client_receive_all(mRosCommTcpClientType *client, char* data, mRosSizeType length, mRosSizeType *res); 45 | 46 | 47 | #ifdef __cplusplus 48 | } 49 | #endif 50 | 51 | #endif /* _MROS_COMM_TCP_CLIENT_CIMPL_H_ */ 52 | -------------------------------------------------------------------------------- /mros-lib/mros-src/comm/cimpl/mros_comm_tcp_client_factory_cimpl.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 mbed.org, MIT License 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 4 | * and associated documentation files (the "Software"), to deal in the Software without restriction, 5 | * including without limitation the rights to use, copy, modify, merge, publish, distribute, 6 | * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 7 | * furnished to do so, subject to the following conditions: 8 | * 9 | * The above copyright notice and this permission notice shall be included in all copies or 10 | * substantial portions of the Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 13 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 14 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 17 | */ 18 | #include "mros_comm_tcp_client_factory_cimpl.h" 19 | #include "mros_sys_config.h" 20 | 21 | static mRosCommTcpClientEntryHeadType mros_comm_tcp_client_factory MROS_MATTR_BSS_NOCLR; 22 | static mRosCommTcpClientListReqEntryType mros_comm_tcp_client_entries[MROS_TOPIC_TCP_CLIENT_MAX_NUM] MROS_MATTR_BSS_NOCLR; 23 | 24 | mRosReturnType mros_comm_tcp_client_factory_init(void) 25 | { 26 | 27 | List_Init(&mros_comm_tcp_client_factory, mRosCommTcpClientListReqEntryType, MROS_TOPIC_TCP_CLIENT_MAX_NUM, mros_comm_tcp_client_entries); 28 | return MROS_E_OK; 29 | } 30 | 31 | mRosCommTcpClientListReqEntryType *mros_comm_tcp_client_alloc(void) 32 | { 33 | mRosCommTcpClientListReqEntryType *p; 34 | ListEntry_Alloc(&mros_comm_tcp_client_factory, mRosCommTcpClientListReqEntryType, &p); 35 | if (p == MROS_NULL) { 36 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, MROS_E_NOMEM); 37 | return MROS_NULL; 38 | } 39 | ListEntry_AddEntry(&mros_comm_tcp_client_factory, p); 40 | return p; 41 | } 42 | mRosCommTcpClientListReqEntryType *mros_comm_tcp_client_alloc_copy(mRosCommTcpClientType *client) 43 | { 44 | mRosCommTcpClientListReqEntryType *p; 45 | 46 | p = mros_comm_tcp_client_alloc(); 47 | if (p == MROS_NULL) { 48 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, MROS_E_NOMEM); 49 | return MROS_NULL; 50 | } 51 | p->data.client = *client; 52 | 53 | return p; 54 | } 55 | void mros_comm_tcp_client_free(mRosCommTcpClientListReqEntryType *client) 56 | { 57 | ListEntry_RemoveEntry(&mros_comm_tcp_client_factory, client); 58 | ListEntry_Free(&mros_comm_tcp_client_factory, client); 59 | return; 60 | } 61 | -------------------------------------------------------------------------------- /mros-lib/mros-src/comm/cimpl/mros_comm_tcp_client_factory_cimpl.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 mbed.org, MIT License 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 4 | * and associated documentation files (the "Software"), to deal in the Software without restriction, 5 | * including without limitation the rights to use, copy, modify, merge, publish, distribute, 6 | * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 7 | * furnished to do so, subject to the following conditions: 8 | * 9 | * The above copyright notice and this permission notice shall be included in all copies or 10 | * substantial portions of the Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 13 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 14 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 17 | */ 18 | #ifndef _MROS_COMM_TCP_CLIENT_FACTORY_CIMPL_H_ 19 | #define _MROS_COMM_TCP_CLIENT_FACTORY_CIMPL_H_ 20 | 21 | #ifdef __cplusplus 22 | extern "C" { 23 | #endif 24 | 25 | #include "mros_comm_tcp_client_cimpl.h" 26 | #include "mros_memory.h" 27 | #include "mros_wait_queue.h" 28 | 29 | typedef struct { 30 | mRosTopicIdType topic_id; 31 | mros_uint32 ipaddr; 32 | mros_int32 port; 33 | void* api_reqp; 34 | mRosWaitListEntryType waitobj; 35 | } mRosRquestObjectType; 36 | 37 | typedef struct { 38 | mRosReturnType (*topic_data_send) (mRosCommTcpClientType *client, const char *data, mRosSizeType datalen); 39 | mRosReturnType (*topic_data_receive) (mRosCommTcpClientType *client, mRosMemoryManagerType *mempool, mRosMemoryListEntryType **retp); 40 | void (*free) (void* reqp); 41 | } mRosCommOperationType; 42 | 43 | typedef struct { 44 | mRosCommTcpClientType client; 45 | mRosCommOperationType op; 46 | mRosRquestObjectType reqobj; 47 | } mRosCommTcpClientReqEntryType; 48 | typedef ListEntryType(mRosCommTcpClientListReqEntryType, mRosCommTcpClientReqEntryType) mRosCommTcpClientListReqEntryType; 49 | typedef ListHeadType(mRosCommTcpClientListReqEntryType) mRosCommTcpClientEntryHeadType; 50 | 51 | extern mRosReturnType mros_comm_tcp_client_factory_init(void); 52 | extern mRosCommTcpClientListReqEntryType *mros_comm_tcp_client_alloc(void); 53 | extern mRosCommTcpClientListReqEntryType *mros_comm_tcp_client_alloc_copy(mRosCommTcpClientType *client); 54 | extern void mros_comm_tcp_client_free(mRosCommTcpClientListReqEntryType *client); 55 | 56 | 57 | #ifdef __cplusplus 58 | } 59 | #endif 60 | 61 | #endif /* _MROS_COMM_TCP_CLIENT_FACTORY_CIMPL_H_ */ 62 | -------------------------------------------------------------------------------- /mros-lib/mros-src/comm/cimpl/mros_comm_tcp_server_cimpl.c: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 mbed.org, MIT License 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 4 | * and associated documentation files (the "Software"), to deal in the Software without restriction, 5 | * including without limitation the rights to use, copy, modify, merge, publish, distribute, 6 | * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 7 | * furnished to do so, subject to the following conditions: 8 | * 9 | * The above copyright notice and this permission notice shall be included in all copies or 10 | * substantial portions of the Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 13 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 14 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 17 | */ 18 | #include "mros_comm_tcp_server_cimpl.h" 19 | 20 | mRosReturnType mros_comm_tcp_server_init(mRosCommTcpServerType *server) 21 | { 22 | return mros_comm_socket_init(&server->socket, MROS_COMM_SOCKET_TYPE_TCP); 23 | } 24 | 25 | mRosReturnType mros_comm_tcp_server_bind(mRosCommTcpServerType *server, mros_int32 port) 26 | { 27 | mRosReturnType ret; 28 | mRosSockAddrInType addr; 29 | 30 | mros_comm_inet_local_sockaddr_init(&addr, port); 31 | ret = mros_comm_bind(server->socket.sock_fd, (mRosSockAddrType*)&addr, sizeof(mRosSockAddrInType)); 32 | if (ret != MROS_E_OK) { 33 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 34 | return MROS_E_SYSERR; 35 | } 36 | return MROS_E_OK; 37 | } 38 | mRosReturnType mros_comm_tcp_server_listen(mRosCommTcpServerType *server, mros_int32 max) 39 | { 40 | mRosReturnType ret; 41 | if (server->socket.sock_fd < 0) { 42 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, MROS_E_SYSERR); 43 | return MROS_E_SYSERR; 44 | } 45 | 46 | ret = mros_comm_listen(server->socket.sock_fd, max); 47 | if (ret != MROS_E_OK) { 48 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 49 | return MROS_E_SYSERR; 50 | } 51 | return MROS_E_OK; 52 | } 53 | 54 | mRosReturnType mros_comm_tcp_server_accept(mRosCommTcpServerType *server, mRosCommTcpClientType *client) 55 | { 56 | mRosReturnType ret; 57 | mRosSockAddrInType addr; 58 | 59 | if (server->socket.sock_fd < 0) { 60 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, MROS_E_SYSERR); 61 | return MROS_E_SYSERR; 62 | } 63 | if (server->socket.blocking == MROS_FALSE) { 64 | ret = mros_comm_socket_wait_readable(&server->socket, server->socket.timeout); 65 | if (ret != MROS_E_OK) { 66 | if (ret != MROS_E_NOENT) { 67 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 68 | } 69 | return ret; 70 | } 71 | } 72 | 73 | mRosSizeType len = sizeof(mRosSockAddrInType); 74 | client->socket.sock_fd = mros_comm_accept(server->socket.sock_fd, (mRosSockAddrType*)&addr, &len); 75 | if (client->socket.sock_fd < 0) { 76 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, MROS_E_SYSERR); 77 | return MROS_E_SYSERR; 78 | } 79 | int i=0; 80 | mros_comm_setsockopt(client->socket.sock_fd, IPPROTO_TCP, TCP_NODELAY, &i, sizeof(int)); 81 | client->socket.blocking = MROS_TRUE; 82 | client->socket.timeout = MROS_COMM_DEFAULT_TIMEOUT; 83 | client->connected = MROS_TRUE; 84 | 85 | return MROS_E_OK; 86 | } 87 | -------------------------------------------------------------------------------- /mros-lib/mros-src/comm/cimpl/mros_comm_tcp_server_cimpl.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 mbed.org, MIT License 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 4 | * and associated documentation files (the "Software"), to deal in the Software without restriction, 5 | * including without limitation the rights to use, copy, modify, merge, publish, distribute, 6 | * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 7 | * furnished to do so, subject to the following conditions: 8 | * 9 | * The above copyright notice and this permission notice shall be included in all copies or 10 | * substantial portions of the Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 13 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 14 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 17 | */ 18 | #ifndef _MROS_COMM_TCP_SERVER_CIMPL_H_ 19 | #define _MROS_COMM_TCP_SERVER_CIMPL_H_ 20 | 21 | #ifdef __cplusplus 22 | extern "C" { 23 | #endif 24 | 25 | #include "mros_comm_tcp_client_cimpl.h" 26 | 27 | typedef struct { 28 | mRosCommSocketType socket; 29 | } mRosCommTcpServerType; 30 | 31 | extern mRosReturnType mros_comm_tcp_server_init(mRosCommTcpServerType *server); 32 | extern mRosReturnType mros_comm_tcp_server_bind(mRosCommTcpServerType *server, mros_int32 port); 33 | #define MROS_COMM_TCP_SERVER_LISTEN_MAX_DEFAULT_VALUE 1 34 | extern mRosReturnType mros_comm_tcp_server_listen(mRosCommTcpServerType *server, mros_int32 max); 35 | extern mRosReturnType mros_comm_tcp_server_accept(mRosCommTcpServerType *server, mRosCommTcpClientType *client); 36 | 37 | #ifdef __cplusplus 38 | } 39 | #endif 40 | 41 | #endif /* _MROS_COMM_TCP_SERVER_CIMPL_H_ */ 42 | -------------------------------------------------------------------------------- /mros-lib/mros-src/comm/cimpl/target/lwip/mros_comm_target.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 mbed.org, MIT License 2 | * 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 4 | * and associated documentation files (the "Software"), to deal in the Software without restriction, 5 | * including without limitation the rights to use, copy, modify, merge, publish, distribute, 6 | * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 7 | * furnished to do so, subject to the following conditions: 8 | * 9 | * The above copyright notice and this permission notice shall be included in all copies or 10 | * substantial portions of the Software. 11 | * 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 13 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 14 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 17 | */ 18 | #ifndef _MROS_COMM_TARGET_H_ 19 | #define _MROS_COMM_TARGET_H_ 20 | 21 | #ifdef __cplusplus 22 | extern "C" { 23 | #endif 24 | 25 | #include "mros_types.h" 26 | #include "lwip/sockets.h" 27 | #include "lwip/init.h" 28 | #include "lwip/netdb.h" 29 | 30 | typedef struct sockaddr mRosSockAddrType; 31 | typedef struct sockaddr_in mRosSockAddrInType; 32 | typedef struct hostent mRosHostEntType; 33 | typedef ip_addr_t mRosIpAaddrType; 34 | typedef fd_set mRosFdSetType; 35 | typedef struct timeval mRosTimeValType; 36 | 37 | #define MROS_FD_ZERO(arg) FD_ZERO(arg) 38 | #define MROS_FD_SET(arg1, arg2) FD_SET(arg1, arg2) 39 | #define MROS_FD_SETSIZE FD_SETSIZE 40 | #define MROS_FD_ISSET(arg1, arg2) FD_ISSET(arg1, arg2) 41 | 42 | #define MROS_SOCK_STREAM SOCK_STREAM 43 | #define MROS_SOCK_DGRAM SOCK_DGRAM 44 | #define MROS_SOCK_RAW SOCK_RAW 45 | 46 | #define MROS_SOCK_UNSPEC AF_UNSPEC 47 | #define MROS_SOCK_AF_INET AF_INET 48 | 49 | 50 | #ifdef __cplusplus 51 | } 52 | #endif 53 | 54 | 55 | #endif /* _MROS_COMM_TARGET_H_ */ 56 | -------------------------------------------------------------------------------- /mros-lib/mros-src/config/mros_sys_config.c: -------------------------------------------------------------------------------- 1 | #include "mros_types.h" 2 | #include "mros_memory.h" 3 | #include "mros_sys_config.h" 4 | #include "kernel.h" 5 | 6 | void mros_sys_config_init(void) 7 | { 8 | mRosReturnType ret; 9 | 10 | ret = mros_mem_init(ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM, ros_inner_topic_publisher_config, &ros_inner_topic_publisher_mempool); 11 | if (ret != MROS_E_OK) { 12 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 13 | return; 14 | } 15 | ret = mros_mem_init(ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM, ros_outer_topic_publisher_config, &ros_outer_topic_publisher_mempool); 16 | if (ret != MROS_E_OK) { 17 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 18 | return; 19 | } 20 | return; 21 | } 22 | 23 | 24 | void usr_task_activation(void) 25 | { 26 | mros_uint32 i; 27 | for (i = 0; i < MROS_USR_TASK_NUM; i++) { 28 | act_tsk(mros_usr_task_table[i]); 29 | } 30 | } 31 | 32 | -------------------------------------------------------------------------------- /mros-lib/mros-src/config/mros_sys_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_SYS_CONFIG_H_ 2 | #define _MROS_SYS_CONFIG_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_usr_config.h" 9 | #include "mros_os_config.h" 10 | #include "mros_packet_config.h" 11 | 12 | 13 | 14 | /************************************** 15 | * PROTOCOL 16 | **************************************/ 17 | /* 18 | * portno of master 19 | */ 20 | #define MROS_MASTER_PORT_NO 11311 21 | /* 22 | * dhcp option of self node(0:not use, 1:use) 23 | */ 24 | #define MROS_NODE_USE_DHCP 0 25 | /* 26 | * ipaddr of master 27 | */ 28 | #define MROS_MASTER_IPADDR "0.0.0.0" 29 | 30 | /* 31 | * ipaddr of self node 32 | */ 33 | #define MROS_NODE_IPADDR "127.0.0.1" 34 | 35 | /* 36 | * subnet mask of self node 37 | */ 38 | #define MROS_NODE_SUBNET_MASK "255.255.255.0" 39 | 40 | /* 41 | * portno of slave 42 | */ 43 | #define MROS_SLAVE_PORT_NO 11411 44 | 45 | /* 46 | * portno of pub 47 | */ 48 | #define MROS_PUBLISHER_PORT_NO 11511 49 | 50 | /* 51 | * do not change this parameter 52 | */ 53 | #define MROS_TOPIC_TCP_CLIENT_MAX_NUM ( MROS_PUB_TOPIC_CONNECTOR_MAX_NUM + MROS_SUB_TOPIC_CONNECTOR_MAX_NUM ) 54 | 55 | 56 | /***************************************** 57 | * EXCLUSIVE AREA 58 | *****************************************/ 59 | 60 | /* 61 | * do not change this parameter 62 | */ 63 | #define MROS_GIANT_EXCLUSIVE_AREA_PRIORITY ( \ 64 | ( MROS_USR_TASK_PRI < MROS_TASK_PRI) ? \ 65 | MROS_USR_TASK_PRI : \ 66 | MROS_TASK_PRI \ 67 | ) 68 | 69 | 70 | extern void mros_sys_config_init(void); 71 | extern void usr_task_activation(void); 72 | 73 | #ifdef __cplusplus 74 | } 75 | #endif 76 | 77 | #endif /* _MROS_SYS_CONFIG_H_ */ 78 | 79 | -------------------------------------------------------------------------------- /mros-lib/mros-src/config/mros_usr_config.c: -------------------------------------------------------------------------------- 1 | #include "mros_types.h" 2 | #include "mros_memory.h" 3 | #include "mros_usr_config.h" 4 | #include "kernel_cfg.h" 5 | 6 | /******************************************************* 7 | * START: Inner Publish topic data memory Config 8 | *******************************************************/ 9 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool1, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL1_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL1_SIZE); 10 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool2, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL2_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL2_SIZE); 11 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool3, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL3_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL3_SIZE); 12 | mRosMemoryConfigType *ros_inner_topic_publisher_config[ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM] = { 13 | &ros_inner_topic_publisher_mempool1_config, 14 | &ros_inner_topic_publisher_mempool2_config, 15 | &ros_inner_topic_publisher_mempool3_config, 16 | }; 17 | MROS_MEMORY_CONFIG_DECLARE_MANAGER(ros_inner_topic_publisher_mempool, ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM); 18 | /******************************************************* 19 | * END 20 | *******************************************************/ 21 | 22 | 23 | /******************************************************* 24 | * START: Outer Publish topic data memory Config 25 | *******************************************************/ 26 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool1, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL1_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL1_SIZE); 27 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool2, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL2_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL2_SIZE); 28 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool3, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL3_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL3_SIZE); 29 | mRosMemoryConfigType *ros_outer_topic_publisher_config[ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM] = { 30 | &ros_outer_topic_publisher_mempool1_config, 31 | &ros_outer_topic_publisher_mempool2_config, 32 | &ros_outer_topic_publisher_mempool3_config, 33 | }; 34 | MROS_MEMORY_CONFIG_DECLARE_MANAGER(ros_outer_topic_publisher_mempool, ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM); 35 | /******************************************************* 36 | * END 37 | *******************************************************/ 38 | 39 | 40 | /**************************************** 41 | * USR OS TASK 42 | ****************************************/ 43 | 44 | mRosTaskIdType mros_usr_task_table[MROS_USR_TASK_NUM] = { 45 | USR_TASK1, 46 | USR_TASK2, 47 | }; 48 | -------------------------------------------------------------------------------- /mros-lib/mros-src/config/os/target/os_asp/mros_os_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_OS_CONFIG_H_ 2 | #define _MROS_OS_CONFIG_H_ 3 | 4 | #include "kernel.h" 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | extern void main_task(void); 11 | extern void sub_task(void); 12 | extern void pub_task(void); 13 | extern void xml_slv_task(void); 14 | extern void xml_mas_task(void); 15 | extern void cyclic_handler(intptr_t exinf); 16 | 17 | /**************************************** 18 | * kernel cfg 19 | ****************************************/ 20 | #ifndef MROS_USR_TASK_PRI 21 | /* 22 | * main task priority 23 | * 24 | * do not change this parameter 25 | */ 26 | #define MAIN_TASK_PRI 7 27 | /* 28 | * mROS task priority 29 | * 30 | * do not change this parameter 31 | */ 32 | #define MROS_TASK_PRI 6 33 | 34 | /* 35 | * user task priority 36 | * 37 | * following config parameter is an example. 38 | */ 39 | #define MROS_USR_TASK1_PRI 8 40 | #define MROS_USR_TASK2_PRI 9 41 | 42 | /* 43 | * user task max priority 44 | * 45 | * please set max priority of user tasks. 46 | */ 47 | #define MROS_USR_TASK_PRI 8 48 | #endif /* ROS_USR_TASK_PRI */ 49 | 50 | #ifndef TASK_PORTID 51 | #define TASK_PORTID 1 /* serial port ID for something typing */ 52 | #endif /* TASK_PORTID */ 53 | 54 | #ifndef MROS_TASK_STACK_SIZE 55 | /* 56 | * user task stack size 57 | */ 58 | #define MROS_USR2_STACK_SIZE 1024 * 1 //for user task2 59 | #define MROS_USR1_STACK_SIZE 1024 * 1 //for user task1 60 | /* 61 | * mROS task stack size 62 | * 63 | * do not change this parameter 64 | */ 65 | #define MROS_TASK_STACK_SIZE 1024 * 2 //for mros task 66 | #endif /*MROS_TASK_STACK_SIZE*/ 67 | 68 | /* 69 | * do not change this parameter 70 | */ 71 | #ifndef CYC 72 | #define MROS_LOOP_RATE 100 73 | #define CYC 74 | #endif /*CYC*/ 75 | 76 | /* 77 | * do not change this parameter 78 | */ 79 | #ifndef LOOP_REF 80 | #define LOOP_REF ULONG_C(1000000) /* number of loops to evaluate speed */ 81 | #endif /* LOOP_REF */ 82 | 83 | #ifdef __cplusplus 84 | } 85 | #endif 86 | 87 | 88 | #endif /* _MROS_OS_CONFIG_H_ */ 89 | -------------------------------------------------------------------------------- /mros-lib/mros-src/inc/mros_array_container.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_ARRAY_CONTAINER_H_ 2 | #define _MROS_ARRAY_CONTAINER_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | typedef struct { 11 | mros_uint32 count; 12 | mros_uint32 array_num; 13 | mRosContainerObjType *array; 14 | } mRosArrayContainerType; 15 | 16 | #define MROS_ARRAY_CONTAINER_CONFIG_DECLARE_MANAGER(manager_name, array_num) \ 17 | static mRosContainerObjType manager_name##_array [(array_num)] MROS_MATTR_BSS_NOCLR; \ 18 | static mRosArrayContainerType manager_name = { \ 19 | (0), \ 20 | (array_num), \ 21 | manager_name##_array, \ 22 | }; 23 | 24 | static inline void mros_array_container_add(mRosArrayContainerType *mgrp, mRosContainerObjType obj) 25 | { 26 | if (mgrp->count >= mgrp->array_num) { 27 | return; 28 | } 29 | mgrp->array[mgrp->count] = obj; 30 | mgrp->count++; 31 | } 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | #endif /* _MROS_ARRAY_CONTAINER_H_ */ 37 | -------------------------------------------------------------------------------- /mros-lib/mros-src/inc/mros_exclusive_area.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_EXCLUSIVE_AREA_H_ 2 | #define _MROS_EXCLUSIVE_AREA_H_ 3 | #ifdef __cplusplus 4 | extern "C" { 5 | #endif 6 | 7 | #include "mros_exclusive_ops.h" 8 | #include "mros_wait_queue.h" 9 | 10 | extern mRosExclusiveObjectType mros_exclusive_area MROS_MATTR_BSS_NOCLR; 11 | extern mRosWaitQueueType mros_master_wait_queue MROS_MATTR_BSS_NOCLR; 12 | extern mRosWaitQueueType mros_subscribe_wait_queue MROS_MATTR_BSS_NOCLR; 13 | 14 | extern void mros_exclusive_area_init(mRosTaskIdType mas_task_id, mRosTaskIdType sub_task_id); 15 | 16 | #ifdef __cplusplus 17 | } 18 | #endif 19 | 20 | #endif /* _MROS_EXCLUSIVE_AREA_H_ */ 21 | -------------------------------------------------------------------------------- /mros-lib/mros-src/inc/mros_exclusive_ops.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_EXCLUSIVE_OPS_H_ 2 | #define _MROS_EXCLUSIVE_OPS_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | #include "mros_os.h" 10 | 11 | 12 | typedef struct { 13 | /* 14 | * 排他エリアのプライオリティ 15 | * 排他エリア内では，タスクのプライオリティは本プライオリティに引き上げられる 16 | */ 17 | mRosTaskPriorityType priority; 18 | } mRosExclusiveObjectType; 19 | 20 | typedef struct { 21 | mRosTaskPriorityType org_priority; 22 | } mROsExclusiveUnlockObjType; 23 | 24 | extern void mros_exclusive_init(mRosExclusiveObjectType *exobj, mRosTaskPriorityType priority); 25 | extern void mros_exclusive_lock(mRosExclusiveObjectType *exobj, mROsExclusiveUnlockObjType *unlock_obj); 26 | extern void mros_exclusive_unlock(mROsExclusiveUnlockObjType *unlock_objj); 27 | 28 | #ifdef __cplusplus 29 | } 30 | #endif 31 | 32 | #endif /* _MROS_EXCLUSIVE_OPS_H_ */ 33 | -------------------------------------------------------------------------------- /mros-lib/mros-src/inc/mros_list.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_LIST_H_ 2 | #define _MROS_LIST_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | /* 11 | * PRIVATE 12 | */ 13 | #define ListEntry_Init(entry) \ 14 | do { \ 15 | (entry)->next = (entry); \ 16 | (entry)->prev = (entry); \ 17 | } while (0) 18 | 19 | #define ListEntry_InsertHead(first, elm) \ 20 | do { \ 21 | if ((first) != MROS_NULL) { \ 22 | (elm)->next = (first); \ 23 | (elm)->prev = (first)->prev; \ 24 | (first)->prev->next = (elm); \ 25 | (first)->prev = (elm); \ 26 | } \ 27 | (first) = (elm); \ 28 | } while (0) 29 | 30 | #define List_IsEmpty(first) ((first)->next == (first)) 31 | #define ListEntry_First(first) (first) 32 | 33 | #define ListEntry_Remove(first, elm) \ 34 | do { \ 35 | if ((first) != MROS_NULL) { \ 36 | if ((first) == (elm)) { \ 37 | if (List_IsEmpty(first)) { \ 38 | (first) = MROS_NULL; \ 39 | } \ 40 | else { \ 41 | (first) = (elm)->next; \ 42 | } \ 43 | } \ 44 | (elm)->next->prev = (elm)->prev; \ 45 | (elm)->prev->next = (elm)->next; \ 46 | ListEntry_Init(elm); \ 47 | } \ 48 | } while (0) 49 | 50 | 51 | /* 52 | * PUBLIC 53 | */ 54 | #define ListEntryType(name, dataTypeName) \ 55 | struct name { \ 56 | struct name *next; \ 57 | struct name *prev; \ 58 | dataTypeName data; \ 59 | } 60 | 61 | #define ListHeadType(entry_type) \ 62 | struct { \ 63 | mros_uint32 entry_size; \ 64 | mros_uint32 entry_num; \ 65 | struct entry_type *entries; \ 66 | struct entry_type *free; \ 67 | mros_uint32 free_num; \ 68 | } 69 | 70 | #define List_Init(headp, entry_type, prealloc_size, memp) \ 71 | do { \ 72 | mros_uint32 _i;\ 73 | (headp)->entry_size = sizeof(entry_type); \ 74 | (headp)->entry_num = 0; \ 75 | (headp)->entries = MROS_NULL; \ 76 | (headp)->free = MROS_NULL; \ 77 | (headp)->free_num = (prealloc_size); \ 78 | for (_i = 0; _i < (prealloc_size); _i++) { \ 79 | entry_type *_tmp = &(((entry_type*)(memp))[_i]); \ 80 | ListEntry_Init(_tmp); \ 81 | ListEntry_InsertHead((headp)->free, _tmp); \ 82 | } \ 83 | } while (0) 84 | 85 | #define List_InitEmpty(headp, entry_type) \ 86 | do { \ 87 | List_Init(headp, entry_type, 0, MROS_NULL); \ 88 | } while (0) 89 | 90 | #define ListEntry_Alloc(headp, entry_type, new_entrypp) \ 91 | do { \ 92 | entry_type *_tmp; \ 93 | if ((headp)->free_num > 0) { \ 94 | _tmp = ListEntry_First((headp)->free); \ 95 | ListEntry_Remove((headp)->free, _tmp); \ 96 | (headp)->free_num--; \ 97 | *(new_entrypp) = _tmp; \ 98 | } \ 99 | else { \ 100 | *(new_entrypp) = MROS_NULL; \ 101 | } \ 102 | } while (0) 103 | 104 | #define ListEntry_Free(headp, entryp) \ 105 | do { \ 106 | ListEntry_Init(entryp); \ 107 | ListEntry_InsertHead((headp)->free, entryp); \ 108 | (headp)->free_num++; \ 109 | } while (0) 110 | 111 | 112 | #define ListEntry_AddEntry(headp, entryp) \ 113 | do { \ 114 | ListEntry_Init(entryp); \ 115 | ListEntry_InsertHead((headp)->entries, entryp); \ 116 | (headp)->entry_num++; \ 117 | } while (0) 118 | 119 | #define ListEntry_RemoveEntry(headp, entryp) \ 120 | do { \ 121 | ListEntry_Remove((headp)->entries, entryp); \ 122 | (headp)->entry_num--; \ 123 | } while (0) 124 | 125 | #define ListEntry_GetFirst(headp, entrypp) \ 126 | do { \ 127 | *(entrypp) = (headp)->entries; \ 128 | } while (0) 129 | 130 | 131 | #define ListEntry_RemoveAll(headp, entry_type) \ 132 | do { \ 133 | while ((headp)->entry_num > 0) { \ 134 | entry_type _tmp = ListEntry_First((headp)->entries); \ 135 | ListEntry_RemoveEntry(headp, _tmp); \ 136 | } \ 137 | } while (0) 138 | 139 | #define ListEntry_Foreach(headp, var) \ 140 | mros_uint32 _i; \ 141 | (var) = (headp)->entries; \ 142 | for (_i = 0; \ 143 | _i < (headp)->entry_num; \ 144 | (var) = (var)->next, _i++) 145 | 146 | #ifdef __cplusplus 147 | } 148 | #endif 149 | 150 | #endif /* _MROS_LIST_H_ */ 151 | -------------------------------------------------------------------------------- /mros-lib/mros-src/inc/mros_memory.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_MEMORY_H_ 2 | #define _MROS_MEMORY_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | #include "mros_list.h" 10 | 11 | typedef mros_uint32 mRosMemorySizeIdType; 12 | 13 | struct mRosMemoryManagerType; 14 | typedef struct { 15 | /* 16 | * manager 17 | */ 18 | struct mRosMemoryManagerType *mgrp; 19 | /* 20 | * id of memory_entries 21 | */ 22 | mRosIdType memory_id; 23 | /* 24 | * id of parent header 25 | */ 26 | mRosMemorySizeIdType header_id; 27 | /* 28 | * preallocation unit size 29 | */ 30 | mRosSizeType memsize; 31 | /* 32 | * real using size 33 | */ 34 | mRosSizeType size; 35 | char *memp; 36 | } mRosMemoryEntryType; 37 | 38 | typedef ListEntryType(mRosMemoryListEntryType, mRosMemoryEntryType) mRosMemoryListEntryType; 39 | typedef ListHeadType(mRosMemoryListEntryType) mRosMemoryListHeadType; 40 | 41 | typedef struct { 42 | /* 43 | * memory list header 44 | */ 45 | mRosMemoryListHeadType head; 46 | /* 47 | * num of preallocation memory 48 | */ 49 | mRosSizeType max_memory_num; 50 | /* 51 | * preallocation unit size 52 | */ 53 | mRosSizeType memsize; 54 | /* 55 | * memory list entry 56 | */ 57 | mRosMemoryListEntryType *memory_entries; 58 | /* 59 | * raw data 60 | */ 61 | char *memory; 62 | } mRosMemoryHeaderType; 63 | 64 | typedef struct mRosMemoryManagerType { 65 | /* 66 | * num of header 67 | */ 68 | mRosSizeType header_num; 69 | /* 70 | * header array 71 | */ 72 | mRosMemoryHeaderType *header_array; 73 | } mRosMemoryManagerType; 74 | 75 | /* 76 | * config memory 77 | */ 78 | typedef struct { 79 | mRosSizeType max_memory_num; 80 | mRosSizeType memsize; 81 | mRosMemoryListEntryType *memory_entries; 82 | char *memory; 83 | } mRosMemoryConfigType; 84 | 85 | extern mRosReturnType mros_mem_init(mRosSizeType config_num, mRosMemoryConfigType **config, mRosMemoryManagerType *mgrp); 86 | extern mRosReturnType mros_mem_alloc(mRosMemoryManagerType *mgrp, mRosSizeType size, mRosMemoryListEntryType **memory); 87 | extern mRosReturnType mros_mem_free(mRosMemoryManagerType *mgrp, mRosMemoryListEntryType *memory); 88 | 89 | /* 90 | * Memory Config APIs 91 | */ 92 | #define MROS_MEMORY_CONFIG_DECLARE_ENTRY(entry_name, max_memory_num, memsize) \ 93 | static mRosMemoryListEntryType entry_name##_entries [(max_memory_num)] MROS_MATTR_BSS_NOCLR; \ 94 | static char entry_name##_memory [(max_memory_num) * (memsize)] MROS_MATTR_BSS_NOCLR; \ 95 | static mRosMemoryConfigType entry_name##_config = { \ 96 | (max_memory_num), \ 97 | (memsize), \ 98 | entry_name##_entries , \ 99 | entry_name##_memory , \ 100 | }; 101 | 102 | #define MROS_MEMORY_CONFIG_DECLARE_MANAGER(mem_manager_name, config_num) \ 103 | static mRosMemoryHeaderType mem_manager_name##_head_array [(config_num)] MROS_MATTR_BSS_NOCLR; \ 104 | mRosMemoryManagerType mem_manager_name = { \ 105 | (config_num), \ 106 | mem_manager_name##_head_array, \ 107 | }; 108 | 109 | 110 | #ifdef __cplusplus 111 | } 112 | #endif 113 | 114 | #endif /* _MROS_MEMORY_H_ */ 115 | -------------------------------------------------------------------------------- /mros-lib/mros-src/inc/mros_name.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_NAME_H_ 2 | #define _MROS_NAME_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | extern void mros_name_formalize(const char *src_name, mros_uint32 src_namelen, char *dst_name, mros_uint32 *dst_namelen); 11 | 12 | 13 | #ifdef __cplusplus 14 | } 15 | #endif 16 | 17 | #endif /* _MROS_NAME_H_ */ 18 | -------------------------------------------------------------------------------- /mros-lib/mros-src/inc/mros_os.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_OS_H_ 2 | #define _MROS_OS_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | extern mRosTaskIdType mros_get_taskid(void); 11 | extern mRosTaskPriorityType mros_get_taskpri(void); 12 | extern void mros_change_taskpri(mRosTaskPriorityType priority); 13 | extern void mros_sleep_task(void); 14 | extern void mros_sleep_task_msec(mRosTaskSleepIntervalType msec); 15 | extern void mros_wakeup_task(mRosTaskIdType task_id); 16 | 17 | #ifdef __cplusplus 18 | } 19 | #endif 20 | 21 | 22 | #endif /* _MROS_OS_H_ */ 23 | -------------------------------------------------------------------------------- /mros-lib/mros-src/inc/mros_types.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_TYPES_H_ 2 | #define _MROS_TYPES_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | typedef int mros_int32; 9 | typedef char mros_int8; 10 | typedef unsigned char mros_uint8; 11 | typedef unsigned short mros_uint16; 12 | typedef unsigned int mros_uint32; 13 | typedef unsigned char mros_boolean; 14 | #define MROS_FALSE 0 15 | #define MROS_TRUE 1 16 | 17 | #define MROS_ID_NONE 0 18 | #define MROS_ID(index) ((index) + 1U) 19 | #define MROS_INDEX(id) ((id) - 1U) 20 | 21 | typedef mros_uint32 mRosNodeIdType; 22 | typedef mros_uint32 mRosTopicIdType; 23 | typedef mros_uint32 mRosTopicConnectorIdType; 24 | typedef mros_uint32 mRosPacketIdType; 25 | typedef mros_uint32 mRosIdType; 26 | typedef mros_uint32* mRosFuncIdType; 27 | typedef mros_int8* mRosPtrType; 28 | 29 | 30 | typedef mros_uint32* mRosContainerObjType; 31 | #define MROS_NULL (0) 32 | #define MROS_COBJ_NULL ((mRosContainerObjType)MROS_NULL) 33 | 34 | typedef mros_uint32 mRosSizeType; 35 | 36 | #define MROS_E_OK 0 37 | #define MROS_E_NOENT 2 38 | #define MROS_E_NOMEM 12 39 | #define MROS_E_BUSY 13 40 | #define MROS_E_EXIST 17 41 | #define MROS_E_INVAL 22 42 | #define MROS_E_RANGE 34 43 | #define MROS_E_LIMIT 111 44 | #define MROS_E_NOTCONN 112 45 | #define MROS_E_SYSERR -1 46 | typedef mros_int32 mRosReturnType; 47 | 48 | /* 49 | * Os dependent data types 50 | */ 51 | #include "mros_os_target.h" 52 | #define MROS_TASKID_NONE 0U 53 | 54 | #ifdef TARGET_ATHRILL 55 | /* 56 | * NC_BSS means non clear bss section on athrill. 57 | */ 58 | #define MROS_MATTR_BSS_NOCLR __attribute__((section("NC_BSS"))) 59 | #else 60 | /* 61 | * NC_BSS must not set on target board because NC_BSS means non cache region. 62 | */ 63 | #define MROS_MATTR_BSS_NOCLR 64 | #endif /* TARGET_ATHRILL */ 65 | 66 | /* 67 | * Comm dependent data types 68 | */ 69 | #include "mros_comm_target.h" 70 | 71 | #ifdef __cplusplus 72 | } 73 | #endif 74 | 75 | #endif /* _MROS_TYPES_H_ */ 76 | -------------------------------------------------------------------------------- /mros-lib/mros-src/inc/mros_wait_queue.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_WAIT_QUEUE_H_ 2 | #define _MROS_WAIT_QUEUE_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_list.h" 9 | 10 | typedef struct { 11 | mRosTaskIdType task_id; 12 | mRosTaskPriorityType task_priority; 13 | void *reqp; 14 | } mRosWaitEntryType; 15 | typedef ListEntryType(mRosWaitListEntryType, mRosWaitEntryType) mRosWaitListEntryType; 16 | typedef ListHeadType(mRosWaitListEntryType) mRosWaitListHeadType; 17 | 18 | typedef struct { 19 | mRosTaskIdType task_id; 20 | mRosWaitListHeadType head; 21 | } mRosWaitQueueType; 22 | 23 | extern void mros_client_wait_entry_init(mRosWaitListEntryType *wait_entry, void *reqp); 24 | extern void mros_server_queue_init(mRosWaitQueueType *wait_queue, mRosTaskIdType task_id); 25 | 26 | extern void mros_client_wakeup(mRosWaitListEntryType *wait_entry); 27 | extern void mros_client_put_request(mRosWaitQueueType *wait_queue, mRosWaitListEntryType *wait_entry); 28 | extern void mros_client_wait_for_request_done(mRosWaitQueueType *wait_queue, mRosWaitListEntryType *wait_entry); 29 | extern mRosWaitListEntryType *mros_server_queue_wait(mRosWaitQueueType *wait_queue); 30 | 31 | 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | #endif /* _MROS_WAIT_QUEUE_H_ */ 37 | -------------------------------------------------------------------------------- /mros-lib/mros-src/lib/mros_memory.c: -------------------------------------------------------------------------------- 1 | #include "mros_memory.h" 2 | 3 | 4 | #define MEMORY_ID(index) ((index) + 1U) 5 | #define MEMORY_INDEX(id) ((id) - 1U) 6 | 7 | #define MEMORY_OBJ(mid, id) memory_manager[(mid)].memory_entries[MEMORY_INDEX((id))] 8 | 9 | mRosReturnType mros_mem_init(mRosSizeType config_num, mRosMemoryConfigType **config, mRosMemoryManagerType *mgrp) 10 | { 11 | mros_uint32 i; 12 | mros_uint32 j; 13 | mgrp->header_num = config_num; 14 | for (i = 0; i < config_num; i++) { 15 | mgrp->header_array[i].max_memory_num = config[i]->max_memory_num; 16 | mgrp->header_array[i].memsize = config[i]->memsize; 17 | 18 | mgrp->header_array[i].memory_entries = config[i]->memory_entries; 19 | mgrp->header_array[i].memory = config[i]->memory; 20 | 21 | for (j = 0; j < mgrp->header_array[i].max_memory_num; j++) { 22 | mRosMemoryListEntryType *entry = &(mgrp->header_array[i].memory_entries[j]); 23 | entry->data.mgrp = mgrp; 24 | entry->data.header_id = (mRosMemorySizeIdType)i; 25 | entry->data.memory_id = MEMORY_ID(j); 26 | entry->data.memsize = mgrp->header_array[i].memsize; 27 | entry->data.size = 0; 28 | entry->data.memp = &(mgrp->header_array[i].memory[(mgrp->header_array[i].memsize * j)]); 29 | } 30 | List_Init(&mgrp->header_array[i].head, mRosMemoryListEntryType, mgrp->header_array[i].max_memory_num, mgrp->header_array[i].memory_entries); 31 | } 32 | return MROS_E_OK; 33 | } 34 | 35 | mRosReturnType mros_mem_alloc(mRosMemoryManagerType *mgrp, mRosSizeType size, mRosMemoryListEntryType **memory) 36 | { 37 | mros_uint32 i; 38 | if (memory == MROS_NULL) { 39 | return MROS_E_INVAL; 40 | } 41 | for (i = 0; i < mgrp->header_num; i++) { 42 | if (size > mgrp->header_array[i].memsize) { 43 | continue; 44 | } 45 | if (mgrp->header_array[i].head.free_num <= 0) { 46 | continue; 47 | } 48 | ListEntry_Alloc(&mgrp->header_array[i].head, mRosMemoryListEntryType, memory); 49 | (*memory)->data.size = size; 50 | return MROS_E_OK; 51 | } 52 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, MROS_E_NOMEM); 53 | return MROS_E_NOMEM; 54 | } 55 | 56 | mRosReturnType mros_mem_free(mRosMemoryManagerType *mgrp, mRosMemoryListEntryType *memory) 57 | { 58 | if (memory == MROS_NULL) { 59 | return MROS_E_OK; 60 | } 61 | if (memory->data.header_id >= mgrp->header_num) { 62 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, MROS_E_INVAL); 63 | return MROS_E_INVAL; 64 | } 65 | if (memory->data.memory_id > mgrp->header_array[memory->data.header_id].max_memory_num) { 66 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, MROS_E_RANGE); 67 | return MROS_E_RANGE; 68 | } 69 | memory->data.size = 0; 70 | ListEntry_Free(&mgrp->header_array[memory->data.header_id].head, memory); 71 | return MROS_E_OK; 72 | } 73 | 74 | -------------------------------------------------------------------------------- /mros-lib/mros-src/lib/mros_name.c: -------------------------------------------------------------------------------- 1 | #include "mros_name.h" 2 | #include 3 | 4 | void mros_name_formalize(const char *src_name, mros_uint32 src_namelen, char *dst_name, mros_uint32 *dst_namelen) 5 | { 6 | mros_uint32 i; 7 | mros_uint32 slash_count = 0; 8 | const char* src_copy_head = MROS_NULL; 9 | mros_uint32 src_copy_len; 10 | 11 | for (i = 0; i < src_namelen; i++) { 12 | if (src_name[i] != '/') { 13 | src_copy_head = &src_name[i]; 14 | break; 15 | } 16 | else { 17 | slash_count++; 18 | } 19 | } 20 | src_copy_len = src_namelen - slash_count; 21 | dst_name[0] = '/'; 22 | memcpy(&dst_name[1], src_copy_head, src_copy_len); 23 | dst_name[src_copy_len + 1] = '\0'; 24 | *dst_namelen = src_copy_len + 1; 25 | return; 26 | } 27 | -------------------------------------------------------------------------------- /mros-lib/mros-src/lib/mros_wait_queue.c: -------------------------------------------------------------------------------- 1 | #include "mros_wait_queue.h" 2 | #include "mros_os.h" 3 | 4 | void mros_client_wait_entry_init(mRosWaitListEntryType *wait_entry, void *reqp) 5 | { 6 | wait_entry->data.reqp = reqp; 7 | wait_entry->data.task_id = mros_get_taskid(); 8 | wait_entry->data.task_priority = mros_get_taskpri(); 9 | return; 10 | } 11 | 12 | void mros_server_queue_init(mRosWaitQueueType *wait_queue, mRosTaskIdType task_id) 13 | { 14 | wait_queue->task_id = task_id; 15 | List_InitEmpty(&wait_queue->head, mRosWaitListEntryType); 16 | 17 | return; 18 | } 19 | 20 | void mros_client_wakeup(mRosWaitListEntryType *wait_entry) 21 | { 22 | mros_wakeup_task(wait_entry->data.task_id); 23 | return; 24 | } 25 | 26 | 27 | void mros_client_put_request(mRosWaitQueueType *wait_queue, mRosWaitListEntryType *wait_entry) 28 | { 29 | ListEntry_AddEntry(&wait_queue->head, wait_entry); 30 | mros_wakeup_task(wait_queue->task_id); 31 | return; 32 | } 33 | 34 | void mros_client_wait_for_request_done(mRosWaitQueueType *wait_queue, mRosWaitListEntryType *wait_entry) 35 | { 36 | mros_client_put_request(wait_queue, wait_entry); 37 | mros_sleep_task(); 38 | return; 39 | } 40 | static mRosWaitListEntryType *mros_server_queue_get(mRosWaitQueueType *wait_queue) 41 | { 42 | mRosTaskPriorityType min_priority = MROS_TASK_MIN_PRIORITY; 43 | mRosWaitListEntryType *entry; 44 | mRosWaitListEntryType *target = MROS_NULL; 45 | 46 | ListEntry_Foreach(&wait_queue->head, entry) { 47 | if (entry->data.task_priority < min_priority) { 48 | min_priority = entry->data.task_priority; 49 | target = entry; 50 | } 51 | } 52 | if (target != MROS_NULL) { 53 | ListEntry_RemoveEntry(&wait_queue->head, target); 54 | } 55 | return target; 56 | } 57 | 58 | mRosWaitListEntryType *mros_server_queue_wait(mRosWaitQueueType *wait_queue) 59 | { 60 | if (wait_queue->head.entry_num == 0) { 61 | mros_sleep_task(); 62 | } 63 | return mros_server_queue_get(wait_queue); 64 | } 65 | -------------------------------------------------------------------------------- /mros-lib/mros-src/node/cimpl/mros_node_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_NODE_CIMPL_H_ 2 | #define _MROS_NODE_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | typedef enum { 11 | MROS_NODE_TYPE_INNER = 0, 12 | MROS_NODE_TYPE_OUTER, 13 | MROS_NODE_TYPE_NUM, 14 | } mRosNodeEnumType; 15 | 16 | 17 | extern mRosReturnType mros_node_init(void); 18 | extern mRosReturnType mros_node_get_byname(const char *node_name, mRosNodeIdType *id); 19 | extern mRosReturnType mros_node_get_bytid(mRosNodeIdType *id); 20 | extern mRosNodeEnumType mros_node_type(mRosNodeIdType id); 21 | extern const char* mros_node_name(mRosNodeIdType id); 22 | 23 | extern mRosReturnType mros_node_create_inner(const char *node_name, mRosNodeIdType *id); 24 | extern mRosReturnType mros_node_create_outer(mRosNodeIdType *id); 25 | 26 | extern mRosReturnType mros_node_remove(mRosNodeIdType id); 27 | 28 | #ifdef __cplusplus 29 | } 30 | #endif 31 | #endif /* _MROS_NODE_CIMPL_H_ */ 32 | -------------------------------------------------------------------------------- /mros-lib/mros-src/os/mros_exclusive_area.c: -------------------------------------------------------------------------------- 1 | #include "mros_exclusive_area.h" 2 | #include "mros_wait_queue.h" 3 | #include "mros_sys_config.h" 4 | 5 | mRosExclusiveObjectType mros_exclusive_area MROS_MATTR_BSS_NOCLR; 6 | mRosWaitQueueType mros_master_wait_queue MROS_MATTR_BSS_NOCLR; 7 | 8 | mRosWaitQueueType mros_subscribe_wait_queue MROS_MATTR_BSS_NOCLR; 9 | 10 | void mros_exclusive_area_init(mRosTaskIdType mas_task_id, mRosTaskIdType sub_task_id) 11 | { 12 | mros_exclusive_init(&mros_exclusive_area, MROS_GIANT_EXCLUSIVE_AREA_PRIORITY); 13 | mros_server_queue_init(&mros_master_wait_queue, mas_task_id); 14 | mros_server_queue_init(&mros_subscribe_wait_queue, sub_task_id); 15 | 16 | return; 17 | } 18 | -------------------------------------------------------------------------------- /mros-lib/mros-src/os/mros_exclusive_ops.c: -------------------------------------------------------------------------------- 1 | #include "mros_os.h" 2 | #include "mros_exclusive_ops.h" 3 | 4 | void mros_exclusive_init(mRosExclusiveObjectType *exobj, mRosTaskPriorityType priority) 5 | { 6 | exobj->priority = priority; 7 | return; 8 | } 9 | 10 | void mros_exclusive_lock(mRosExclusiveObjectType *exobj, mROsExclusiveUnlockObjType *unlock_obj) 11 | { 12 | unlock_obj->org_priority = mros_get_taskpri(); 13 | mros_change_taskpri(exobj->priority); 14 | return; 15 | } 16 | 17 | void mros_exclusive_unlock(mROsExclusiveUnlockObjType *unlock_obj) 18 | { 19 | mros_change_taskpri(unlock_obj->org_priority); 20 | return; 21 | } 22 | -------------------------------------------------------------------------------- /mros-lib/mros-src/os/target/os_asp/mros_log.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_LOG_H_ 2 | #define _MROS_LOG_H_ 3 | 4 | #include 5 | #include 6 | 7 | #ifdef MROS_LOG_DISABLE_DEBUG 8 | #define ROS_DEBUG(...) 9 | #else 10 | #define ROS_DEBUG(...) syslog(LOG_DEBUG, __VA_ARGS__) 11 | #endif 12 | 13 | #ifdef MROS_LOG_DISABLE_INFO 14 | #define ROS_INFO(...) 15 | #else 16 | #define ROS_INFO(...) syslog(LOG_NOTICE, __VA_ARGS__) 17 | #endif 18 | 19 | #ifdef MROS_LOG_DISABLE_WARN 20 | #define ROS_WARN(...) 21 | #else 22 | #define ROS_WARN(...) syslog(LOG_WARNING, __VA_ARGS__) 23 | #endif 24 | 25 | #ifdef MROS_LOG_DISABLE_ERROR 26 | #define ROS_ERROR(...) 27 | #else 28 | #define ROS_ERROR(...) syslog(LOG_ERROR, __VA_ARGS__) 29 | #endif 30 | 31 | #ifdef MROS_LOG_DISABLE_FATAL 32 | #define ROS_FATAL(...) 33 | #else 34 | #define ROS_FATAL(...) syslog(LOG_EMERG, __VA_ARGS__) 35 | #endif 36 | 37 | #endif /* _MROS_LOG_H_ */ -------------------------------------------------------------------------------- /mros-lib/mros-src/os/target/os_asp/mros_os.c: -------------------------------------------------------------------------------- 1 | #include "mros_os_target.h" 2 | 3 | mRosTaskIdType mros_get_taskid(void) 4 | { 5 | mRosTaskIdType tid; 6 | (void)get_tid(&tid); 7 | return tid; 8 | } 9 | 10 | mRosTaskPriorityType mros_get_taskpri(void) 11 | { 12 | mRosTaskIdType tid; 13 | PRI pri; 14 | (void)get_tid(&tid); 15 | (void)get_pri(tid, &pri); 16 | return pri; 17 | } 18 | 19 | void mros_change_taskpri(mRosTaskPriorityType priority) 20 | { 21 | mRosTaskIdType tid; 22 | (void)get_tid(&tid); 23 | chg_pri(tid, priority); 24 | return; 25 | } 26 | 27 | void mros_sleep_task(void) 28 | { 29 | (void)slp_tsk(); 30 | return; 31 | } 32 | 33 | void mros_sleep_task_msec(mRosTaskSleepIntervalType msec) 34 | { 35 | (void)dly_tsk(msec); 36 | return; 37 | } 38 | 39 | void mros_wakeup_task(mRosTaskIdType task_id) 40 | { 41 | (void)wup_tsk(task_id); 42 | return; 43 | } 44 | 45 | -------------------------------------------------------------------------------- /mros-lib/mros-src/os/target/os_asp/mros_os_target.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_OS_TARGET_H_ 2 | #define _MROS_OS_TARGET_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include 9 | #include 10 | 11 | #include "mros_types.h" 12 | 13 | typedef ID mRosTaskIdType; 14 | typedef PRI mRosTaskPriorityType; 15 | typedef mros_uint32 mRosTaskSleepIntervalType; 16 | 17 | #define MROS_TASK_MAX_PRIORITY TMIN_TPRI 18 | #define MROS_TASK_MIN_PRIORITY TMAX_TPRI 19 | 20 | #define MROS_LOG_EMRG LOG_EMERG 21 | #define MROS_LOG_ALERT LOG_ALERT 22 | #define MROS_LOG_CRIT LOG_CRIT 23 | #define MROS_LOG_ERROR LOG_ERROR 24 | #define MROS_LOG_WARNING LOG_WARNING 25 | #define MROS_LOG_NOTICE LOG_NOTICE 26 | #define MROS_LOG_INFO LOG_INFO 27 | #define MROS_LOG_DEBUG LOG_DEBUG 28 | 29 | #include "mros_log.h" 30 | 31 | 32 | #ifdef __cplusplus 33 | } 34 | #endif 35 | 36 | 37 | #endif /* _MROS_OS_TARGET_H_ */ 38 | -------------------------------------------------------------------------------- /mros-lib/mros-src/os/target/os_asp/mros_test.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_TEST_H_ 2 | #define _MROS_TEST_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | extern void do_test_register_publisher(void); 9 | extern void do_test_register_subscriber(void); 10 | extern void do_test_request_topic(void); 11 | extern void do_test_tcpros_topic(void); 12 | extern void do_test_server(void); 13 | 14 | 15 | #ifdef __cplusplus 16 | } 17 | #endif 18 | 19 | 20 | #endif /* _MROS_TEST_H_ */ 21 | -------------------------------------------------------------------------------- /mros-lib/mros-src/packet/cimpl/mros_packet_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PACKET_CIMPL_H_ 2 | #define _MROS_PACKET_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | typedef enum { 11 | MROS_PACKET_DATA_REGISTER_PUBLISHER_REQ = 0, 12 | MROS_PACKET_DATA_REGISTER_SUBSCRIBER_REQ, 13 | MROS_PACKET_DATA_REQUEST_TOPIC_REQ, 14 | MROS_PACKET_DATA_REQUEST_TOPIC_RES, 15 | MROS_PACKET_DATA_PUBLISHER_UPDATE_REQ, 16 | MROS_PACKET_DATA_TCPROS_TOPIC_REQ, 17 | MROS_PACKET_DATA_TCPROS_TOPIC_RES, 18 | MROS_PACKET_DATA_TOPIC, 19 | MROS_PACKET_DATA_NUM, 20 | } mRosPacketDataEnumType; 21 | #define MROS_PACKET_DATA_INVALID MROS_PACKET_DATA_NUM 22 | 23 | typedef struct { 24 | mRosSizeType total_size; 25 | mRosSizeType data_size; 26 | char *data; 27 | } mRosPacketType; 28 | 29 | #ifdef __cplusplus 30 | } 31 | #endif 32 | 33 | #endif /* _MROS_PACKET_CIMPL_H_ */ 34 | -------------------------------------------------------------------------------- /mros-lib/mros-src/packet/cimpl/mros_packet_decoder_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PACKET_DECODER_CIMPL_H_ 2 | #define _MROS_PACKET_DECODER_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_packet_cimpl.h" 9 | 10 | extern mRosReturnType mros_packet_decoder_init(void); 11 | 12 | 13 | /************************************************ 14 | * XML RPC PACKET 15 | ************************************************/ 16 | /* 17 | * XML request 18 | */ 19 | extern mros_boolean mros_xmlpacket_has_request_end(mRosPacketType *packet); 20 | /* 21 | * XML response 22 | */ 23 | extern mros_boolean mros_xmlpacket_has_response_end(mRosPacketType *packet); 24 | 25 | //MASTER 26 | /* 27 | * ReigsterPublish response 28 | */ 29 | extern mRosReturnType mros_xmlpacket_pubres_result(mRosPacketType *packet); 30 | 31 | /* 32 | * ReigsterSubscribe response 33 | */ 34 | extern mRosReturnType mros_xmlpacket_subres_result(mRosPacketType *packet); 35 | extern mRosPtrType mros_xmlpacket_subres_get_first_uri(mRosPacketType *packet, mros_uint32 *ipaddr, mros_int32 *port); 36 | extern mRosPtrType mros_xmlpacket_subres_get_next_uri(mRosPtrType ptr, mRosPacketType *packet, mros_uint32 *ipaddr, mros_int32 *port); 37 | 38 | 39 | //SLAVE 40 | typedef struct { 41 | struct { 42 | char *start_key; 43 | char *end_key; 44 | } req; 45 | struct { 46 | char *head; 47 | char *tail; 48 | mRosSizeType len; 49 | } res; 50 | } mRosPacketMemberInfoType; 51 | 52 | typedef struct { 53 | mRosPacketDataEnumType packet_type; 54 | mRosPacketMemberInfoType method; 55 | union { 56 | struct { 57 | mRosPacketMemberInfoType node_name; 58 | mRosPacketMemberInfoType topic_name; 59 | } topic; 60 | struct { 61 | mRosPacketMemberInfoType name; 62 | mRosPacketMemberInfoType topic_name; 63 | } publisher_update; 64 | } request; 65 | } mRosPacketDecodedRequestType; 66 | extern mRosPacketDataEnumType mros_xmlpacket_slave_request_decode(mRosPacketType *packet, mRosPacketDecodedRequestType *decoded_infop); 67 | 68 | 69 | /* 70 | * RequestTopic request 71 | */ 72 | extern mRosReturnType mros_xmlpacket_slave_reqtopic_get_topic_name(mRosPacketType *packet, char* topic_name, mros_uint32 len); 73 | 74 | /* 75 | * RequestTopic response 76 | */ 77 | extern mRosReturnType mros_xmlpacket_reqtopicres_result(mRosPacketType *packet); 78 | extern mRosPtrType mros_xmlpacket_reqtopicres_get_first_uri(mRosPacketType *packet, mros_uint32 *ipaddr, mros_int32 *port); 79 | extern mRosPtrType mros_xmlpacket_reqtopicres_get_next_uri(mRosPtrType ptr, mRosPacketType *packet, mros_uint32 *ipaddr, mros_int32 *port); 80 | 81 | /* 82 | * publisherUpdate request 83 | */ 84 | extern mRosPtrType mros_xmlpacket_pubupreq_get_first_uri(char *packet_data, mros_uint32 *ipaddr, mros_int32 *port); 85 | extern mRosPtrType mros_xmlpacket_pubupreq_get_next_uri(mRosPtrType ptr, mRosPacketType *packet, mros_uint32 *ipaddr, mros_int32 *port); 86 | 87 | /**************************************************** 88 | * TCPROS 89 | ****************************************************/ 90 | extern mRosReturnType mros_tcprospacket_get_body_size(mRosPacketType *packet, mRosSizeType *len); 91 | typedef struct { 92 | mros_uint16 callerid_len; 93 | mros_uint16 tcp_nodelay_len; 94 | mros_uint16 topic_len; 95 | mros_uint16 type_len; 96 | mros_uint16 md5sum_len; 97 | char *callerid; 98 | char *tcp_nodely; 99 | char *topic; 100 | char* type; 101 | char *md5sum; 102 | } mRosTcpRosPacketType; 103 | extern mRosReturnType mros_tcprospacket_decode(mRosPacketType *packet, mRosTcpRosPacketType *decoded_packet); 104 | 105 | 106 | /**************************************************** 107 | * TOPIC DATA 108 | ****************************************************/ 109 | extern mRosReturnType mros_topicpacket_get_body_size(mRosPacketType *packet, mRosSizeType *len); 110 | 111 | 112 | 113 | #ifdef __cplusplus 114 | } 115 | #endif 116 | 117 | #endif /* _MROS_PACKET_DECODER_CIMPL_H_ */ 118 | -------------------------------------------------------------------------------- /mros-lib/mros-src/packet/cimpl/mros_packet_encoder_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PACKET_ENCODER_CIMPL_H_ 2 | #define _MROS_PACKET_ENCODER_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_packet_cimpl.h" 9 | 10 | /************************************* 11 | * Encoder 12 | *************************************/ 13 | #define MROS_ENCODE_ARGS_MAX 5 14 | typedef struct { 15 | mRosPacketDataEnumType type; 16 | mRosSizeType args_char; 17 | const char* argv[MROS_ENCODE_ARGS_MAX]; 18 | mRosSizeType args_int; 19 | mros_uint32 argi[MROS_ENCODE_ARGS_MAX]; 20 | } mRosEncodeArgType; 21 | 22 | extern mRosReturnType mros_packet_encoder_init(void); 23 | 24 | /* 25 | * arg.type = MROS_PACKET_DATA_REGISTER_PUBLISHER_REQ; 26 | * arg.argv[0] = "registerPublisher"; 27 | * arg.argv[1] = req->node_name; 28 | * arg.argv[2] = req->topic_name; 29 | * arg.argv[3] = req->topic_typename; 30 | * arg.argv[4] = MROS_URI_SLAVE; 31 | */ 32 | /* 33 | * arg.type = MROS_PACKET_DATA_REGISTER_SUBSCRIBER_REQ; 34 | * arg.argv[0] = "registerSubscriber"; 35 | * arg.argv[1] = req->node_name; 36 | * arg.argv[2] = req->topic_name; 37 | * arg.argv[3] = req->topic_typename; 38 | * arg.argv[4] = MROS_URI_SLAVE; 39 | */ 40 | /* 41 | * arg.type = MROS_PACKET_DATA_REQUEST_TOPIC_REQ; 42 | * arg.argv[0] = "requestTopic"; 43 | * arg.argv[1] = req->node_name; 44 | * arg.argv[2] = req->topic_name; 45 | * arg.argv[3] = "TCPROS"; 46 | */ 47 | /* 48 | * arg.type = MROS_PACKET_DATA_REQUEST_TOPIC_RES; 49 | * arg.argi[0] = MROS_PUBLISHER_PORT_NO; 50 | * arg.argv[0] ="TCPROS"; 51 | * arg.argv[1] = MROS_NODE_IPADDR; 52 | */ 53 | extern mRosReturnType mros_packet_encode(mRosEncodeArgType *arg, mRosPacketType *packet); 54 | 55 | #ifdef __cplusplus 56 | } 57 | #endif 58 | 59 | #endif /* _MROS_PACKET_ENCODER_CIMPL_H_ */ 60 | -------------------------------------------------------------------------------- /mros-lib/mros-src/packet/cimpl/version/kinetic/mros_packet_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PACKET_CONFIG_H_ 2 | #define _MROS_PACKET_CONFIG_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #define MROS_TOPIC_RAWDATA_HEADER_SIZE 4 9 | #define MROS_TCPROS_RAWDATA_HEADER_SIZE 4 10 | 11 | /************************************** 12 | * PACKET 13 | **************************************/ 14 | #define MROS_PACKET_MAXSIZE_REQ_REGISTER_PUBLISHER 1024 15 | #define MROS_PACKET_MAXSIZE_RES_REGISTER_PUBLISHER 1024 16 | #define MROS_PACKET_MAXSIZE_REQ_REGISTER_SUBSCRIBER 1024 17 | #define MROS_PACKET_MAXSIZE_RES_REGISTER_SUBSCRIBER 1024 18 | #define MROS_PACKET_MAXSIZE_REQ_REQUEST_TOPIC 1024 19 | #define MROS_PACKET_MAXSIZE_RES_REQUEST_TOPIC 1024 20 | #define MROS_PACKET_MAXSIZE_REQ_PUBLISHER_UPDATE 1024 21 | 22 | 23 | #define MROS_PACKET_MAXSIZE_REQ_TCPROS 512 24 | #define MROS_PACKET_MAXSIZE_RES_TCPROS 512 25 | 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | 31 | #endif /* _MROS_PACKET_CONFIG_H_ */ 32 | -------------------------------------------------------------------------------- /mros-lib/mros-src/packet/template/version/kinetic/mros_packet_fmt_http.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PACKET_FMT_HTTP_H_ 2 | #define _MROS_PACKET_FMT_HTTP_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #define MROS_PACKET_HTT_OK_CODE_LEN 3 9 | #define MROS_PACKET_HTT_OK_CODE "200" 10 | 11 | /* 12 | * ARG1: xml size 13 | */ 14 | #define MROS_PACKET_FMT_HTTP_POST \ 15 | "POST /RPC2 HTTP/1.1\n" \ 16 | "Host: \n" \ 17 | "Accept-Encoding: \n" \ 18 | "User-Agent: \n" \ 19 | "Content-Type: \n" \ 20 | "Content-Length: %u\n\n" \ 21 | 22 | 23 | /* 24 | * ARG1: xml size 25 | */ 26 | #define MROS_PACKET_FMT_HTTP_OK \ 27 | "HTTP/1.1 200 OK\n" \ 28 | "Host: \n" \ 29 | "Accept-Encoding: \n" \ 30 | "User-Agent: \n" \ 31 | "Content-Type: \n" \ 32 | "Content-Length: %u\n\n" \ 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif 37 | 38 | #endif /* _MROS_PACKET_FMT_HTTP_H_ */ 39 | -------------------------------------------------------------------------------- /mros-lib/mros-src/packet/template/version/kinetic/mros_packet_fmt_tcpros.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PACKET_FMT_TCPROS_H_ 2 | #define _MROS_PACKET_FMT_TCPROS_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #define MROS_PACKET_FMT_TCPROS_CALLER_ID "callerid=%s" 9 | #define MROS_PACKET_FMT_TCPROS_TOPIC "topic=%s" 10 | #define MROS_PACKET_FMT_TCPROS_TYPE "type=%s" 11 | #define MROS_PACKET_FMT_TCPROS_MD5SUM "md5sum=%s" 12 | #define MROS_PACKET_FMT_TCPROS_TCPNODELAY "tcp_nodelay=%s" 13 | 14 | /* 15 | * ARG1: callerid 16 | * ARG2: topic 17 | * ARG3: type 18 | * ARG4: md5sum 19 | */ 20 | #define MROS_PACKET_FMT_TCPROS_TOPIC_REQ \ 21 | "SIZE" \ 22 | "SIZE" \ 23 | "callerid=%s" \ 24 | "SIZE" \ 25 | "tcp_nodelay=1" \ 26 | "SIZE" \ 27 | "topic=%s" \ 28 | "SIZE" \ 29 | "type=%s" \ 30 | "SIZE" \ 31 | "md5sum=%s" 32 | 33 | 34 | /* 35 | * ARG1: callerid 36 | * ARG2: topic 37 | * ARG3: type 38 | * ARG4: md5sum 39 | */ 40 | #define MROS_PACKET_FMT_TCPROS_TOPIC_RES \ 41 | "SIZE" \ 42 | "SIZE" \ 43 | "callerid=%s" \ 44 | "SIZE" \ 45 | "topic=%s" \ 46 | "SIZE" \ 47 | "type=%s" \ 48 | "SIZE" \ 49 | "md5sum=%s" 50 | 51 | 52 | #ifdef __cplusplus 53 | } 54 | #endif 55 | 56 | 57 | #endif /* _MROS_PACKET_FMT_TCPROS_H_ */ 58 | -------------------------------------------------------------------------------- /mros-lib/mros-src/packet/template/version/kinetic/mros_packet_fmt_xml.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PACKET_FMT_XML_H_ 2 | #define _MROS_PACKET_FMT_XML_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #define MROS_PACKET_XMLRPC_REQ_END_STR "" 9 | #define MROS_PACKET_XMLRPC_RES_END_STR "" 10 | 11 | /* 12 | * ARGS1: method name 13 | * ARGS2: node name 14 | * ARGS3: topic 15 | * ARGS4: type 16 | * ARGS5: uri 17 | */ 18 | #define MROS_PACKET_FMT_XML_REGISTER_REQ \ 19 | "\n" \ 20 | "\n" \ 21 | "%s\n" \ 22 | "\n" \ 23 | "\n" \ 24 | "%s\n" \ 25 | "\n" \ 26 | "\n" \ 27 | "%s\n" \ 28 | "\n" \ 29 | "\n" \ 30 | "%s\n" \ 31 | "\n" \ 32 | "\n" \ 33 | "%s\n" \ 34 | "\n" \ 35 | "" \ 36 | "\n" 37 | 38 | /* 39 | * ARGS1: method name 40 | * ARGS2: node name 41 | * ARGS3: topic 42 | * ARGS4: tcpros 43 | */ 44 | #define MROS_PACKET_FMT_XML_REQUEST_TOPIC_REQ \ 45 | "\n" \ 46 | "\n" \ 47 | "%s\n" \ 48 | "\n" \ 49 | "\n" \ 50 | "%s\n" \ 51 | "\n" \ 52 | "\n" \ 53 | "%s\n" \ 54 | "\n" \ 55 | "\n" \ 56 | "" \ 57 | "\n" \ 58 | "" \ 59 | "" \ 60 | "\n" \ 61 | "" \ 62 | "%s" \ 63 | "\n" \ 64 | "" \ 65 | "" \ 66 | "\n" \ 67 | "" \ 68 | "\n" \ 69 | "\n" \ 70 | "" \ 71 | "\n" \ 72 | 73 | /* 74 | * ARGS1: TCPROS 75 | * ARGS2: ipaddr 76 | * ARGS3: port 77 | */ 78 | #define MROS_PACKET_FMT_XML_REQUEST_TOPIC_RES \ 79 | "\n" \ 80 | "\n" \ 81 | "\n" \ 82 | "\n" \ 83 | "" \ 84 | "" \ 85 | "" \ 86 | "" \ 87 | "1" \ 88 | "\n" \ 89 | "\n" \ 90 | "" \ 91 | "\n" \ 92 | "" \ 93 | "%s\n" \ 94 | "%s\n" \ 95 | "%u\n" \ 96 | "" \ 97 | "" \ 98 | "\n" \ 99 | "" \ 100 | "" \ 101 | "" \ 102 | "\n" \ 103 | "\n" \ 104 | "\n" \ 105 | 106 | 107 | #ifdef __cplusplus 108 | } 109 | #endif 110 | 111 | #endif /* _MROS_PACKET_FMT_XML_H_ */ 112 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_client_rpc_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PROTOCOL_CLIENT_RPC_CIMPL_H_ 2 | #define _MROS_PROTOCOL_CLIENT_RPC_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | #include "mros_packet_cimpl.h" 10 | #include "mros_comm_tcp_client_cimpl.h" 11 | 12 | typedef struct { 13 | const char* node_name; 14 | const char* topic_name; 15 | const char* topic_typename; 16 | mRosPacketType *req_packet; 17 | } mRosRegisterTopicReqType; 18 | 19 | typedef struct { 20 | mRosReturnType result; 21 | mRosPacketType *reply_packet; 22 | } mRosRegisterTopicResType; 23 | extern mRosReturnType mros_rpc_register_publisher(mRosCommTcpClientType *client, mRosRegisterTopicReqType *req, mRosRegisterTopicResType *res); 24 | extern mRosReturnType mros_rpc_register_subscriber(mRosCommTcpClientType *client, mRosRegisterTopicReqType *req, mRosRegisterTopicResType *res); 25 | 26 | typedef struct { 27 | const char* node_name; 28 | const char* topic_name; 29 | mRosPacketType *req_packet; 30 | } mRosRequestTopicReqType; 31 | 32 | typedef struct { 33 | mRosReturnType result; 34 | mRosPacketType *reply_packet; 35 | } mRosRequestTopicResType; 36 | extern mRosReturnType mros_rpc_request_topic(mRosCommTcpClientType *client, mRosRequestTopicReqType *req, mRosRequestTopicResType *res); 37 | 38 | typedef struct { 39 | const char* node_name; 40 | const char* topic_name; 41 | const char* topic_typename; 42 | const char* md5sum; 43 | mRosPacketType *req_packet; 44 | } mRosRcpRosReqType; 45 | 46 | typedef struct { 47 | mRosReturnType result; 48 | mRosPacketType *reply_packet; 49 | } mRosTcpRosResType; 50 | extern mRosReturnType mros_rpc_tcpros(mRosCommTcpClientType *client, mRosRcpRosReqType *req, mRosTcpRosResType *res); 51 | 52 | 53 | #ifdef __cplusplus 54 | } 55 | #endif 56 | #endif /* _MROS_PROTOCOL_CLIENT_RPC_CIMPL_H_ */ 57 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_master_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PROTOCOL_MASTER_CIMPL_H_ 2 | #define _MROS_PROTOCOL_MASTER_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | typedef enum { 11 | MROS_PROTOCOL_MASTER_STATE_WAITING = 0, 12 | MROS_PROTOCOL_MASTER_STATE_REGISTER_PUBLISHER, 13 | MROS_PROTOCOL_MASTER_STATE_REGISTER_SUBSCRIBER, 14 | MROS_PROTOCOL_MASTER_STATE_REQUESTING_TOPIC, 15 | } mRosProtocolMasterStateEnumType; 16 | 17 | typedef enum { 18 | MROS_PROTOCOL_MASTER_REQ_REGISTER_PUBLISHER = 0, 19 | MROS_PROTOCOL_MASTER_REQ_REGISTER_SUBSCRIBER, 20 | } mRosProtocolMasterRequestEnumType; 21 | 22 | typedef struct { 23 | mRosProtocolMasterRequestEnumType req_type; 24 | mRosContainerObjType connector_obj; 25 | } mRosProtocolMasterRequestType; 26 | 27 | extern mRosReturnType mros_protocol_master_init(void); 28 | extern void mros_protocol_master_run(void); 29 | 30 | #ifdef __cplusplus 31 | } 32 | #endif 33 | #endif /* _MROS_PROTOCOL_MASTER_CIMPL_H_ */ 34 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_operation_cimpl.c: -------------------------------------------------------------------------------- 1 | #include "mros_protocol_operation_cimpl.h" 2 | #include "mros_comm_tcp_client_factory_cimpl.h" 3 | #include "mros_packet_decoder_cimpl.h" 4 | #include "mros_packet_encoder_cimpl.h" 5 | #include "mros_sys_config.h" 6 | #include 7 | 8 | mRosSizeType mros_protocol_get_buffersize(mRosSizeType body_size) 9 | { 10 | return (body_size + MROS_TOPIC_RAWDATA_HEADER_SIZE); 11 | } 12 | 13 | char* mros_protocol_get_body(char *buffer) 14 | { 15 | return &buffer[MROS_TOPIC_RAWDATA_HEADER_SIZE]; 16 | } 17 | 18 | 19 | mRosReturnType mros_protocol_topic_data_send(mRosCommTcpClientType *client, const char *data, mRosSizeType datalen) 20 | { 21 | mRosReturnType ret; 22 | mRosSizeType res; 23 | mRosPacketType packet; 24 | mRosEncodeArgType arg; 25 | 26 | packet.total_size = MROS_TOPIC_RAWDATA_HEADER_SIZE; 27 | packet.data_size = 0; 28 | 29 | packet.data = (char*)data; 30 | arg.type = MROS_PACKET_DATA_TOPIC; 31 | arg.args_int = 1; 32 | arg.argi[0] = datalen; 33 | arg.args_char = 0; 34 | ret = mros_packet_encode(&arg, &packet); 35 | if (ret != MROS_E_OK) { 36 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 37 | return ret; 38 | } 39 | //send header & body 40 | return mros_comm_tcp_client_send_all(client, data, datalen, &res); 41 | } 42 | 43 | mRosReturnType mros_protocol_topic_data_receive(mRosCommTcpClientType *client, mRosMemoryManagerType *mempool, mRosMemoryListEntryType **retp) 44 | { 45 | mRosPacketType packet; 46 | mRosSizeType len; 47 | mRosSizeType res; 48 | mRosReturnType ret; 49 | mRosMemoryListEntryType *mem_entryp; 50 | mros_int8 rawdata[MROS_TOPIC_RAWDATA_HEADER_SIZE]; 51 | *retp = MROS_NULL; 52 | 53 | ret = mros_comm_socket_wait_readable(&client->socket, 0); 54 | if (ret != MROS_E_OK) { 55 | //ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 56 | return ret; 57 | } 58 | //receive header 59 | ret = mros_comm_tcp_client_receive_all(client, rawdata, MROS_TOPIC_RAWDATA_HEADER_SIZE, &res); 60 | if (ret != MROS_E_OK) { 61 | //ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 62 | return ret; 63 | } 64 | //decode header 65 | packet.total_size = MROS_TOPIC_RAWDATA_HEADER_SIZE; 66 | packet.data_size = MROS_TOPIC_RAWDATA_HEADER_SIZE; 67 | packet.data = rawdata; 68 | ret = mros_topicpacket_get_body_size(&packet, &len); 69 | if (ret != MROS_E_OK) { 70 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 71 | return ret; 72 | } 73 | //receive body 74 | ret = mros_mem_alloc(mempool, len + MROS_TOPIC_RAWDATA_HEADER_SIZE, &mem_entryp); 75 | if (ret != MROS_E_OK) { 76 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 77 | return ret; 78 | } 79 | memcpy(&mem_entryp->data.memp[0], rawdata, MROS_TOPIC_RAWDATA_HEADER_SIZE); 80 | 81 | ret = mros_comm_tcp_client_receive_all(client, &mem_entryp->data.memp[MROS_TOPIC_RAWDATA_HEADER_SIZE], len, &res); 82 | if (ret != MROS_E_OK) { 83 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 84 | return ret; 85 | } 86 | *retp = mem_entryp; 87 | return MROS_E_OK; 88 | } 89 | 90 | void mros_protocol_client_obj_free(void* reqp) 91 | { 92 | mros_comm_tcp_client_free((mRosCommTcpClientListReqEntryType *)reqp); 93 | return; 94 | } 95 | 96 | 97 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_operation_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PROTOCOL_OPERATION_CIMPL_H_ 2 | #define _MROS_PROTOCOL_OPERATION_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_memory.h" 9 | #include "mros_comm_tcp_client_cimpl.h" 10 | 11 | extern mRosReturnType mros_protocol_topic_data_send(mRosCommTcpClientType *client, const char *data, mRosSizeType datalen); 12 | extern mRosReturnType mros_protocol_topic_data_receive(mRosCommTcpClientType *client, mRosMemoryManagerType *mempool, mRosMemoryListEntryType **retp); 13 | 14 | extern mRosSizeType mros_protocol_get_buffersize(mRosSizeType body_size); 15 | extern char* mros_protocol_get_body(char *buffer); 16 | 17 | /* 18 | * reqp: mRosCommTcpClientListReqEntryType 19 | */ 20 | extern void mros_protocol_client_obj_free(void* reqp); 21 | 22 | 23 | #ifdef __cplusplus 24 | } 25 | #endif 26 | #endif /* _MROS_PROTOCOL_OPERATION_CIMPL_H_ */ 27 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_publish_cimpl.c: -------------------------------------------------------------------------------- 1 | #include "mros_protocol_publish_cimpl.h" 2 | #include "mros_protocol_server_proc_cimpl.h" 3 | #include "mros_comm_tcp_client_cimpl.h" 4 | #include "mros_comm_tcp_server_cimpl.h" 5 | #include "mros_exclusive_area.h" 6 | #include "mros_sys_config.h" 7 | 8 | typedef union { 9 | char buffer; 10 | char buffer1[MROS_PACKET_MAXSIZE_REQ_TCPROS]; 11 | char buffer2[MROS_PACKET_MAXSIZE_RES_TCPROS]; 12 | } mRosPublishPacketBufferType; 13 | static mRosPublishPacketBufferType mros_publish_packet_buffer MROS_MATTR_BSS_NOCLR; 14 | 15 | typedef struct { 16 | mRosProtocolPublishStateEnumType state; 17 | mRosPacketType packet; 18 | mRosCommTcpServerType server_comm; 19 | mRosCommTcpClientType client_comm; 20 | } mRosProtocolPublishType; 21 | 22 | static mRosProtocolPublishType mros_protocol_publish MROS_MATTR_BSS_NOCLR; 23 | 24 | mRosReturnType mros_protocol_publish_init(void) 25 | { 26 | mRosReturnType ret = mros_comm_tcp_server_init(&mros_protocol_publish.server_comm); 27 | if (ret != MROS_E_OK) { 28 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 29 | return ret; 30 | } 31 | mros_protocol_publish.packet.total_size = sizeof(mRosPublishPacketBufferType); 32 | mros_protocol_publish.packet.data = &mros_publish_packet_buffer.buffer; 33 | mros_protocol_publish.state = MROS_PROTOCOL_PUBLISH_STATE_WAITING; 34 | ret = mros_comm_tcp_server_bind(&mros_protocol_publish.server_comm, MROS_PUBLISHER_PORT_NO); 35 | if (ret != MROS_E_OK) { 36 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 37 | return ret; 38 | } 39 | ret = mros_comm_tcp_server_listen(&mros_protocol_publish.server_comm, MROS_COMM_TCP_SERVER_LISTEN_MAX_DEFAULT_VALUE); 40 | if (ret != MROS_E_OK) { 41 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 42 | return ret; 43 | } 44 | return MROS_E_OK; 45 | } 46 | 47 | void mros_protocol_publish_run(void) 48 | { 49 | mRosReturnType ret; 50 | mROsExclusiveUnlockObjType unlck_obj; 51 | 52 | while (MROS_TRUE) { 53 | mros_protocol_publish.state = MROS_PROTOCOL_PUBLISH_STATE_WAITING; 54 | ret = mros_comm_tcp_server_accept(&mros_protocol_publish.server_comm, &mros_protocol_publish.client_comm); 55 | if (ret != MROS_E_OK) { 56 | continue; 57 | } 58 | ROS_INFO("INFO: pub accepted"); 59 | mros_protocol_publish.state = MROS_PROTOCOL_PUBLISH_STATE_STARTING_PUBLISH_TOPIC; 60 | ret = mros_proc_tcpros_receive(&mros_protocol_publish.client_comm, &mros_protocol_publish.packet); 61 | if (ret != MROS_E_OK) { 62 | ROS_WARN("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 63 | mros_comm_tcp_client_close(&mros_protocol_publish.client_comm); 64 | continue; 65 | } 66 | mros_exclusive_lock(&mros_exclusive_area, &unlck_obj); 67 | ret = mros_proc_pub_tcpros(&mros_protocol_publish.client_comm, &mros_protocol_publish.packet); 68 | mros_exclusive_unlock(&unlck_obj); 69 | if (ret != MROS_E_OK) { 70 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 71 | } 72 | } 73 | return; 74 | } 75 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_publish_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PROTOCOL_PUBLISH_CIMPL_H_ 2 | #define _MROS_PROTOCOL_PUBLISH_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | typedef enum { 11 | MROS_PROTOCOL_PUBLISH_STATE_WAITING = 0, 12 | MROS_PROTOCOL_PUBLISH_STATE_STARTING_PUBLISH_TOPIC, 13 | } mRosProtocolPublishStateEnumType; 14 | 15 | 16 | extern mRosReturnType mros_protocol_publish_init(void); 17 | extern void mros_protocol_publish_run(void); 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | #endif /* _MROS_PROTOCOL_PUBLISH_CIMPL_H_ */ 23 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_server_proc_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PROTOCOL_SERVER_PROC_CIMPL_H_ 2 | #define _MROS_PROTOCOL_SERVER_PROC_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_packet_cimpl.h" 9 | #include "mros_comm_tcp_client_cimpl.h" 10 | #include "mros_protocol_client_rpc_cimpl.h" 11 | #include "mros_node_cimpl.h" 12 | #include "mros_topic_connector_factory_cimpl.h" 13 | 14 | extern mRosReturnType mros_proc_init(void); 15 | extern mRosReturnType mros_proc_receive(mRosCommTcpClientType *client, mRosPacketType *packet); 16 | extern mRosReturnType mros_proc_tcpros_receive(mRosCommTcpClientType *client, mRosPacketType *packet); 17 | extern mRosReturnType mros_proc_slave(mRosCommTcpClientType *client, mRosPacketType *packet, mros_uint32 self_ipaddr); 18 | extern mRosReturnType mros_proc_pub_tcpros(mRosCommTcpClientType *client, mRosPacketType *packet); 19 | extern mRosReturnType mros_proc_request_outer_node_addition(mRosTopicIdType topic_id, mRosRequestTopicResType *rpc_response, void *api_reqp); 20 | typedef struct { 21 | mros_uint32 ipaddr; 22 | mros_int32 port; 23 | } mRosTopicOuterTcpConnectionType; 24 | extern mRosNodeIdType mros_proc_connector_get_first(mRosTopicIdType topic_id, mRosTopicConnectorEnumType type, mRosNodeEnumType nodeType, mRosTopicOuterTcpConnectionType *tcp_conn); 25 | 26 | 27 | #ifdef __cplusplus 28 | } 29 | #endif 30 | #endif /* _MROS_PROTOCOL_SERVER_PROC_CIMPL_H_ */ 31 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_slave_cimpl.c: -------------------------------------------------------------------------------- 1 | #include "mros_protocol_slave_cimpl.h" 2 | #include "mros_protocol_server_proc_cimpl.h" 3 | #include "mros_comm_tcp_client_cimpl.h" 4 | #include "mros_comm_tcp_server_cimpl.h" 5 | #include "mros_packet_encoder_cimpl.h" 6 | #include "mros_exclusive_area.h" 7 | #include "mros_sys_config.h" 8 | 9 | typedef union { 10 | char buffer; 11 | char buffer1[MROS_PACKET_MAXSIZE_REQ_REQUEST_TOPIC]; 12 | char buffer2[MROS_PACKET_MAXSIZE_RES_REQUEST_TOPIC]; 13 | char buffer3[MROS_PACKET_MAXSIZE_REQ_PUBLISHER_UPDATE]; 14 | } mRosSlavePacketBufferType; 15 | static mRosSlavePacketBufferType mros_slave_packet_buffer MROS_MATTR_BSS_NOCLR; 16 | 17 | typedef struct { 18 | mRosProtocolSlaveStateEnumType state; 19 | mRosEncodeArgType arg; 20 | mRosPacketType packet; 21 | mRosCommTcpServerType server_comm; 22 | mRosCommTcpClientType client_comm; 23 | mros_uint32 self_ipaddr; 24 | } mRosProtocolSlaveType; 25 | 26 | static mRosProtocolSlaveType mros_protocol_slave MROS_MATTR_BSS_NOCLR; 27 | 28 | mRosReturnType mros_protocol_slave_init(void) 29 | { 30 | mRosReturnType ret = mros_comm_tcp_server_init(&mros_protocol_slave.server_comm); 31 | if (ret != MROS_E_OK) { 32 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 33 | return ret; 34 | } 35 | 36 | mros_protocol_slave.packet.total_size = sizeof(mRosSlavePacketBufferType); 37 | mros_protocol_slave.packet.data = &mros_slave_packet_buffer.buffer; 38 | mros_protocol_slave.state = MROS_PROTOCOL_SLAVE_STATE_WAITING; 39 | ret = mros_comm_tcp_server_bind(&mros_protocol_slave.server_comm, MROS_SLAVE_PORT_NO); 40 | if (ret != MROS_E_OK) { 41 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 42 | return ret; 43 | } 44 | ret = mros_comm_tcp_server_listen(&mros_protocol_slave.server_comm, MROS_COMM_TCP_SERVER_LISTEN_MAX_DEFAULT_VALUE); 45 | if (ret != MROS_E_OK) { 46 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 47 | return ret; 48 | } 49 | ret = mros_comm_inet_get_ipaddr((const char *)mros_comm_config.mros_node_ipaddr, &mros_protocol_slave.self_ipaddr); 50 | if (ret != MROS_E_OK) { 51 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 52 | return ret; 53 | } 54 | return MROS_E_OK; 55 | } 56 | 57 | void mros_protocol_slave_run(void) 58 | { 59 | mRosReturnType ret; 60 | mROsExclusiveUnlockObjType unlck_obj; 61 | 62 | while (MROS_TRUE) { 63 | mros_protocol_slave.state = MROS_PROTOCOL_SLAVE_STATE_WAITING; 64 | ret = mros_comm_tcp_server_accept(&mros_protocol_slave.server_comm, &mros_protocol_slave.client_comm); 65 | if (ret != MROS_E_OK) { 66 | continue; 67 | } 68 | ROS_INFO("INFO: slave accepted"); 69 | mros_protocol_slave.state = MROS_PROTOCOL_SLAVE_STATE_REPLYING_REQUEST_TOPIC; 70 | ret = mros_proc_receive(&mros_protocol_slave.client_comm, &mros_protocol_slave.packet); 71 | if (ret != MROS_E_OK) { 72 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 73 | mros_comm_tcp_client_close(&mros_protocol_slave.client_comm); 74 | continue; 75 | } 76 | mros_exclusive_lock(&mros_exclusive_area, &unlck_obj); 77 | ret = mros_proc_slave(&mros_protocol_slave.client_comm, &mros_protocol_slave.packet, mros_protocol_slave.self_ipaddr); 78 | mros_exclusive_unlock(&unlck_obj); 79 | if (ret != MROS_E_OK) { 80 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 81 | continue; 82 | } 83 | mros_comm_tcp_client_close(&mros_protocol_slave.client_comm); 84 | } 85 | return; 86 | } 87 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_slave_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PROTOCOL_SLAVE_CIMPL_H_ 2 | #define _MROS_PROTOCOL_SLAVE_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | typedef enum { 11 | MROS_PROTOCOL_SLAVE_STATE_WAITING = 0, 12 | MROS_PROTOCOL_SLAVE_STATE_REPLYING_REQUEST_TOPIC, 13 | } mRosProtocolSlaveStateEnumType; 14 | 15 | 16 | extern mRosReturnType mros_protocol_slave_init(void); 17 | extern void mros_protocol_slave_run(void); 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | #endif /* _MROS_PROTOCOL_SLAVE_CIMPL_H_ */ 23 | -------------------------------------------------------------------------------- /mros-lib/mros-src/protocol/cimpl/mros_protocol_subscribe_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_PROTOCOL_SUBSCRIBE_CIMPL_H_ 2 | #define _MROS_PROTOCOL_SUBSCRIBE_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | typedef enum { 11 | MROS_PROTOCOL_SUBSCRIBE_STATE_WAITING = 0, 12 | MROS_PROTOCOL_SUBSCRIBE_STATE_PUB_CONNECTING, 13 | MROS_PROTOCOL_SUBSCRIBE_STATE_PUB_REQUESTING, 14 | } mRosProtocolSubscribeStateEnumType; 15 | 16 | extern mRosReturnType mros_protocol_subscribe_init(void); 17 | extern void mros_protocol_subscribe_run(void); 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | #endif /* _MROS_PROTOCOL_SUBSCRIBE_CIMPL_H_ */ 23 | -------------------------------------------------------------------------------- /mros-lib/mros-src/topic/cimpl/mros_topic_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_TOPIC_CIMPL_H_ 2 | #define _MROS_TOPIC_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_memory.h" 9 | 10 | extern mRosReturnType mros_topic_init(void); 11 | extern mRosContainerObjType mros_topic_get_first(void); 12 | extern mRosContainerObjType mros_topic_get_next(mRosContainerObjType obj); 13 | extern const char *mros_topic_get_topic_name(mRosTopicIdType id); 14 | extern const char *mros_topic_get_topic_typename(mRosTopicIdType id); 15 | extern mRosTopicIdType mros_topic_get_id(mRosContainerObjType obj); 16 | extern mRosReturnType mros_topic_get(const char *topic_name, mRosTopicIdType *id); 17 | extern mRosReturnType mros_topic_create(const char *topic_name, const char *topic_typename, mRosTopicIdType *id); 18 | extern mRosReturnType mros_topic_set_quesize_byname(const char *topic_name, mRosSizeType size); 19 | extern mRosReturnType mros_topic_set_quesize_byid(mRosTopicIdType id, mRosSizeType size); 20 | extern mRosReturnType mros_topic_remove_byname(const char *topic_name); 21 | extern mRosReturnType mros_topic_remove_byid(mRosTopicIdType id); 22 | extern mRosReturnType mros_topic_add_data(mRosTopicIdType id, mRosMemoryListEntryType *data); 23 | extern mRosReturnType mros_topic_get_data(mRosTopicIdType id, mRosMemoryListEntryType **data); 24 | 25 | 26 | extern mRosReturnType mros_topic_set_typeid(mRosTopicIdType topic_id, mros_uint32 type_id); 27 | extern mRosReturnType mros_topic_get_typeid(mRosTopicIdType topic_id, mros_uint32 *type_id); 28 | 29 | extern mRosReturnType mros_topic_set_definition(mRosTopicIdType topic_id, const char* definition); 30 | extern mRosReturnType mros_topic_get_definition(mRosTopicIdType topic_id, const char **definition); 31 | 32 | extern mRosReturnType mros_topic_set_md5sum(mRosTopicIdType topic_id, const char* md5sum); 33 | extern mRosReturnType mros_topic_get_md5sum(mRosTopicIdType topic_id, const char **md5sum); 34 | 35 | #ifdef __cplusplus 36 | } 37 | #endif 38 | #endif /* _MROS_TOPIC_CIMPL_H_ */ 39 | -------------------------------------------------------------------------------- /mros-lib/mros-src/topic/cimpl/mros_topic_connector_factory_cimpl.c: -------------------------------------------------------------------------------- 1 | #include "mros_topic_connector_factory_cimpl.h" 2 | #include "mros_sys_config.h" 3 | 4 | static mros_boolean mros_topic_connector_is_inialized[MROS_TOPIC_CONNECTOR_NUM] = { 5 | MROS_FALSE, 6 | MROS_FALSE, 7 | }; 8 | /* 9 | * topic connector Config APIs 10 | */ 11 | #define MROS_TOPIC_CONNECTOR_CONFIG_DECLARE_MANAGER(manager_name, conn_entry_num) \ 12 | static mRosTopicConnectorListEntryType manager_name##_conn_array [(conn_entry_num)] MROS_MATTR_BSS_NOCLR; \ 13 | static mRosTopicConnectorListEntryRootType manager_name##_topic_array [MROS_TOPIC_MAX_NUM] MROS_MATTR_BSS_NOCLR; \ 14 | static mRosTopicConnectorManagerType manager_name MROS_MATTR_BSS_NOCLR; \ 15 | static mRosTopicConnectorConfigType manager_name##_config = { \ 16 | (conn_entry_num), \ 17 | manager_name##_conn_array, \ 18 | manager_name##_topic_array, \ 19 | }; 20 | 21 | MROS_TOPIC_CONNECTOR_CONFIG_DECLARE_MANAGER(pub_connector_mgr, (MROS_PUB_TOPIC_CONNECTOR_MAX_NUM) ); 22 | MROS_TOPIC_CONNECTOR_CONFIG_DECLARE_MANAGER(sub_connector_mgr, (MROS_SUB_TOPIC_CONNECTOR_MAX_NUM) ); 23 | 24 | mRosTopicConnectorManagerType *mros_topic_connector_factory_create(mRosTopicConnectorEnumType type) 25 | { 26 | mRosTopicConnectorManagerType *mgrp = MROS_NULL; 27 | mRosTopicConnectorConfigType *cfgp = MROS_NULL; 28 | mRosReturnType ret; 29 | 30 | switch (type) { 31 | case MROS_TOPIC_CONNECTOR_PUB: 32 | mgrp = &pub_connector_mgr; 33 | cfgp = &pub_connector_mgr_config; 34 | break; 35 | case MROS_TOPIC_CONNECTOR_SUB: 36 | mgrp = &sub_connector_mgr; 37 | cfgp = &sub_connector_mgr_config; 38 | break; 39 | default: 40 | break; 41 | } 42 | if (mgrp == MROS_NULL) { 43 | return MROS_NULL; 44 | } 45 | if (mros_topic_connector_is_inialized[type] == MROS_TRUE) { 46 | return mgrp; 47 | } 48 | ret = mros_topic_connector_init(cfgp, mgrp); 49 | if (ret != MROS_E_OK) { 50 | return MROS_NULL; 51 | } 52 | mros_topic_connector_is_inialized[type] = MROS_TRUE; 53 | return mgrp; 54 | } 55 | 56 | mRosTopicConnectorManagerType *mros_topic_connector_factory_get(mRosTopicConnectorEnumType type) 57 | { 58 | mRosTopicConnectorManagerType *mgrp = MROS_NULL; 59 | 60 | switch (type) { 61 | case MROS_TOPIC_CONNECTOR_PUB: 62 | mgrp = &pub_connector_mgr; 63 | break; 64 | case MROS_TOPIC_CONNECTOR_SUB: 65 | mgrp = &sub_connector_mgr; 66 | break; 67 | default: 68 | break; 69 | } 70 | if (mgrp == MROS_NULL) { 71 | return MROS_NULL; 72 | } 73 | if (mros_topic_connector_is_inialized[type] == MROS_FALSE) { 74 | return MROS_NULL; 75 | } 76 | return mgrp; 77 | } 78 | -------------------------------------------------------------------------------- /mros-lib/mros-src/topic/cimpl/mros_topic_connector_factory_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_TOPIC_CONNECTOR_FACTORY_CIMPL_H_ 2 | #define _MROS_TOPIC_CONNECTOR_FACTORY_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_topic_connector_cimpl.h" 9 | 10 | typedef enum { 11 | MROS_TOPIC_CONNECTOR_PUB = 0, 12 | MROS_TOPIC_CONNECTOR_SUB, 13 | MROS_TOPIC_CONNECTOR_NUM, 14 | } mRosTopicConnectorEnumType; 15 | 16 | extern mRosTopicConnectorManagerType *mros_topic_connector_factory_create(mRosTopicConnectorEnumType type); 17 | extern mRosTopicConnectorManagerType *mros_topic_connector_factory_get(mRosTopicConnectorEnumType type); 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | #endif /* _MROS_TOPIC_CONNECTOR_FACTORY_CIMPL_H_ */ 23 | -------------------------------------------------------------------------------- /mros-lib/mros-src/topic/cimpl/mros_topic_runner_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_TOPIC_RUNNER_CIMPL_H_ 2 | #define _MROS_TOPIC_RUNNER_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | extern void mros_topic_data_publisher_run(void); 11 | extern void mros_topic_data_subscriber_run(void); 12 | 13 | #ifdef __cplusplus 14 | } 15 | #endif 16 | #endif /* _MROS_TOPIC_RUNNER_CIMPL_H_ */ 17 | -------------------------------------------------------------------------------- /mros-lib/mros-src/transfer/cimpl/mros_topic_data_publisher_cimpl.c: -------------------------------------------------------------------------------- 1 | #include "mros_topic_data_publisher_cimpl.h" 2 | #include "mros_topic_connector_factory_cimpl.h" 3 | #include "mros_topic_cimpl.h" 4 | #include "mros_array_container.h" 5 | #include "mros_usr_config.h" 6 | 7 | MROS_ARRAY_CONTAINER_CONFIG_DECLARE_MANAGER(mros_topic_pub_mgr, MROS_TOPIC_MAX_NUM); 8 | 9 | static void mros_topic_publish(mRosTopicConnectorManagerType *mgrp, mRosNodeEnumType type, mRosContainerObjType topic_obj) 10 | { 11 | mRosReturnType ret; 12 | mRosContainerObjType obj; 13 | mRosMemoryListEntryType *topic_data; 14 | mRosTopicIdType topic_id; 15 | 16 | ret = mros_topic_connector_get_topic(topic_obj, &topic_id); 17 | if (ret != MROS_E_OK) { 18 | return; 19 | } 20 | 21 | obj = mros_topic_connector_get_first(mgrp, type, topic_obj); 22 | if (obj == MROS_COBJ_NULL) { 23 | return; 24 | } 25 | while (obj != MROS_COBJ_NULL) { 26 | ret = mros_topic_connector_receive_data(mgrp, obj, &topic_data); 27 | if (ret != MROS_E_OK) { 28 | obj = mros_topic_connector_get_next(mgrp, topic_obj, obj); 29 | continue; 30 | } 31 | ret = mros_topic_add_data(topic_id, topic_data); 32 | if (ret != MROS_E_OK) { 33 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 34 | (void)mros_mem_free(topic_data->data.mgrp, topic_data); 35 | } 36 | obj = mros_topic_connector_get_next(mgrp, topic_obj, obj); 37 | } 38 | 39 | return; 40 | } 41 | 42 | mRosReturnType mros_topic_data_publisher_init(void) 43 | { 44 | return MROS_E_OK; 45 | } 46 | 47 | void mros_topic_data_publisher_run(void) 48 | { 49 | mRosContainerObjType topic_obj; 50 | mRosTopicConnectorManagerType *mgrp; 51 | 52 | mgrp = mros_topic_connector_factory_get(MROS_TOPIC_CONNECTOR_PUB); 53 | if (mgrp == MROS_NULL) { 54 | return; 55 | } 56 | mros_topic_pub_mgr.count = 0; 57 | 58 | /************************** 59 | * INNER NODE 60 | **************************/ 61 | topic_obj = mros_topic_connector_get_topic_first(mgrp); 62 | while (topic_obj != MROS_COBJ_NULL) { 63 | mros_array_container_add(&mros_topic_pub_mgr, topic_obj); 64 | 65 | mros_topic_publish(mgrp, MROS_NODE_TYPE_INNER, topic_obj); 66 | topic_obj = mros_topic_connector_get_topic_next(mgrp, topic_obj); 67 | } 68 | 69 | /************************** 70 | * OUTER NODE 71 | **************************/ 72 | mros_uint32 i; 73 | for (i = 0; i < mros_topic_pub_mgr.count; i++) { 74 | mros_topic_publish(mgrp, MROS_NODE_TYPE_OUTER, mros_topic_pub_mgr.array[i]); 75 | } 76 | 77 | mros_topic_connector_purge(mgrp); 78 | return; 79 | } 80 | -------------------------------------------------------------------------------- /mros-lib/mros-src/transfer/cimpl/mros_topic_data_publisher_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_TOPIC_DATA_PUBLISHER_CIMPL_H_ 2 | #define _MROS_TOPIC_DATA_PUBLISHER_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | extern mRosReturnType mros_topic_data_publisher_init(void); 11 | 12 | #ifdef __cplusplus 13 | } 14 | #endif 15 | #endif /* _MROS_TOPIC_DATA_PUBLISHER_CIMPL_H_ */ 16 | -------------------------------------------------------------------------------- /mros-lib/mros-src/transfer/cimpl/mros_topic_data_subscriber_cimpl.c: -------------------------------------------------------------------------------- 1 | #include "mros_topic_data_subscriber_cimpl.h" 2 | #include "mros_topic_connector_factory_cimpl.h" 3 | #include "mros_topic_cimpl.h" 4 | #include "mros_array_container.h" 5 | #include "mros_usr_config.h" 6 | 7 | MROS_ARRAY_CONTAINER_CONFIG_DECLARE_MANAGER(mros_topic_sub_mgr, MROS_TOPIC_MAX_NUM); 8 | 9 | static void mros_topic_subscribe(mRosTopicConnectorManagerType *mgrp, mRosNodeEnumType type, mRosContainerObjType topic_obj, mRosMemoryListEntryType *topic_data) 10 | { 11 | mRosReturnType ret; 12 | mRosContainerObjType obj; 13 | 14 | obj = mros_topic_connector_get_first(mgrp, type, topic_obj); 15 | if (obj == MROS_COBJ_NULL) { 16 | return; 17 | } 18 | 19 | while (obj != MROS_COBJ_NULL) { 20 | ret = mros_topic_connector_send_data(mgrp, obj, topic_data->data.memp, topic_data->data.size); 21 | if (ret != MROS_E_OK) { 22 | ROS_WARN("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 23 | } 24 | obj = mros_topic_connector_get_next(mgrp, topic_obj, obj); 25 | } 26 | 27 | return; 28 | } 29 | 30 | mRosReturnType mros_topic_data_subscriber_init(void) 31 | { 32 | return MROS_E_OK; 33 | } 34 | 35 | void mros_topic_data_subscriber_run(void) 36 | { 37 | mRosContainerObjType topic_obj; 38 | mRosTopicConnectorManagerType *mgrp; 39 | mRosMemoryListEntryType *topic_data; 40 | mRosTopicIdType topic_id; 41 | mRosReturnType ret; 42 | mros_uint32 i; 43 | 44 | mgrp = mros_topic_connector_factory_get(MROS_TOPIC_CONNECTOR_SUB); 45 | if (mgrp == MROS_NULL) { 46 | return; 47 | } 48 | mros_topic_sub_mgr.count = 0; 49 | 50 | topic_obj = mros_topic_connector_get_topic_first(mgrp); 51 | while (topic_obj != MROS_COBJ_NULL) { 52 | mros_array_container_add(&mros_topic_sub_mgr, topic_obj); 53 | topic_obj = mros_topic_connector_get_topic_next(mgrp, topic_obj); 54 | } 55 | for (i = 0; i < mros_topic_sub_mgr.count; i++) { 56 | ret = mros_topic_connector_get_topic(mros_topic_sub_mgr.array[i], &topic_id); 57 | if (ret != MROS_E_OK) { 58 | continue; 59 | } 60 | ret = mros_topic_get_data(topic_id, &topic_data); 61 | if (ret == MROS_E_OK) { 62 | mros_topic_subscribe(mgrp, MROS_NODE_TYPE_INNER, mros_topic_sub_mgr.array[i], topic_data); 63 | mros_topic_subscribe(mgrp, MROS_NODE_TYPE_OUTER, mros_topic_sub_mgr.array[i], topic_data); 64 | (void)mros_mem_free(topic_data->data.mgrp, topic_data); 65 | } 66 | } 67 | 68 | mros_topic_connector_purge(mgrp); 69 | return; 70 | } 71 | -------------------------------------------------------------------------------- /mros-lib/mros-src/transfer/cimpl/mros_topic_data_subscriber_cimpl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_TOPIC_DATA_SUBSCRIBER_CIMPL_H_ 2 | #define _MROS_TOPIC_DATA_SUBSCRIBER_CIMPL_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_types.h" 9 | 10 | extern mRosReturnType mros_topic_data_subscriber_init(void); 11 | 12 | #ifdef __cplusplus 13 | } 14 | #endif 15 | #endif /* _MROS_TOPIC_DATA_SUBSCRIBER_CIMPL_H_ */ 16 | -------------------------------------------------------------------------------- /mros-lib/mros.cfg: -------------------------------------------------------------------------------- 1 | /* 2 | * mROS通信ライブラリコンフィギュレーションファイル 3 | */ 4 | 5 | INCLUDE("target_timer.cfg"); 6 | INCLUDE("syssvc/syslog.cfg"); 7 | INCLUDE("syssvc/banner.cfg"); 8 | INCLUDE("syssvc/serial.cfg"); 9 | INCLUDE("syssvc/logtask.cfg"); 10 | 11 | INCLUDE("../mbed-lib/common/common.cfg"); 12 | INCLUDE("../mbed-lib/EthernetInterface/EthernetInterface.cfg"); 13 | 14 | 15 | CRE_TSK(MAIN_TASK, { TA_ACT, 0, main_task, MAIN_TASK_PRI, MROS_TASK_STACK_SIZE, NULL }); 16 | CRE_TSK(PUB_TASK,{ TA_NULL,0,pub_task,MROS_TASK_PRI,MROS_TASK_STACK_SIZE,NULL }); 17 | CRE_TSK(SUB_TASK,{ TA_NULL,0,sub_task,MROS_TASK_PRI,MROS_TASK_STACK_SIZE,NULL }); 18 | CRE_TSK(XML_SLV_TASK,{ TA_NULL,0,xml_slv_task,MROS_TASK_PRI,MROS_TASK_STACK_SIZE,NULL }); 19 | CRE_TSK(XML_MAS_TASK,{ TA_NULL,0,xml_mas_task,MROS_TASK_PRI,MROS_TASK_STACK_SIZE,NULL }); 20 | AID_TSK(3); 21 | 22 | CRE_CYC(CYC_HDR,{TA_STA,0,cyclic_handler,MROS_LOOP_RATE,1}); 23 | 24 | DEF_KMM({ KMM_SIZE, NULL }); 25 | -------------------------------------------------------------------------------- /mros_msg-gen/including_msgs.json: -------------------------------------------------------------------------------- 1 | { 2 | "catkin_ws_dir": "../../../catkin_ws/", 3 | "ros_include_dir": "/opt/ros/kinetic/include", 4 | "including_msgs": [ 5 | "mros_test/PersonalData.h", 6 | "std_msgs/String.h" 7 | ], 8 | "depending_packages": [ 9 | "std_msgs", 10 | "mros_test" 11 | ], 12 | "depending_build_in_packages": [ 13 | ] 14 | } 15 | -------------------------------------------------------------------------------- /mros_msg-gen/msg_headers_includer.tpl: -------------------------------------------------------------------------------- 1 | {% for msg in std_msgs %} 2 | #include "{{msg.pkg}}/{{msg.name}}.h"{% endfor %} 3 | {% for msg in msgs %} 4 | #include "{{msg.pkg}}/{{msg.name}}.h"{% endfor %} 5 | 6 | 7 | static void callCallback(int id, void (*fp)(void *), char *rbuf){ 8 | switch(id){ 9 | {%for msg in std_msgs %} case {{ msg.id }}: 10 | subtask_methods::CallCallbackFuncs<{{ msg.id }}>().call(fp,rbuf); 11 | break; 12 | {% endfor %} 13 | {%for msg in msgs %} case {{ msg.id }}: 14 | subtask_methods::CallCallbackFuncs<{{ msg.id }}>().call(fp,rbuf); 15 | break; 16 | {% endfor %} 17 | } 18 | } 19 | 20 | static const char* getMD5Sum(int id){ 21 | switch(id){ 22 | {%for msg in std_msgs %} case {{ msg.id }}: 23 | return message_traits::MD5Sum<{{ msg.id }}>().value(); 24 | {% endfor %} 25 | {%for msg in msgs %} case {{ msg.id }}: 26 | return message_traits::MD5Sum<{{ msg.id }}>().value(); 27 | {% endfor %} 28 | } 29 | return NULL; 30 | } -------------------------------------------------------------------------------- /mros_msg-gen/msg_headers_spetializer.tpl: -------------------------------------------------------------------------------- 1 | #include "ros.h" 2 | {% for msg in std_msgs %} 3 | #include "{{msg.pkg}}/{{msg.name}}.h"{% endfor %} 4 | {% for msg in msgs %} 5 | #include "{{msg.pkg}}/{{msg.name}}.h"{% endfor %} 6 | 7 | {% for msg in std_msgs %} 8 | template ros::Subscriber ros::NodeHandle::subscribe(std::string,int,void (*fp)({{msg.pkg}}::{{msg.name}}*)); 9 | template ros::Publisher ros::NodeHandle::advertise<{{msg.pkg}}::{{msg.name}}>(std::string, int); 10 | template void ros::Publisher::publish({{msg.pkg}}::{{msg.name}}&);{% endfor %} 11 | {% for msg in msgs %} 12 | template ros::Subscriber ros::NodeHandle::subscribe(std::string,int,void (*fp)({{msg.pkg}}::{{msg.name}}*)); 13 | template ros::Publisher ros::NodeHandle::advertise<{{msg.pkg}}::{{msg.name}}>(std::string, int); 14 | template void ros::Publisher::publish({{msg.pkg}}::{{msg.name}}&);{% endfor %} 15 | -------------------------------------------------------------------------------- /mros_msg-gen/msg_max_size.h: -------------------------------------------------------------------------------- 1 | #define PUB_MSG_MAX_SIZE 25 -------------------------------------------------------------------------------- /mros_msg-gen/msg_max_size.tpl: -------------------------------------------------------------------------------- 1 | #define PUB_MSG_MAX_SIZE {{size}} 2 | -------------------------------------------------------------------------------- /mros_ws/Makefile.cv: -------------------------------------------------------------------------------- 1 | OPENCV_INCLUDE_PATHS += -I$(OPENCV_DIR)/include 2 | 3 | OPENCV_LIBS += $(OPENCV_DIR)/lib/TOOLCHAIN_GCC_ARM/libopencv_core320.a \ 4 | $(OPENCV_DIR)/lib/TOOLCHAIN_GCC_ARM/libopencv_imgproc320.a 5 | 6 | INCLUDE_PATHS += -I$(OPENCV_DIR)/include 7 | LIBS += $(OPENCV_LIBS) 8 | 9 | -------------------------------------------------------------------------------- /mros_ws/Makefile.mbd: -------------------------------------------------------------------------------- 1 | # This file was automagically generated by mbed.org. For more information, 2 | # see http://mbed.org/handbook/Exporting-to-GCC-ARM-Embedded 3 | 4 | GCC_BIN = 5 | LIB = libmbed 6 | PROJECT = GR-PEACH_mbed 7 | 8 | 9 | ALL_OBJ = $(ASM_OBJ) $(C_OBJ) $(CXX_OBJ) 10 | 11 | LIBRARY_PATHS = 12 | LIBRARIES = 13 | 14 | LINKER_SCRIPT = $(MBED_LIB_DIR)/mbed-src/targets/cmsis/TARGET_RENESAS/TARGET_RZ_A1H/TOOLCHAIN_GCC_ARM/RZA1H.ld 15 | 16 | 17 | ############################################################################### 18 | ifneq ($(USE_TRUESTUDIO),true) 19 | AS = $(GCC_BIN)arm-none-eabi-as 20 | CC = $(GCC_BIN)arm-none-eabi-gcc 21 | CPP = $(GCC_BIN)arm-none-eabi-g++ 22 | LD = $(GCC_BIN)arm-none-eabi-gcc 23 | OBJCOPY = $(GCC_BIN)arm-none-eabi-objcopy 24 | OBJDUMP = $(GCC_BIN)arm-none-eabi-objdump 25 | SIZE = $(GCC_BIN)arm-none-eabi-size 26 | AR = $(GCC_BIN)arm-none-eabi-ar 27 | NM = $(GCC_BIN)arm-none-eabi-nm 28 | RANLIB = $(GCC_BIN)arm-none-eabi-ranlib 29 | else 30 | AS = arm-atollic-eabi-as 31 | CC = arm-atollic-eabi-gcc 32 | CPP = arm-atollic-eabi-g++ 33 | LD = arm-atollic-eabi-gcc 34 | OBJCOPY = arm-atollic-eabi-objcopy 35 | OBJDUMP = arm-atollic-eabi-objdump 36 | SIZE = arm-atollic-eabi-size 37 | AR = arm-atollic-eabi-ar 38 | NM = arm-atollic-eabi-nm 39 | RANLIB = arm-atollic-eabi-ranlib 40 | endif 41 | 42 | ifneq ($(USE_TRUESTUDIO),true) 43 | CPU = -mcpu=cortex-a9 -mthumb -mthumb-interwork -marm -march=armv7-a -mfpu=vfpv3 -mfloat-abi=hard -mno-unaligned-access 44 | else 45 | CPU = -mcpu=cortex-a9 -mthumb -marm -march=armv7-a -mfpu=vfpv3 -mfloat-abi=hard -mno-unaligned-access 46 | endif 47 | 48 | CC_FLAGS = $(CPU) -c -g -fno-common -fmessage-length=0 -Wall -Wextra -Wno-unused-parameter -Wno-missing-field-initializers 49 | CC_FLAGS += -fno-exceptions -fno-builtin -ffunction-sections -fdata-sections -fno-delete-null-pointer-checks -fomit-frame-pointer 50 | CC_FLAGS += -MMD -MP 51 | 52 | CC_SYMBOLS = -DTARGET_RZ_A1H -DTARGET_MBRZA1H -DTARGET_FF_ARDUINO -DTOOLCHAIN_GCC_ARM -DTOOLCHAIN_GCC -DTARGET_CORTEX_A -DMBED_BUILD_TIMESTAMP=1458722901.8 -D__MBED__=1 -DTARGET_RENESAS -DTARGET_LIKE_MBED -D__MBED_CMSIS_RTOS_CA9 -D__EVAL -D__FPU_PRESENT -DTARGET_LIKE_CORTEX_A9 -D__CMSIS_RTOS -DTARGET_A9 -D__CORTEX_A9 -DARM_MATH_CA9 -DTOPPERS_RZA1H -D_DEBUG_HTTP_SERVER_H -DLWIP_DEBUG -DGRPEACH -DWOLFSSL_USER_SETTINGS -DWOLFSSL_LIB 53 | 54 | 55 | #LD_FLAGS = $(CPU) -Wl,--gc-sections --specs=nano.specs -u _printf_float -u _scanf_float -Wl,--wrap,main -Wl,-Map=$(PROJECT).map,--cref 56 | LD_FLAGS = $(CPU) -Wl,--gc-sections -nostartfiles --specs=nano.specs -u _printf_float -u _scanf_float -Wl,-Map=$(PROJECT).map,--cref 57 | # LD_SYS_LIBS = -lstdc++ -lsupc++ -lm -lc -lgcc -lnosys 58 | LD_SYS_LIBS = -lstdc++ -lsupc++ -lm -lc -lc -lgcc -lnosys 59 | 60 | ifeq ($(DEBUG), true) 61 | CC_FLAGS += -DDEBUG -O0 62 | ASM_FLAGS += -DDEBUG -O0 63 | else 64 | CC_FLAGS += -DNDEBUG -Os 65 | endif 66 | 67 | .PHONY: all clean 68 | 69 | all: $(LIB).a 70 | 71 | #clean: 72 | # rm -f $(LIB).lib $(ALL_OBJ) $(DEPS) 73 | 74 | $(ASM_OBJ): %.o: %.S 75 | $(CC) $(CPU) $(ASM_FLAGS) -c -x assembler-with-cpp -o $@ $< 76 | 77 | $(C_OBJ): %.o: %.c 78 | $(CC) $(CC_FLAGS) $(CC_SYMBOLS) -std=gnu99 $(INCLUDE_PATHS) -o $@ $< 79 | 80 | $(CXX_OBJ): %.o: %.cpp 81 | $(CPP) $(CC_FLAGS) $(CC_SYMBOLS) -std=gnu++98 -fno-rtti $(INCLUDE_PATHS) -o $@ $< 82 | 83 | $(LIB).a: $(ALL_OBJ) 84 | rm -f $(LIB).a 85 | $(AR) -rcs $(LIB).a $(ALL_OBJ) 86 | $(RANLIB) $(LIB).a 87 | 88 | # 89 | # ASPカーネルのビルドに関するその他のオプション 90 | # 91 | CLEAN_FILES += $(PROJECT).map $(APPLNAME).d 92 | DEPS += $(ALL_OBJ:.o=.d) 93 | -include $(DEPS) 94 | 95 | APPL_CFLAGS = 96 | APPL_LIBS = libmbed.a 97 | APPL_DIR = . 98 | 99 | INCLUDES += $(INCLUDE_PATHS) # MBEDライブラリのヘッダファイルをアプリで使う 100 | COPTS += -mfpu=vfpv3 -mfloat-abi=hard 101 | CDEFS += -DTOPPERS_OMIT_BSS_INIT -DTOPPERS_OMIT_DATA_INIT # ASPのスタートアップルーチンでは初期化処理は不要 102 | CDEFS += -DGRPEACH 103 | 104 | APPL_CXXFLAGS += $(CC_FLAGS) $(CC_SYMBOLS) -std=gnu++98 -fno-rtti 105 | CXXLIBS += $(LD_SYS_LIBS) 106 | LDFLAGS += $(LD_FLAGS) 107 | LDSCRIPT = $(LINKER_SCRIPT) 108 | -------------------------------------------------------------------------------- /mros_ws/Makefile.mros: -------------------------------------------------------------------------------- 1 | MROS_LIB = libmros 2 | 3 | ALL_MROS_OBJ = $(MROS_C_OBJ) $(MROS_CXX_OBJ) 4 | 5 | 6 | .PHONY: all clean 7 | 8 | all: genmsg $(MROS_LIB).a 9 | #clean: 10 | # rm -f $(MROS_LIB).lib $(ALL_MROS_OBJ) $(DEPS) 11 | 12 | $(MROS_CXX_OBJ): %.o: %.cpp 13 | $(CPP) -c $(CFLAGS) $(APPL_CXXFLAGS) $(MROS_INCLUDE_PATHS) -o $@ $< 14 | 15 | $(MROS_C_OBJ): %.o: %.c 16 | $(CC) -c $(CFLAGS) $(APPL_CFLAGS) $(MROS_INCLUDE_PATHS) -o $@ $< 17 | 18 | ifneq ($(USE_TRUESTUDIO),true) 19 | $(MROS_CXX_OBJ:.o=.s): %.s: %.cpp 20 | $(CPP) -S $(CFLAGS) $(APPL_CXXFLAGS) $(MROS_INCLUDE_PATHS) -o $@ $< 21 | endif 22 | 23 | $(MROS_LIB).a: $(ALL_MROS_OBJ) 24 | rm -f $(MROS_LIB).a 25 | $(AR) -rcs $(MROS_LIB).a $(ALL_MROS_OBJ) 26 | $(RANLIB) $(MROS_LIB).a 27 | 28 | CLEAN_FILES += $(MROS_DIR)/*.o 29 | DEPS += $(ALL_MROS_OBJ:.o=.d) 30 | 31 | APPL_LIBS = $(MROS_LIB).a libmbed.a 32 | 33 | INCLUDES += $(MROS_INCLUDE_PATHS) # MROSライブラリのヘッダファイルをアプリで使う 34 | INCLUDES += -I$(MROS_DIR)/../mros_ws/$(APPNAME)/include 35 | INCLUDES += -I$(MROS_DIR)/mros-msgs 36 | # INCLUDES += -I/opt/ros/kinetic/include -I/usr/include -I/usr/include/x86_64-linux-gnu -I/usr/include/i386-linux-gnu 37 | 38 | genmsg: $(MSG_SETTING_JSON) 39 | ifeq ($(GEN_MSG),true) 40 | $(PYTHON) $(MROS_DIR)/../mros_msg-gen/mros_msg-gen.py $(MSG_SETTING_JSON) 41 | echo "message generated" 42 | endif 43 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/.settings/org.eclipse.core.resources.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | encoding/Makefile=UTF-8 3 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/.settings/org.eclipse.core.runtime.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | line.separator=\r\n 3 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # ターゲットの指定（Makefile.targetで上書きされるのを防ぐため） 3 | # 4 | 5 | all: 6 | 7 | # 8 | # アプリケーションファイル 9 | # 10 | APPNAME = custom_pubsub 11 | APPLDIR = custom_pubsub mros_config mros_config/os/target/os_asp 12 | APPLNAME = app 13 | USE_CXX = true 14 | APPL_CFG = $(APPLNAME).cfg 15 | USE_TRUESTUDIO = true 16 | GEN_MSG = true 17 | MSG_SETTING_JSON = msg_app.json 18 | PYTHON = /usr/bin/python 19 | 20 | DEBUG = true 21 | OMIT_OPTIMIZATION = true 22 | 23 | # 24 | # アプリケーションプログラムに関する定義 25 | # メインファイル以外に追加するものを記述する 26 | # 27 | APPL_ASMOBJS = 28 | APPL_CXXOBJS = 29 | APPL_COBJS = mros_sys_config.o mros_usr_config.o mros_os_config.o 30 | 31 | 32 | # 33 | # mros.cpp のコンパイル時にエラーとなる不具合を解消するため 34 | # オプション無しでコンパイルできるように変更すべき 35 | # 36 | APPL_CXXFLAGS := $(APPL_CXXFLAGS) -fpermissive 37 | 38 | # 39 | # MBEDライブラリのディレクトリの定義 40 | # 41 | MBED_LIB_DIR = ../../asp_mbed/asp-gr_peach_gcc-mbed/mbed-lib 42 | # 43 | # mROSライブラリのディレクトリの定義 44 | # 45 | MROS_DIR = ../../mros-lib 46 | # 47 | # ASPソースファイルのディレクトリの定義 48 | # 49 | SRCDIR = ../../asp_mbed/asp-gr_peach_gcc-mbed/asp-1.9.2-utf8 50 | 51 | 52 | # 53 | # MBEDライブラリのビルド 54 | # 55 | include $(MBED_LIB_DIR)/common/Makefile.cmn 56 | include $(MBED_LIB_DIR)/mbed-src/Makefile.src 57 | include $(MBED_LIB_DIR)/SoftPWM/Makefile.pwm 58 | include $(MBED_LIB_DIR)/EthernetInterface/Makefile.eif 59 | include ../Makefile.mbd 60 | # 61 | # mROSライブラリのビルド 62 | # 63 | include $(MROS_DIR)/Makefile.m 64 | include ../Makefile.mros 65 | 66 | # 67 | # ASPカーネルライブラリ 68 | # 69 | KERNEL_LIB = . 70 | include ../Makefile.asp 71 | 72 | $(ALL_OBJ): kernel_cfg.h 73 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/app.cfg: -------------------------------------------------------------------------------- 1 | /* 2 | * mROSアプリケーションコンフィグファイル 3 | */ 4 | 5 | //mROSライブラリのコンフィグファイルをロード 6 | INCLUDE("../../mros-lib/mros.cfg"); 7 | 8 | //ROSAPIファイルのインクルード 9 | #include "mros_config/os/target/os_asp/mros_os_config.h" 10 | 11 | #include "app.h" 12 | CRE_TSK(USR_TASK1,{ TA_NULL,0,usr_task1,MROS_USR_TASK_PRI,MROS_USR1_STACK_SIZE,NULL }); 13 | CRE_TSK(USR_TASK2,{ TA_NULL,0,usr_task2,MROS_USR_TASK_PRI,MROS_USR2_STACK_SIZE,NULL }); 14 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/app.h: -------------------------------------------------------------------------------- 1 | //ROSのライブラリインクルードだけでmbedのライブラリとかもインクルードしたい？ 2 | //#include "../mros/ros.h" //rosのプログラムでは #include "ros/ros.h" 合わせたほうがいい？ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | void usr_task1(); 8 | void usr_task2(); 9 | #ifdef __cplusplus 10 | } 11 | #endif 12 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/custom_pubsub_debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/include/custom_pubsub/LEDValues.h: -------------------------------------------------------------------------------- 1 | #ifndef _CUSTOM_PUBSUB_LEDVALUES_H 2 | #define _CUSTOM_PUBSUB_LEDVALUES_H 3 | 4 | 5 | 6 | #include 7 | 8 | using namespace std; 9 | 10 | 11 | static const int LEDVALUES_MSG_ID = 101; 12 | 13 | namespace custom_pubsub{ 14 | class LEDValues{ 15 | public: 16 | float red; 17 | float green; 18 | float blue; 19 | 20 | int dataSize(){ 21 | return 4 + 4 + 4 + 4*0; 22 | } 23 | 24 | void memCopy(char*& addrPtr){ 25 | int size; 26 | 27 | memcpy(addrPtr,&red,4); 28 | addrPtr += 4; 29 | 30 | memcpy(addrPtr,&green,4); 31 | addrPtr += 4; 32 | 33 | memcpy(addrPtr,&blue,4); 34 | addrPtr += 4; 35 | 36 | } 37 | 38 | void deserialize(char*& rbuf){ 39 | uint32_t size; 40 | memcpy(&red,rbuf,4); 41 | rbuf += 4; 42 | 43 | memcpy(&green,rbuf,4); 44 | rbuf += 4; 45 | 46 | memcpy(&blue,rbuf,4); 47 | rbuf += 4; 48 | 49 | 50 | } 51 | }; 52 | 53 | } 54 | 55 | namespace message_traits 56 | { 57 | template<> 58 | struct MD5Sum 59 | { 60 | static const char* value() 61 | { 62 | return "fc84fca2ee69069d6d5c4147f9b2e33a"; 63 | } 64 | 65 | }; 66 | 67 | template<> 68 | struct DataType 69 | { 70 | static const char* value() 71 | { 72 | return "custom_pubsub/LEDValues"; 73 | } 74 | 75 | }; 76 | 77 | template<> 78 | struct DataTypeId 79 | { 80 | static int value() 81 | { 82 | return LEDVALUES_MSG_ID; 83 | } 84 | 85 | }; 86 | 87 | template<> 88 | struct Definition 89 | { 90 | static const char* value() 91 | { 92 | return "float32 red\n\ 93 | float32 green\n\ 94 | float32 blue\n\ 95 | "; 96 | } 97 | }; 98 | } 99 | 100 | namespace subtask_methods 101 | { 102 | template<> 103 | struct CallCallbackFuncs{ 104 | static void call(void (*fp)(void *), char *rbuf) 105 | { 106 | custom_pubsub::LEDValues msg; 107 | rbuf += 4; 108 | msg.deserialize(rbuf); 109 | fp(&msg); 110 | } 111 | }; 112 | } 113 | 114 | #endif -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/include/custom_pubsub/PersonName.h: -------------------------------------------------------------------------------- 1 | #ifndef _CUSTOM_PUBSUB_PERSONNAME_H 2 | #define _CUSTOM_PUBSUB_PERSONNAME_H 3 | 4 | 5 | 6 | #include 7 | 8 | using namespace std; 9 | 10 | 11 | static const int PERSONNAME_MSG_ID = 102; 12 | 13 | namespace custom_pubsub{ 14 | class PersonName{ 15 | public: 16 | string firstName; 17 | string lastName; 18 | 19 | int dataSize(){ 20 | return firstName.size() + lastName.size() + 4*2; 21 | } 22 | 23 | void memCopy(char*& addrPtr){ 24 | int size; 25 | 26 | size = firstName.size(); 27 | memcpy(addrPtr,&size,4); 28 | addrPtr += 4; 29 | memcpy(addrPtr, firstName.c_str(),size); 30 | addrPtr += size; 31 | 32 | size = lastName.size(); 33 | memcpy(addrPtr,&size,4); 34 | addrPtr += 4; 35 | memcpy(addrPtr, lastName.c_str(),size); 36 | addrPtr += size; 37 | 38 | } 39 | 40 | void deserialize(char*& rbuf){ 41 | uint32_t size; 42 | { 43 | memcpy(&size,rbuf,4); 44 | rbuf += 4; 45 | char buf_char[size+1]; 46 | memcpy(&buf_char,rbuf,size); 47 | buf_char[size] = '\0'; 48 | firstName = buf_char; 49 | rbuf += size; 50 | } 51 | 52 | { 53 | memcpy(&size,rbuf,4); 54 | rbuf += 4; 55 | char buf_char[size+1]; 56 | memcpy(&buf_char,rbuf,size); 57 | buf_char[size] = '\0'; 58 | lastName = buf_char; 59 | rbuf += size; 60 | } 61 | 62 | 63 | } 64 | }; 65 | 66 | } 67 | 68 | namespace message_traits 69 | { 70 | template<> 71 | struct MD5Sum 72 | { 73 | static const char* value() 74 | { 75 | return "2603aab1ad50ac2b8ea55f4de7eec3a0"; 76 | } 77 | 78 | }; 79 | 80 | template<> 81 | struct DataType 82 | { 83 | static const char* value() 84 | { 85 | return "custom_pubsub/PersonName"; 86 | } 87 | 88 | }; 89 | 90 | template<> 91 | struct DataTypeId 92 | { 93 | static int value() 94 | { 95 | return PERSONNAME_MSG_ID; 96 | } 97 | 98 | }; 99 | 100 | template<> 101 | struct Definition 102 | { 103 | static const char* value() 104 | { 105 | return "string firstName\n\ 106 | string lastName\n\ 107 | "; 108 | } 109 | }; 110 | } 111 | 112 | namespace subtask_methods 113 | { 114 | template<> 115 | struct CallCallbackFuncs{ 116 | static void call(void (*fp)(void *), char *rbuf) 117 | { 118 | custom_pubsub::PersonName msg; 119 | rbuf += 4; 120 | msg.deserialize(rbuf); 121 | fp(&msg); 122 | } 123 | }; 124 | } 125 | 126 | #endif -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/include/custom_pubsub/UserTypeTest.h: -------------------------------------------------------------------------------- 1 | #ifndef _CUSTOM_PUBSUB_USERTYPETEST_H 2 | #define _CUSTOM_PUBSUB_USERTYPETEST_H 3 | 4 | #include "custom_pubsub/LEDValues.h" 5 | #include "custom_pubsub/PersonName.h" 6 | 7 | 8 | #include 9 | 10 | using namespace std; 11 | 12 | 13 | static const int USERTYPETEST_MSG_ID = 103; 14 | 15 | namespace custom_pubsub{ 16 | class UserTypeTest{ 17 | public: 18 | custom_pubsub::LEDValues ledVal; 19 | custom_pubsub::PersonName nameVal; 20 | 21 | int dataSize(){ 22 | return ledVal.dataSize() + nameVal.dataSize() + 4*2; 23 | } 24 | 25 | void memCopy(char*& addrPtr){ 26 | int size; 27 | 28 | ledVal.memCopy(addrPtr); 29 | 30 | nameVal.memCopy(addrPtr); 31 | 32 | } 33 | 34 | void deserialize(char*& rbuf){ 35 | uint32_t size; 36 | 37 | ledVal.deserialize(rbuf); 38 | 39 | 40 | nameVal.deserialize(rbuf); 41 | 42 | 43 | } 44 | }; 45 | 46 | } 47 | 48 | namespace message_traits 49 | { 50 | template<> 51 | struct MD5Sum 52 | { 53 | static const char* value() 54 | { 55 | return "4c13c13aa2f193d25835cfad6215cb75"; 56 | } 57 | 58 | }; 59 | 60 | template<> 61 | struct DataType 62 | { 63 | static const char* value() 64 | { 65 | return "custom_pubsub/UserTypeTest"; 66 | } 67 | 68 | }; 69 | 70 | template<> 71 | struct DataTypeId 72 | { 73 | static int value() 74 | { 75 | return USERTYPETEST_MSG_ID; 76 | } 77 | 78 | }; 79 | 80 | template<> 81 | struct Definition 82 | { 83 | static const char* value() 84 | { 85 | return "LEDValues ledVal\n\ 86 | PersonName nameVal\n\ 87 | "; 88 | } 89 | }; 90 | } 91 | 92 | namespace subtask_methods 93 | { 94 | template<> 95 | struct CallCallbackFuncs{ 96 | static void call(void (*fp)(void *), char *rbuf) 97 | { 98 | custom_pubsub::UserTypeTest msg; 99 | rbuf += 4; 100 | msg.deserialize(rbuf); 101 | fp(&msg); 102 | } 103 | }; 104 | } 105 | 106 | #endif -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/include/message_class_specialization.h: -------------------------------------------------------------------------------- 1 | #include "ros.h" 2 | 3 | 4 | #include "custom_pubsub/UserTypeTest.h" 5 | 6 | 7 | 8 | template ros::Subscriber ros::NodeHandle::subscribe(std::string,int,void (*fp)(custom_pubsub::UserTypeTest*)); 9 | template ros::Publisher ros::NodeHandle::advertise(std::string, int); 10 | template void ros::Publisher::publish(custom_pubsub::UserTypeTest&); -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/include/message_headers.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "custom_pubsub/UserTypeTest.h" 4 | 5 | 6 | static void callCallback(int id, void (*fp)(void *), char *rbuf){ 7 | switch(id){ 8 | 9 | case 103: 10 | subtask_methods::CallCallbackFuncs<103>().call(fp,rbuf); 11 | break; 12 | 13 | } 14 | } 15 | 16 | static const char* getMD5Sum(int id){ 17 | switch(id){ 18 | 19 | case 103: 20 | return message_traits::MD5Sum<103>().value(); 21 | 22 | } 23 | return NULL; 24 | } -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/include/msg_max_size.h: -------------------------------------------------------------------------------- 1 | #define PUB_MSG_MAX_SIZE 12 -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/mros_config/mros_sys_config.c: -------------------------------------------------------------------------------- 1 | #include "mros_types.h" 2 | #include "mros_memory.h" 3 | #include "mros_sys_config.h" 4 | #include "kernel.h" 5 | 6 | void mros_sys_config_init(void) 7 | { 8 | mRosReturnType ret; 9 | 10 | ret = mros_mem_init(ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM, ros_inner_topic_publisher_config, &ros_inner_topic_publisher_mempool); 11 | if (ret != MROS_E_OK) { 12 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 13 | return; 14 | } 15 | ret = mros_mem_init(ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM, ros_outer_topic_publisher_config, &ros_outer_topic_publisher_mempool); 16 | if (ret != MROS_E_OK) { 17 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 18 | return; 19 | } 20 | return; 21 | } 22 | 23 | 24 | void usr_task_activation(void) 25 | { 26 | mros_uint32 i; 27 | for (i = 0; i < MROS_USR_TASK_NUM; i++) { 28 | act_tsk(mros_usr_task_table[i]); 29 | } 30 | } 31 | 32 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/mros_config/mros_sys_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_SYS_CONFIG_H_ 2 | #define _MROS_SYS_CONFIG_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_usr_config.h" 9 | #include "mros_os_config.h" 10 | #include "mros_packet_config.h" 11 | 12 | 13 | 14 | /************************************** 15 | * PROTOCOL 16 | **************************************/ 17 | /* 18 | * portno of master 19 | */ 20 | #define MROS_MASTER_PORT_NO 11311 21 | /* 22 | * dhcp option of self node(0:not use, 1:use) 23 | */ 24 | #define MROS_NODE_USE_DHCP 0 25 | /* 26 | * ipaddr of master 27 | */ 28 | #define MROS_MASTER_IPADDR "0.0.0.0" 29 | 30 | /* 31 | * ipaddr of self node 32 | */ 33 | #define MROS_NODE_IPADDR "127.0.0.1" 34 | 35 | /* 36 | * subnet mask of self node 37 | */ 38 | #define MROS_NODE_SUBNET_MASK "255.255.255.0" 39 | 40 | /* 41 | * portno of slave 42 | */ 43 | #define MROS_SLAVE_PORT_NO 11411 44 | 45 | /* 46 | * portno of pub 47 | */ 48 | #define MROS_PUBLISHER_PORT_NO 11511 49 | 50 | /* 51 | * do not change this parameter 52 | */ 53 | #define MROS_TOPIC_TCP_CLIENT_MAX_NUM ( MROS_PUB_TOPIC_CONNECTOR_MAX_NUM + MROS_SUB_TOPIC_CONNECTOR_MAX_NUM ) 54 | 55 | 56 | /***************************************** 57 | * EXCLUSIVE AREA 58 | *****************************************/ 59 | 60 | /* 61 | * do not change this parameter 62 | */ 63 | #define MROS_GIANT_EXCLUSIVE_AREA_PRIORITY ( \ 64 | ( MROS_USR_TASK_PRI < MROS_TASK_PRI) ? \ 65 | MROS_USR_TASK_PRI : \ 66 | MROS_TASK_PRI \ 67 | ) 68 | 69 | 70 | extern void mros_sys_config_init(void); 71 | extern void usr_task_activation(void); 72 | 73 | #ifdef __cplusplus 74 | } 75 | #endif 76 | 77 | #endif /* _MROS_SYS_CONFIG_H_ */ 78 | 79 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/mros_config/mros_usr_config.c: -------------------------------------------------------------------------------- 1 | #include "mros_types.h" 2 | #include "mros_memory.h" 3 | #include "mros_usr_config.h" 4 | #include "kernel_cfg.h" 5 | 6 | /******************************************************* 7 | * START: Inner Publish topic data memory Config 8 | *******************************************************/ 9 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool1, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL1_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL1_SIZE); 10 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool2, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL2_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL2_SIZE); 11 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool3, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL3_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL3_SIZE); 12 | mRosMemoryConfigType *ros_inner_topic_publisher_config[ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM] = { 13 | &ros_inner_topic_publisher_mempool1_config, 14 | &ros_inner_topic_publisher_mempool2_config, 15 | &ros_inner_topic_publisher_mempool3_config, 16 | }; 17 | MROS_MEMORY_CONFIG_DECLARE_MANAGER(ros_inner_topic_publisher_mempool, ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM); 18 | /******************************************************* 19 | * END 20 | *******************************************************/ 21 | 22 | 23 | /******************************************************* 24 | * START: Outer Publish topic data memory Config 25 | *******************************************************/ 26 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool1, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL1_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL1_SIZE); 27 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool2, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL2_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL2_SIZE); 28 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool3, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL3_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL3_SIZE); 29 | mRosMemoryConfigType *ros_outer_topic_publisher_config[ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM] = { 30 | &ros_outer_topic_publisher_mempool1_config, 31 | &ros_outer_topic_publisher_mempool2_config, 32 | &ros_outer_topic_publisher_mempool3_config, 33 | }; 34 | MROS_MEMORY_CONFIG_DECLARE_MANAGER(ros_outer_topic_publisher_mempool, ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM); 35 | /******************************************************* 36 | * END 37 | *******************************************************/ 38 | 39 | 40 | /**************************************** 41 | * USR OS TASK 42 | ****************************************/ 43 | 44 | mRosTaskIdType mros_usr_task_table[MROS_USR_TASK_NUM] = { 45 | USR_TASK1, 46 | USR_TASK2, 47 | }; 48 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/mros_config/os/target/os_asp/mros_os_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_OS_CONFIG_H_ 2 | #define _MROS_OS_CONFIG_H_ 3 | 4 | #include "kernel.h" 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | extern void main_task(void); 11 | extern void sub_task(void); 12 | extern void pub_task(void); 13 | extern void xml_slv_task(void); 14 | extern void xml_mas_task(void); 15 | extern void cyclic_handler(intptr_t exinf); 16 | 17 | /**************************************** 18 | * kernel cfg 19 | ****************************************/ 20 | #ifndef MROS_USR_TASK_PRI 21 | /* 22 | * main task priority 23 | * 24 | * do not change this parameter 25 | */ 26 | #define MAIN_TASK_PRI 7 27 | /* 28 | * mROS task priority 29 | * 30 | * do not change this parameter 31 | */ 32 | #define MROS_TASK_PRI 6 33 | 34 | /* 35 | * user task priority 36 | * 37 | * following config parameter is an example. 38 | */ 39 | #define MROS_USR_TASK1_PRI 9 40 | #define MROS_USR_TASK2_PRI 9 41 | /*8 42 | * user task max priority8 43 | *8 44 | * please set max priority of user tasks. 45 | */ 46 | #define MROS_USR_TASK_PRI 8 47 | #endif /* ROS_USR_TASK_PRI */ 48 | 49 | #ifndef TASK_PORTID 50 | #define TASK_PORTID 1 /* serial port ID for something typing */ 51 | #endif /* TASK_PORTID */ 52 | 53 | #ifndef MROS_TASK_STACK_SIZE 54 | /* 55 | * user task stack size 56 | */ 57 | #define MROS_USR9_STACK_SIZE 1024 * 1 //for startup task 58 | #define MROS_USR8_STACK_SIZE 1024 * 1 //for user task8 59 | #define MROS_USR7_STACK_SIZE 1024 * 1 //for user task7 60 | #define MROS_USR6_STACK_SIZE 1024 * 1 //for user task6 61 | #define MROS_USR5_STACK_SIZE 1024 * 1 //for user task5 62 | #define MROS_USR4_STACK_SIZE 1024 * 1 //for user task4 63 | #define MROS_USR3_STACK_SIZE 1024 * 1 //for user task3 64 | #define MROS_USR2_STACK_SIZE 1024 * 1 //for user task2 65 | #define MROS_USR1_STACK_SIZE 1024 * 1 //for user task1 66 | /* 67 | * mROS task stack size 68 | * 69 | * do not change this parameter 70 | */ 71 | #define MROS_TASK_STACK_SIZE 1024 * 2 //for mros task 72 | #endif /*MROS_TASK_STACK_SIZE*/ 73 | 74 | #ifndef KMM_SIZE 75 | #define MBED_TASK_STACK_SIZE 1024 * 8 //for mbed task 76 | #define KMM_SIZE (MBED_TASK_STACK_SIZE * 16) /* kernel assign */ 77 | #endif /* KMM_SIZE */ /* size of memory */ 78 | 79 | /* 80 | * do not change this parameter 81 | */ 82 | #ifndef CYC 83 | #define MROS_LOOP_RATE 100 84 | #define CYC 85 | #endif /*CYC*/ 86 | 87 | /* 88 | * do not change this parameter 89 | */ 90 | #ifndef LOOP_REF 91 | #define LOOP_REF ULONG_C(1000000) /* number of loops to evaluate speed */ 92 | #endif /* LOOP_REF */ 93 | 94 | #ifdef __cplusplus 95 | } 96 | #endif 97 | 98 | 99 | #endif /* _MROS_OS_CONFIG_H_ */ 100 | -------------------------------------------------------------------------------- /mros_ws/custom_pubsub/msg_app.json: -------------------------------------------------------------------------------- 1 | { 2 | "catkin_ws_dir": "../../ros_catkin_ws/", 3 | "ros_include_dir": "/opt/ros/kinetic/include", 4 | "including_msgs": [ 5 | "custom_pubsub/UserTypeTest.h" 6 | ], 7 | "depending_packages": [ 8 | "std_msgs", 9 | "custom_pubsub" 10 | ], 11 | "depending_build_in_packages": [ 12 | ] 13 | } 14 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/.settings/org.eclipse.core.resources.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | encoding/Makefile=UTF-8 3 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/.settings/org.eclipse.core.runtime.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | line.separator=\r\n 3 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # ターゲットの指定（Makefile.targetで上書きされるのを防ぐため） 3 | # 4 | 5 | all: 6 | 7 | # 8 | # アプリケーションファイル 9 | # 10 | APPNAME = image_publisher 11 | APPLDIR = image_publisher mros_config mros_config/os/target/os_asp 12 | APPLNAME = app 13 | USE_CXX = true 14 | APPL_CFG = $(APPLNAME).cfg 15 | USE_TRUESTUDIO = false 16 | GEN_MSG = true 17 | MSG_SETTING_JSON = camera_app.json 18 | PYTHON = /usr/bin/python 19 | 20 | DEBUG = true 21 | OMIT_OPTIMIZATION = true 22 | 23 | # 24 | # アプリケーションプログラムに関する定義 25 | # メインファイル以外に追加するものを記述する 26 | # 27 | APPL_ASMOBJS = 28 | APPL_CXXOBJS = 29 | APPL_COBJS = mros_sys_config.o mros_usr_config.o mros_os_config.o 30 | 31 | 32 | # 33 | # mros.cpp のコンパイル時にエラーとなる不具合を解消するため 34 | # オプション無しでコンパイルできるように変更すべき 35 | # 36 | APPL_CXXFLAGS := $(APPL_CXXFLAGS) -fpermissive 37 | 38 | # 39 | # MBEDライブラリのディレクトリの定義 40 | # 41 | MBED_LIB_DIR = ../../asp_mbed/asp-gr_peach_gcc-mbed/mbed-lib 42 | # 43 | # mROSライブラリのディレクトリの定義 44 | # 45 | MROS_DIR = ../../mros-lib 46 | # 47 | # opencvライブラリのディレクトリ定義 48 | # 49 | OPENCV_DIR = ../../opencv-lib 50 | # 51 | # ASPソースファイルのディレクトリの定義 52 | # 53 | SRCDIR = ../../asp_mbed/asp-gr_peach_gcc-mbed/asp-1.9.2-utf8 54 | 55 | 56 | # 57 | # MBEDライブラリのビルド 58 | # 59 | include $(MBED_LIB_DIR)/common/Makefile.cmn 60 | include $(MBED_LIB_DIR)/mbed-src/Makefile.src 61 | include $(MBED_LIB_DIR)/SoftPWM/Makefile.pwm 62 | include $(MBED_LIB_DIR)/EthernetInterface/Makefile.eif 63 | include $(MBED_LIB_DIR)/GR-PEACH_video/Makefile.gpv 64 | include $(MBED_LIB_DIR)/GraphicsFramework/Makefile.gfw 65 | include ../Makefile.mbd 66 | # 67 | # mROSライブラリのビルド 68 | # 69 | include $(MROS_DIR)/Makefile.m 70 | include ../Makefile.mros 71 | 72 | # 73 | # OpenCVライブラリのビルド 74 | # 75 | include ../Makefile.cv 76 | 77 | # 78 | # ASPカーネルライブラリ 79 | # 80 | KERNEL_LIB = . 81 | include ../Makefile.asp 82 | 83 | $(ALL_OBJ): kernel_cfg.h 84 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/app.cfg: -------------------------------------------------------------------------------- 1 | /* 2 | * mROSアプリケーションコンフィグファイル 3 | */ 4 | 5 | //mROSライブラリのコンフィグファイルをロード 6 | INCLUDE("../../mros-lib/mros.cfg"); 7 | 8 | //mbedのインクルード 9 | INCLUDE("../../mbed-lib/GraphicsFramework/GraphicsFramework.cfg"); 10 | 11 | //ROSAPIファイルのインクルード 12 | #include "mros_config/os/target/os_asp/mros_os_config.h" 13 | 14 | #include "app.h" 15 | CRE_TSK(USR_TASK1,{ TA_NULL,0,usr_task1,MROS_USR_TASK_PRI,MROS_USR1_STACK_SIZE,NULL }); 16 | CRE_TSK(USR_TASK2,{ TA_NULL,0,usr_task2,MROS_USR_TASK_PRI,MROS_USR2_STACK_SIZE,NULL }); 17 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/app.h: -------------------------------------------------------------------------------- 1 | //ROSのライブラリインクルードだけでmbedのライブラリとかもインクルードしたい？ 2 | //#include "../mros/ros.h" //rosのプログラムでは #include "ros/ros.h" 合わせたほうがいい？ 3 | #define FREQ (20) 4 | 5 | #ifdef __cplusplus 6 | extern "C" { 7 | #endif 8 | void usr_task1(); 9 | void usr_task2(); 10 | #ifdef __cplusplus 11 | } 12 | #endif 13 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/camera_app.json: -------------------------------------------------------------------------------- 1 | { 2 | "catkin_ws_dir": "../../ros_catkin_ws/", 3 | "ros_include_dir": "/opt/ros/kinetic/include", 4 | "including_msgs": [ 5 | "sensor_msgs/Image.h" 6 | ], 7 | "depending_packages": [ 8 | "std_msgs" 9 | ], 10 | "depending_build_in_packages": [ 11 | ] 12 | } 13 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/image_publisher_debug.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/include/message_class_specialization.h: -------------------------------------------------------------------------------- 1 | #include "ros.h" 2 | 3 | #include "sensor_msgs/Image.h" 4 | 5 | 6 | 7 | template ros::Subscriber ros::NodeHandle::subscribe(std::string,int,void (*fp)(sensor_msgs::Image*)); 8 | template ros::Publisher ros::NodeHandle::advertise(std::string, int); 9 | template void ros::Publisher::publish(sensor_msgs::Image&); 10 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/include/message_headers.h: -------------------------------------------------------------------------------- 1 | 2 | #include "sensor_msgs/Image.h" 3 | 4 | 5 | 6 | static void callCallback(int id, void (*fp)(void *), char *rbuf){ 7 | switch(id){ 8 | case 20: 9 | subtask_methods::CallCallbackFuncs<20>().call(fp,rbuf); 10 | break; 11 | 12 | 13 | } 14 | } 15 | 16 | static const char* getMD5Sum(int id){ 17 | switch(id){ 18 | case 20: 19 | return message_traits::MD5Sum<20>().value(); 20 | 21 | 22 | } 23 | return NULL; 24 | } -------------------------------------------------------------------------------- /mros_ws/image_publisher/include/msg_max_size.h: -------------------------------------------------------------------------------- 1 | #define PUB_MSG_MAX_SIZE 0 -------------------------------------------------------------------------------- /mros_ws/image_publisher/mros_config/mros_sys_config.c: -------------------------------------------------------------------------------- 1 | #include "mros_types.h" 2 | #include "mros_memory.h" 3 | #include "mros_sys_config.h" 4 | #include "kernel.h" 5 | 6 | void mros_sys_config_init(void) 7 | { 8 | mRosReturnType ret; 9 | 10 | ret = mros_mem_init(ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM, ros_inner_topic_publisher_config, &ros_inner_topic_publisher_mempool); 11 | if (ret != MROS_E_OK) { 12 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 13 | return; 14 | } 15 | ret = mros_mem_init(ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM, ros_outer_topic_publisher_config, &ros_outer_topic_publisher_mempool); 16 | if (ret != MROS_E_OK) { 17 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 18 | return; 19 | } 20 | return; 21 | } 22 | 23 | 24 | void usr_task_activation(void) 25 | { 26 | mros_uint32 i; 27 | for (i = 0; i < MROS_USR_TASK_NUM; i++) { 28 | act_tsk(mros_usr_task_table[i]); 29 | } 30 | } 31 | 32 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/mros_config/mros_sys_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_SYS_CONFIG_H_ 2 | #define _MROS_SYS_CONFIG_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_usr_config.h" 9 | #include "mros_os_config.h" 10 | #include "mros_packet_config.h" 11 | 12 | 13 | 14 | /************************************** 15 | * PROTOCOL 16 | **************************************/ 17 | /* 18 | * portno of master 19 | */ 20 | #define MROS_MASTER_PORT_NO 11311 21 | /* 22 | * dhcp option of self node(0:not use, 1:use) 23 | */ 24 | #define MROS_NODE_USE_DHCP 0 25 | /* 26 | * ipaddr of master 27 | */ 28 | #define MROS_MASTER_IPADDR "0.0.0.0" 29 | 30 | /* 31 | * ipaddr of self node 32 | */ 33 | #define MROS_NODE_IPADDR "127.0.0.1" 34 | 35 | /* 36 | * subnet mask of self node 37 | */ 38 | #define MROS_NODE_SUBNET_MASK "255.255.255.0" 39 | 40 | /* 41 | * portno of slave 42 | */ 43 | #define MROS_SLAVE_PORT_NO 11411 44 | 45 | /* 46 | * portno of pub 47 | */ 48 | #define MROS_PUBLISHER_PORT_NO 11511 49 | 50 | /* 51 | * do not change this parameter 52 | */ 53 | #define MROS_TOPIC_TCP_CLIENT_MAX_NUM ( MROS_PUB_TOPIC_CONNECTOR_MAX_NUM + MROS_SUB_TOPIC_CONNECTOR_MAX_NUM ) 54 | 55 | 56 | /***************************************** 57 | * EXCLUSIVE AREA 58 | *****************************************/ 59 | 60 | /* 61 | * do not change this parameter 62 | */ 63 | #define MROS_GIANT_EXCLUSIVE_AREA_PRIORITY ( \ 64 | ( MROS_USR_TASK_PRI < MROS_TASK_PRI) ? \ 65 | MROS_USR_TASK_PRI : \ 66 | MROS_TASK_PRI \ 67 | ) 68 | 69 | 70 | extern void mros_sys_config_init(void); 71 | extern void usr_task_activation(void); 72 | 73 | #ifdef __cplusplus 74 | } 75 | #endif 76 | 77 | #endif /* _MROS_SYS_CONFIG_H_ */ 78 | 79 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/mros_config/mros_usr_config.c: -------------------------------------------------------------------------------- 1 | #include "mros_types.h" 2 | #include "mros_memory.h" 3 | #include "mros_usr_config.h" 4 | #include "kernel_cfg.h" 5 | 6 | /******************************************************* 7 | * START: Inner Publish topic data memory Config 8 | *******************************************************/ 9 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool1, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL1_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL1_SIZE); 10 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool2, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL2_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL2_SIZE); 11 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool3, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL3_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL3_SIZE); 12 | mRosMemoryConfigType *ros_inner_topic_publisher_config[ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM] = { 13 | &ros_inner_topic_publisher_mempool1_config, 14 | &ros_inner_topic_publisher_mempool2_config, 15 | &ros_inner_topic_publisher_mempool3_config, 16 | }; 17 | MROS_MEMORY_CONFIG_DECLARE_MANAGER(ros_inner_topic_publisher_mempool, ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM); 18 | /******************************************************* 19 | * END 20 | *******************************************************/ 21 | 22 | 23 | /******************************************************* 24 | * START: Outer Publish topic data memory Config 25 | *******************************************************/ 26 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool1, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL1_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL1_SIZE); 27 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool2, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL2_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL2_SIZE); 28 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool3, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL3_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL3_SIZE); 29 | mRosMemoryConfigType *ros_outer_topic_publisher_config[ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM] = { 30 | &ros_outer_topic_publisher_mempool1_config, 31 | &ros_outer_topic_publisher_mempool2_config, 32 | &ros_outer_topic_publisher_mempool3_config, 33 | }; 34 | MROS_MEMORY_CONFIG_DECLARE_MANAGER(ros_outer_topic_publisher_mempool, ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM); 35 | /******************************************************* 36 | * END 37 | *******************************************************/ 38 | 39 | 40 | /**************************************** 41 | * USR OS TASK 42 | ****************************************/ 43 | 44 | mRosTaskIdType mros_usr_task_table[MROS_USR_TASK_NUM] = { 45 | USR_TASK1, 46 | USR_TASK2, 47 | }; 48 | -------------------------------------------------------------------------------- /mros_ws/image_publisher/mros_config/os/target/os_asp/mros_os_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_OS_CONFIG_H_ 2 | #define _MROS_OS_CONFIG_H_ 3 | 4 | #include "kernel.h" 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | extern void main_task(void); 11 | extern void sub_task(void); 12 | extern void pub_task(void); 13 | extern void xml_slv_task(void); 14 | extern void xml_mas_task(void); 15 | extern void cyclic_handler(intptr_t exinf); 16 | 17 | /**************************************** 18 | * kernel cfg 19 | ****************************************/ 20 | #ifndef MROS_USR_TASK_PRI 21 | /* 22 | * main task priority 23 | * 24 | * do not change this parameter 25 | */ 26 | #define MAIN_TASK_PRI 7 27 | /* 28 | * mROS task priority 29 | * 30 | * do not change this parameter 31 | */ 32 | #define MROS_TASK_PRI 6 33 | 34 | /* 35 | * user task priority 36 | * 37 | * following config parameter is an example. 38 | */ 39 | #define MROS_USR_TASK1_PRI 9 40 | #define MROS_USR_TASK2_PRI 9 41 | /*8 42 | * user task max priority8 43 | *8 44 | * please set max priority of user tasks. 45 | */ 46 | #define MROS_USR_TASK_PRI 8 47 | #endif /* ROS_USR_TASK_PRI */ 48 | 49 | #ifndef TASK_PORTID 50 | #define TASK_PORTID 1 /* serial port ID for something typing */ 51 | #endif /* TASK_PORTID */ 52 | 53 | #ifndef MROS_TASK_STACK_SIZE 54 | /* 55 | * user task stack size 56 | */ 57 | #define MROS_USR9_STACK_SIZE 1024 * 1 //for startup task 58 | #define MROS_USR8_STACK_SIZE 1024 * 1 //for user task8 59 | #define MROS_USR7_STACK_SIZE 1024 * 1 //for user task7 60 | #define MROS_USR6_STACK_SIZE 1024 * 1 //for user task6 61 | #define MROS_USR5_STACK_SIZE 1024 * 1 //for user task5 62 | #define MROS_USR4_STACK_SIZE 1024 * 1 //for user task4 63 | #define MROS_USR3_STACK_SIZE 1024 * 1 //for user task3 64 | #define MROS_USR2_STACK_SIZE 1024 * 1 //for user task2 65 | #define MROS_USR1_STACK_SIZE 1024 * 1 //for user task1 66 | /* 67 | * mROS task stack size 68 | * 69 | * do not change this parameter 70 | */ 71 | #define MROS_TASK_STACK_SIZE 1024 * 2 //for mros task 72 | #endif /*MROS_TASK_STACK_SIZE*/ 73 | 74 | #ifndef KMM_SIZE 75 | #define MBED_TASK_STACK_SIZE 1024 * 8 //for mbed task 76 | #define KMM_SIZE (MBED_TASK_STACK_SIZE * 16) /* kernel assign */ 77 | #endif /* KMM_SIZE */ /* size of memory */ 78 | 79 | /* 80 | * do not change this parameter 81 | */ 82 | #ifndef CYC 83 | #define MROS_LOOP_RATE 100 84 | #define CYC 85 | #endif /*CYC*/ 86 | 87 | /* 88 | * do not change this parameter 89 | */ 90 | #ifndef LOOP_REF 91 | #define LOOP_REF ULONG_C(1000000) /* number of loops to evaluate speed */ 92 | #endif /* LOOP_REF */ 93 | 94 | #ifdef __cplusplus 95 | } 96 | #endif 97 | 98 | 99 | #endif /* _MROS_OS_CONFIG_H_ */ 100 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/.settings/com.atollic.truestudio.debug.hardware_device.prefs: -------------------------------------------------------------------------------- 1 | BOARD=None 2 | CODE_LOCATION=RAM 3 | ENDIAN=Little-endian 4 | MCU=R7S721001 5 | MCU_VENDOR=Renesas 6 | MODEL=Lite 7 | PROJECT_FORMAT_VERSION=2 8 | TARGET=ARM\u00AE 9 | VERSION=7.1.2 10 | eclipse.preferences.version=1 11 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/.settings/org.eclipse.core.resources.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | encoding/Makefile=UTF-8 3 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/.settings/org.eclipse.core.runtime.prefs: -------------------------------------------------------------------------------- 1 | eclipse.preferences.version=1 2 | line.separator=\r\n 3 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # ターゲットの指定（Makefile.targetで上書きされるのを防ぐため） 3 | # 4 | 5 | all: 6 | 7 | # 8 | # アプリケーションファイル 9 | # 10 | APPNAME = string_pubsub 11 | APPLDIR = string_pubsub mros_config mros_config/os/target/os_asp 12 | APPLNAME = app 13 | USE_CXX = true 14 | APPL_CFG = $(APPLNAME).cfg 15 | USE_TRUESTUDIO = false 16 | GEN_MSG = false 17 | MSG_SETTING_JSON = app.json 18 | PYTHON = /usr/bin/python 19 | 20 | DEBUG = true 21 | OMIT_OPTIMIZATION = true 22 | 23 | # 24 | # アプリケーションプログラムに関する定義 25 | # メインファイル以外に追加するものを記述する 26 | # 27 | APPL_ASMOBJS = 28 | APPL_CXXOBJS = 29 | APPL_COBJS = mros_sys_config.o mros_usr_config.o mros_os_config.o 30 | 31 | 32 | # 33 | # mros.cpp のコンパイル時にエラーとなる不具合を解消するため 34 | # オプション無しでコンパイルできるように変更すべき 35 | # 36 | APPL_CXXFLAGS := $(APPL_CXXFLAGS) -fpermissive 37 | 38 | # 39 | # MBEDライブラリのディレクトリの定義 40 | # 41 | MBED_LIB_DIR = ../../asp_mbed/asp-gr_peach_gcc-mbed/mbed-lib 42 | # 43 | # mROSライブラリのディレクトリの定義 44 | # 45 | MROS_DIR = ../../mros-lib 46 | # 47 | # ASPソースファイルのディレクトリの定義 48 | # 49 | SRCDIR = ../../asp_mbed/asp-gr_peach_gcc-mbed/asp-1.9.2-utf8 50 | 51 | 52 | # 53 | # MBEDライブラリのビルド 54 | # 55 | include $(MBED_LIB_DIR)/common/Makefile.cmn 56 | include $(MBED_LIB_DIR)/mbed-src/Makefile.src 57 | include $(MBED_LIB_DIR)/SoftPWM/Makefile.pwm 58 | include $(MBED_LIB_DIR)/EthernetInterface/Makefile.eif 59 | include ../Makefile.mbd 60 | # 61 | # mROSライブラリのビルド 62 | # 63 | include $(MROS_DIR)/Makefile.m 64 | include ../Makefile.mros 65 | 66 | # 67 | # ASPカーネルライブラリ 68 | # 69 | KERNEL_LIB = . 70 | include ../Makefile.asp 71 | 72 | $(ALL_OBJ): kernel_cfg.h 73 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/app.cfg: -------------------------------------------------------------------------------- 1 | /* 2 | * mROSアプリケーションコンフィグファイル 3 | */ 4 | 5 | //mROSライブラリのコンフィグファイルをロード 6 | INCLUDE("../../mros-lib/mros.cfg"); 7 | 8 | //ROSAPIファイルのインクルード 9 | //#include "../../mros-lib/mros.h" 10 | #include "mros_config/os/target/os_asp/mros_os_config.h" 11 | 12 | #include "app.h" 13 | CRE_TSK(USR_TASK1,{ TA_NULL,0,usr_task1,MROS_USR_TASK1_PRI,MROS_USR1_STACK_SIZE,NULL }); 14 | CRE_TSK(USR_TASK2,{ TA_NULL,0,usr_task2,MROS_USR_TASK2_PRI,MROS_USR2_STACK_SIZE,NULL }); 15 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/app.h: -------------------------------------------------------------------------------- 1 | //ROSのライブラリインクルードだけでmbedのライブラリとかもインクルードしたい？ 2 | //#include "../mros/ros.h" //rosのプログラムでは #include "ros/ros.h" 合わせたほうがいい？ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | void usr_task1(); 8 | void usr_task2(); 9 | #ifdef __cplusplus 10 | } 11 | #endif 12 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/app.json: -------------------------------------------------------------------------------- 1 | { 2 | "catkin_ws_dir": "../../ros_catkin_ws/", 3 | "ros_include_dir": "/opt/ros/kinetic/include", 4 | "including_msgs": [ 5 | "std_msgs/String.h" 6 | ], 7 | "depending_packages": [ 8 | "std_msgs" 9 | ], 10 | "depending_build_in_packages": [ 11 | ] 12 | } 13 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/include/message_class_specialization.h: -------------------------------------------------------------------------------- 1 | #include "ros.h" 2 | 3 | #include "std_msgs/String.h" 4 | 5 | 6 | 7 | template ros::Subscriber ros::NodeHandle::subscribe(std::string,int,void (*fp)(std_msgs::String*)); 8 | template ros::Publisher ros::NodeHandle::advertise(std::string, int); 9 | template void ros::Publisher::publish(std_msgs::String&); 10 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/include/message_headers.h: -------------------------------------------------------------------------------- 1 | 2 | #include "std_msgs/String.h" 3 | 4 | 5 | 6 | static void callCallback(int id, void (*fp)(void *), char *rbuf){ 7 | switch(id){ 8 | case 9: 9 | subtask_methods::CallCallbackFuncs<9>().call(fp,rbuf); 10 | break; 11 | 12 | 13 | } 14 | } 15 | 16 | static const char* getMD5Sum(int id){ 17 | switch(id){ 18 | case 9: 19 | return message_traits::MD5Sum<9>().value(); 20 | 21 | 22 | } 23 | return NULL; 24 | } -------------------------------------------------------------------------------- /mros_ws/string_pubsub/include/msg_max_size.h: -------------------------------------------------------------------------------- 1 | #define PUB_MSG_MAX_SIZE 0 -------------------------------------------------------------------------------- /mros_ws/string_pubsub/mros_config/mros_sys_config.c: -------------------------------------------------------------------------------- 1 | #include "mros_types.h" 2 | #include "mros_memory.h" 3 | #include "mros_sys_config.h" 4 | #include "kernel.h" 5 | 6 | void mros_sys_config_init(void) 7 | { 8 | mRosReturnType ret; 9 | 10 | ret = mros_mem_init(ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM, ros_inner_topic_publisher_config, &ros_inner_topic_publisher_mempool); 11 | if (ret != MROS_E_OK) { 12 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 13 | return; 14 | } 15 | ret = mros_mem_init(ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM, ros_outer_topic_publisher_config, &ros_outer_topic_publisher_mempool); 16 | if (ret != MROS_E_OK) { 17 | ROS_ERROR("%s %s() %u ret=%d", __FILE__, __FUNCTION__, __LINE__, ret); 18 | return; 19 | } 20 | return; 21 | } 22 | 23 | 24 | void usr_task_activation(void) 25 | { 26 | mros_uint32 i; 27 | for (i = 0; i < MROS_USR_TASK_NUM; i++) { 28 | act_tsk(mros_usr_task_table[i]); 29 | } 30 | } 31 | 32 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/mros_config/mros_sys_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_SYS_CONFIG_H_ 2 | #define _MROS_SYS_CONFIG_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | #include "mros_usr_config.h" 9 | #include "mros_os_config.h" 10 | #include "mros_packet_config.h" 11 | 12 | 13 | 14 | /************************************** 15 | * PROTOCOL 16 | **************************************/ 17 | /* 18 | * portno of master 19 | */ 20 | #define MROS_MASTER_PORT_NO 11311 21 | /* 22 | * dhcp option of self node(0:not use, 1:use) 23 | */ 24 | #define MROS_NODE_USE_DHCP 1 25 | /* 26 | * ipaddr of master 27 | */ 28 | #define MROS_MASTER_IPADDR "192.168.11.5" 29 | 30 | /* 31 | * ipaddr of self node 32 | */ 33 | #define MROS_NODE_IPADDR "127.0.0.1" 34 | 35 | /* 36 | * subnet mask of self node 37 | */ 38 | #define MROS_NODE_SUBNET_MASK "255.255.255.0" 39 | 40 | /* 41 | * portno of slave 42 | */ 43 | #define MROS_SLAVE_PORT_NO 11411 44 | 45 | /* 46 | * portno of pub 47 | */ 48 | #define MROS_PUBLISHER_PORT_NO 11511 49 | 50 | /* 51 | * do not change this parameter 52 | */ 53 | #define MROS_TOPIC_TCP_CLIENT_MAX_NUM ( MROS_PUB_TOPIC_CONNECTOR_MAX_NUM + MROS_SUB_TOPIC_CONNECTOR_MAX_NUM ) 54 | 55 | 56 | /***************************************** 57 | * EXCLUSIVE AREA 58 | *****************************************/ 59 | 60 | /* 61 | * do not change this parameter 62 | */ 63 | #define MROS_GIANT_EXCLUSIVE_AREA_PRIORITY ( \ 64 | ( MROS_USR_TASK_PRI < MROS_TASK_PRI) ? \ 65 | MROS_USR_TASK_PRI : \ 66 | MROS_TASK_PRI \ 67 | ) 68 | 69 | 70 | extern void mros_sys_config_init(void); 71 | extern void usr_task_activation(void); 72 | 73 | #ifdef __cplusplus 74 | } 75 | #endif 76 | 77 | #endif /* _MROS_SYS_CONFIG_H_ */ 78 | 79 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/mros_config/mros_usr_config.c: -------------------------------------------------------------------------------- 1 | #include "mros_types.h" 2 | #include "mros_memory.h" 3 | #include "mros_usr_config.h" 4 | #include "kernel_cfg.h" 5 | 6 | /******************************************************* 7 | * START: Inner Publish topic data memory Config 8 | *******************************************************/ 9 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool1, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL1_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL1_SIZE); 10 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool2, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL2_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL2_SIZE); 11 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_inner_topic_publisher_mempool3, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL3_QUELEN, ROS_INNER_TOPIC_PUBLISHER_MEMPOOL3_SIZE); 12 | mRosMemoryConfigType *ros_inner_topic_publisher_config[ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM] = { 13 | &ros_inner_topic_publisher_mempool1_config, 14 | &ros_inner_topic_publisher_mempool2_config, 15 | &ros_inner_topic_publisher_mempool3_config, 16 | }; 17 | MROS_MEMORY_CONFIG_DECLARE_MANAGER(ros_inner_topic_publisher_mempool, ROS_INNER_TOPIC_PUBLISHER_CONFIG_NUM); 18 | /******************************************************* 19 | * END 20 | *******************************************************/ 21 | 22 | 23 | /******************************************************* 24 | * START: Outer Publish topic data memory Config 25 | *******************************************************/ 26 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool1, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL1_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL1_SIZE); 27 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool2, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL2_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL2_SIZE); 28 | MROS_MEMORY_CONFIG_DECLARE_ENTRY(ros_outer_topic_publisher_mempool3, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL3_QUELEN, ROS_OUTER_TOPIC_PUBLISHER_MEMPOOL3_SIZE); 29 | mRosMemoryConfigType *ros_outer_topic_publisher_config[ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM] = { 30 | &ros_outer_topic_publisher_mempool1_config, 31 | &ros_outer_topic_publisher_mempool2_config, 32 | &ros_outer_topic_publisher_mempool3_config, 33 | }; 34 | MROS_MEMORY_CONFIG_DECLARE_MANAGER(ros_outer_topic_publisher_mempool, ROS_OUTER_TOPIC_PUBLISHER_CONFIG_NUM); 35 | /******************************************************* 36 | * END 37 | *******************************************************/ 38 | 39 | 40 | /**************************************** 41 | * USR OS TASK 42 | ****************************************/ 43 | 44 | mRosTaskIdType mros_usr_task_table[MROS_USR_TASK_NUM] = { 45 | USR_TASK1, 46 | USR_TASK2, 47 | }; 48 | -------------------------------------------------------------------------------- /mros_ws/string_pubsub/mros_config/os/target/os_asp/mros_os_config.h: -------------------------------------------------------------------------------- 1 | #ifndef _MROS_OS_CONFIG_H_ 2 | #define _MROS_OS_CONFIG_H_ 3 | 4 | #include "kernel.h" 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | extern void main_task(void); 11 | extern void sub_task(void); 12 | extern void pub_task(void); 13 | extern void xml_slv_task(void); 14 | extern void xml_mas_task(void); 15 | extern void cyclic_handler(intptr_t exinf); 16 | 17 | /**************************************** 18 | * kernel cfg 19 | ****************************************/ 20 | #ifndef MROS_USR_TASK_PRI 21 | /* 22 | * main task priority 23 | * 24 | * do not change this parameter 25 | */ 26 | #define MAIN_TASK_PRI 7 27 | /* 28 | * mROS task priority 29 | * 30 | * do not change this parameter 31 | */ 32 | #define MROS_TASK_PRI 6 33 | 34 | /* 35 | * user task priority 36 | * 37 | * following config parameter is an example. 38 | */ 39 | #define MROS_USR_TASK1_PRI 9 40 | #define MROS_USR_TASK2_PRI 9 41 | /*8 42 | * user task max priority8 43 | *8 44 | * please set max priority of user tasks. 45 | */ 46 | #define MROS_USR_TASK_PRI 8 47 | #endif /* ROS_USR_TASK_PRI */ 48 | 49 | #ifndef TASK_PORTID 50 | #define TASK_PORTID 1 /* serial port ID for something typing */ 51 | #endif /* TASK_PORTID */ 52 | 53 | #ifndef MROS_TASK_STACK_SIZE 54 | /* 55 | * user task stack size 56 | */ 57 | #define MROS_USR9_STACK_SIZE 1024 * 1 //for startup task 58 | #define MROS_USR8_STACK_SIZE 1024 * 1 //for user task8 59 | #define MROS_USR7_STACK_SIZE 1024 * 1 //for user task7 60 | #define MROS_USR6_STACK_SIZE 1024 * 1 //for user task6 61 | #define MROS_USR5_STACK_SIZE 1024 * 1 //for user task5 62 | #define MROS_USR4_STACK_SIZE 1024 * 1 //for user task4 63 | #define MROS_USR3_STACK_SIZE 1024 * 1 //for user task3 64 | #define MROS_USR2_STACK_SIZE 1024 * 1 //for user task2 65 | #define MROS_USR1_STACK_SIZE 1024 * 1 //for user task1 66 | /* 67 | * mROS task stack size 68 | * 69 | * do not change this parameter 70 | */ 71 | #define MROS_TASK_STACK_SIZE 1024 * 2 //for mros task 72 | #endif /*MROS_TASK_STACK_SIZE*/ 73 | 74 | #ifndef KMM_SIZE 75 | #define MBED_TASK_STACK_SIZE 1024 * 8 //for mbed task 76 | #define KMM_SIZE (MBED_TASK_STACK_SIZE * 16) /* kernel assign */ 77 | #endif /* KMM_SIZE */ /* size of memory */ 78 | 79 | /* 80 | * do not change this parameter 81 | */ 82 | #ifndef CYC 83 | #define MROS_LOOP_RATE 100 84 | #define CYC 85 | #endif /*CYC*/ 86 | 87 | /* 88 | * do not change this parameter 89 | */ 90 | #ifndef LOOP_REF 91 | #define LOOP_REF ULONG_C(1000000) /* number of loops to evaluate speed */ 92 | #endif /* LOOP_REF */ 93 | 94 | #ifdef __cplusplus 95 | } 96 | #endif 97 | 98 | 99 | #endif /* _MROS_OS_CONFIG_H_ */ 100 | -------------------------------------------------------------------------------- /ros_catkin_ws/src/custom_pubsub/msg/LEDValues.msg: -------------------------------------------------------------------------------- 1 | float32 red 2 | float32 green 3 | float32 blue -------------------------------------------------------------------------------- /ros_catkin_ws/src/custom_pubsub/msg/PersonName.msg: -------------------------------------------------------------------------------- 1 | string firstName 2 | string lastName 3 | -------------------------------------------------------------------------------- /ros_catkin_ws/src/custom_pubsub/msg/UserTypeTest.msg: -------------------------------------------------------------------------------- 1 | LEDValues ledVal 2 | PersonName nameVal 3 | -------------------------------------------------------------------------------- /ros_catkin_ws/src/custom_pubsub/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | custom_pubsub 4 | 0.0.0 5 | The custom_pubsub package 6 | 7 | 8 | 9 | 10 | mori 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /ros_catkin_ws/src/custom_pubsub/src/custom_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "custom_pubsub/UserTypeTest.h" 4 | 5 | #include 6 | #include 7 | 8 | using namespace ros; 9 | 10 | 11 | int main(int argc,char **argv){ 12 | init(argc,argv,"custom_publisher"); 13 | 14 | NodeHandle n; 15 | Publisher pub = n.advertise("test_msg",1000); 16 | Rate loop_rate(1); 17 | int count =0; 18 | bool set_msg=false; 19 | custom_pubsub::UserTypeTest msg; 20 | std::stringstream ss; 21 | std::string nav; 22 | std::string str; 23 | int hoge = 0; 24 | int val; 25 | /* quit by Ctrl+C */ 26 | while(ros::ok()){ 27 | nav = "type first name"; 28 | ROS_INFO("%s",nav.c_str()); 29 | std::cin >> str; 30 | msg.nameVal.firstName = str; 31 | nav = "type last name"; 32 | ROS_INFO("%s",nav.c_str()); 33 | std::cin >> str; 34 | msg.nameVal.lastName = str; 35 | int ledVal; 36 | nav = "type red value (0-128)"; 37 | ROS_INFO("%s",nav.c_str()); 38 | while (1) { 39 | if (scanf("%d", &ledVal) == 1 && ledVal >= 0 && ledVal <= 128){ 40 | break; 41 | } 42 | nav = "wrong input: please try again"; 43 | ROS_INFO("%s",nav.c_str()); 44 | scanf("%*[^\n]"); 45 | } 46 | msg.ledVal.red = (float)ledVal/128.0; 47 | while (1) { 48 | nav = "type green value (0-128)"; 49 | ROS_INFO("%s",nav.c_str()); 50 | if (scanf("%d", &ledVal) == 1 && ledVal >= 0 && ledVal <= 128){ 51 | break; 52 | } 53 | nav = "wrong input: please try again"; 54 | ROS_INFO("%s",nav.c_str()); 55 | scanf("%*[^\n]"); 56 | } 57 | msg.ledVal.green = (float)ledVal/128.0; 58 | nav = "type blue value (0-128)"; 59 | ROS_INFO("%s",nav.c_str()); 60 | while (1) { 61 | if (scanf("%d", &ledVal) == 1 && ledVal >= 0 && ledVal <= 128){ 62 | break; 63 | } 64 | nav = "wrong input: please try again"; 65 | ROS_INFO("%s",nav.c_str()); 66 | scanf("%*[^\n]"); 67 | } 68 | msg.ledVal.blue = (float)ledVal/128.0; 69 | pub.publish(msg); 70 | } 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /ros_catkin_ws/src/custom_pubsub/src/custom_subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "custom_pubsub/UserTypeTest.h" 3 | #include 4 | 5 | using namespace ros; 6 | 7 | 8 | void mrosCallback(custom_pubsub::UserTypeTest msg){ 9 | ROS_INFO("I heard from [%s %s]",msg.nameVal.firstName.c_str(),msg.nameVal.lastName.c_str()); 10 | ROS_INFO("red value [%f]",msg.ledVal.red); 11 | ROS_INFO("green value [%f]",msg.ledVal.green); 12 | ROS_INFO("blue value [%f]",msg.ledVal.blue); 13 | } 14 | 15 | int main(int argc,char **argv){ 16 | init(argc,argv,"custom_subscriber"); 17 | 18 | NodeHandle n; 19 | 20 | Subscriber sub = n.subscribe("mros_msg",1000,mrosCallback); 21 | 22 | spin(); 23 | 24 | return 0; 25 | } 26 | -------------------------------------------------------------------------------- /ros_catkin_ws/src/string_pubsub/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | string_pubsub 4 | 0.0.0 5 | The string_pubsub package 6 | 7 | 8 | 9 | 10 | mori 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /ros_catkin_ws/src/string_pubsub/src/led_switch_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | 4 | #include 5 | #include 6 | 7 | using namespace ros; 8 | 9 | 10 | int main(int argc,char **argv){ 11 | init(argc,argv,"led_switch_publisher"); 12 | 13 | NodeHandle n; 14 | 15 | Publisher pub = n.advertise("test_string",1000); 16 | 17 | Rate loop_rate(10); 18 | int count =0; 19 | bool set_msg=false; 20 | /* quit by Ctrl+C */ 21 | while(ros::ok()){ 22 | std_msgs::String msg; 23 | std::stringstream ss; 24 | std::string str,nav; 25 | //ss << "Hello World!" << count; 26 | //ss << std::cin; 27 | //nav = "\n\n[Type LED color] \n [red] or [green] or [blue] or (reset)"; 28 | nav = "Type what you send\n"; 29 | ROS_INFO("%s",nav.c_str()); 30 | std::cin >> str; 31 | msg.data = str; 32 | ROS_INFO("%s",msg.data.c_str()); 33 | 34 | //if(set_msg){ 35 | pub.publish(msg); 36 | set_msg = false; 37 | // } 38 | spinOnce(); 39 | loop_rate.sleep(); 40 | ++count; 41 | } 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /ros_catkin_ws/src/string_pubsub/src/ultrasonic_subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | 4 | #include 5 | 6 | using namespace ros; 7 | 8 | 9 | void mrosCallback(const std_msgs::String::ConstPtr& msg){ 10 | ROS_INFO("I heard [%s]",msg->data.c_str()); 11 | } 12 | 13 | int main(int argc,char **argv){ 14 | init(argc,argv,"ultrasonic_subscriber"); 15 | 16 | NodeHandle n; 17 | 18 | Subscriber sub = n.subscribe("mros_msg",1000,mrosCallback); 19 | 20 | spin(); 21 | 22 | return 0; 23 | } 24 | --------------------------------------------------------------------------------