├── .github └── ISSUE_TEMPLATE │ ├── CODEOWNERS │ ├── bug_report.md │ └── feature_request.md ├── .gitignore ├── .travis.yml ├── .travis ├── Dockerfile └── entrypoint.sh ├── LICENSE ├── README.md ├── actionlib ├── README.md ├── action.go ├── action_client.go ├── action_server.go ├── actionlib.go ├── client_goal_handler.go ├── client_state_machine.go ├── goal_id_gen.go ├── server_goal_handler.go ├── server_state_machine.go ├── simple_action_client.go └── simple_action_server.go ├── gengo ├── context.go ├── gen_test.go ├── generate.go ├── generate_test.go ├── main.go ├── msgs.go ├── parser.go ├── parser_test.go └── testing_util.go ├── ros ├── duration.go ├── duration_test.go ├── header.go ├── header_test.go ├── log.go ├── master.go ├── message.go ├── name.go ├── name_test.go ├── network.go ├── network_test.go ├── node.go ├── node_test.go ├── publisher.go ├── rate.go ├── rate_test.go ├── ros.go ├── serialization.go ├── serialization_test.go ├── service.go ├── service_client.go ├── service_server.go ├── set.go ├── set_test.go ├── subscriber.go ├── temporal.go ├── temporal_test.go ├── time.go └── time_test.go ├── test ├── client.py ├── listener.py ├── server.py ├── talker.py ├── test_client │ └── main.go ├── test_listener │ └── main.go ├── test_listener_with_event │ └── main.go ├── test_message │ ├── AllFieldTypes.msg │ └── message_test.go ├── test_param │ └── main.go ├── test_server │ └── main.go ├── test_simple_action_client │ └── main.go ├── test_simple_action_client_with_callbacks │ └── main.go ├── test_simple_action_server │ └── main.go ├── test_simple_action_server_with_callbacks │ └── main.go ├── test_talker │ └── main.go └── test_talker_with_callbacks │ └── main.go └── xmlrpc ├── xmlrpc.go └── xmlrpc_test.go /.github/ISSUE_TEMPLATE/CODEOWNERS: -------------------------------------------------------------------------------- 1 | 2 | # This file allows changes in specific paths to automatically request review from individuals. 3 | # See https://help.github.com/articles/about-code-owners/ for details. 4 | 5 | # Default all changes to the fetchrobotics opensource team. 6 | * @fetchrobotics/opensource 7 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: cjds 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 16 | **Expected behavior** 17 | A clear and concise description of what you expected to happen. 18 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: cjds 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | # Compiled Object files, Static and Dynamic libs (Shared Objects) 3 | *.o 4 | *.a 5 | *.so 6 | 7 | # Folders 8 | _obj 9 | _test 10 | 11 | # Architecture specific extensions/prefixes 12 | *.[568vq] 13 | [568vq].out 14 | 15 | *.cgo1.go 16 | *.cgo2.c 17 | _cgo_defun.c 18 | _cgo_gotypes.go 19 | _cgo_export.* 20 | 21 | _testmain.go 22 | 23 | *.exe 24 | 25 | vendor/**/* 26 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: go 2 | 3 | go: 4 | - "1.11" 5 | - "1.12" 6 | - "1.13" 7 | - "1.14" 8 | 9 | env: 10 | - ROS_DOCKER=ros:kinetic-ros-base 11 | - ROS_DOCKER=ros:melodic-ros-base 12 | - ROS_DOCKER=ros:noetic-ros-base 13 | 14 | sudo: required 15 | 16 | services: 17 | - docker 18 | 19 | install: 20 | - docker build -f .travis/Dockerfile -t akio/rosgo . --build-arg ROS_DOCKER_TAG=$ROS_DOCKER 21 | 22 | script: 23 | - docker run -v $GOROOT:/usr/local/go akio/rosgo 24 | 25 | -------------------------------------------------------------------------------- /.travis/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DOCKER=ros:noetic-ros-base 2 | FROM $ROS_DOCKER 3 | 4 | RUN apt-get update && apt-get install -y wget 5 | 6 | VOLUME /usr/local/go 7 | 8 | RUN mkdir -p src/github.com/fetchrobotics/rosgo 9 | COPY . src/github.com/fetchrobotics/rosgo 10 | COPY .travis/entrypoint.sh ./entrypoint.sh 11 | 12 | CMD ./entrypoint.sh 13 | -------------------------------------------------------------------------------- /.travis/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | source /opt/ros/noetic/setup.bash 3 | export PATH=$PWD/bin:/usr/local/go/bin:$PATH 4 | export GOPATH=$PWD:/usr/local/go 5 | 6 | roscore & 7 | go install github.com/fetchrobotics/rosgo/gengo 8 | go generate github.com/fetchrobotics/rosgo/test/test_message 9 | go test github.com/fetchrobotics/rosgo/xmlrpc 10 | go test github.com/fetchrobotics/rosgo/ros 11 | go test github.com/fetchrobotics/rosgo/test/test_message 12 | 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2013 akio 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## rosgo 2 | 3 | [![GoDoc](https://godoc.org/github.com/fetchrobotics/rosgo?status.svg)](https://godoc.org/github.com/fetchrobotics/rosgo) 4 | [![Build Status](https://travis-ci.org/fetchrobotics/rosgo.svg?branch=master)](https://travis-ci.org/fetchrobotics/rosgo) 5 | 6 | ## Package Summary 7 | 8 | **rosgo** is pure Go implementation of [ROS](http://www.ros.org/) client library. 9 | 10 | - Author: Akio Ochiai 11 | - Maintainer: Fetch Robotics 12 | - License: Apache License 2.0 13 | - Source: git [https://github.com/fetchrobotics/rosgo](https://github.com/fetchrobotics/rosgo) 14 | - ROS Version Support: [Indigo] [Jade] [Melodic] [Noetic] 15 | 16 | ## Prerequisites 17 | 18 | To use this library you should have installed ROS: [Install](wiki.ros.org/noetic/Installation/Ubuntu). 19 | To run the tests please install all sensor msgs: `sudo apt install ros-noetic-desktop-full` for Ubuntu 20 | 21 | ## Status 22 | 23 | **rosgo** is under development to implement all features of [ROS Client Library Requiements](http://www.ros.org/wiki/Implementing%20Client%20Libraries). 24 | 25 | At present, following basic functions are provided. 26 | 27 | - Parameter API (get/set/search....) 28 | - ROS Slave API (with some exceptions) 29 | - Publisher/Subscriber API (with TCPROS) 30 | - Remapping 31 | - Message Generation 32 | - Action Servers 33 | - Bus Statistics 34 | 35 | Work to do: 36 | 37 | - Go Module Support 38 | - Tutorials 39 | - ROS 2 Support 40 | 41 | ## How to use 42 | 43 | Please look in the [test](test) folder for how to use rosgo in your projects. 44 | 45 | ## See also 46 | 47 | - [rosgo in ROS Wiki](http://www.ros.org/wiki/rosgo) 48 | -------------------------------------------------------------------------------- /actionlib/README.md: -------------------------------------------------------------------------------- 1 | # actionlib [WIP] 2 | 3 | ## Package Summary 4 | 5 | A pure go implementation for ROS action library built on top of ROSGO. This package is unstable and the API can change in future. 6 | 7 | ## Prerequisites 8 | 9 | This library uses messages `GoalID`, `GoalStatus` and `GoalStatusArray` from `actionlib_msgs` package. Please generate Go code for the messages in `actionlib_msgs` package and place them in your `$GOPATH/src`. 10 | 11 | Use the following commands after install `gengo`. 12 | 13 | ```cmd 14 | gengo -out=$GOPATH/src msg actionlib_msgs/GoalID 15 | gengo -out=$GOPATH/src msg actionlib_msgs/GoalStatus 16 | gengo -out=$GOPATH/src msg actionlib_msgs/GoalStatusArray 17 | ``` 18 | 19 | ## Status 20 | 21 | This package implements all the features of actionlib library but is still very unstable and is still a work in progress to fix known issues and make this packge more robust. Following are the features that are implemented and what's to be added in the future. 22 | 23 | ### Implemented 24 | 25 | - Action Client 26 | - Action Server 27 | - Simple Action Client 28 | - Simple Action Server 29 | - Client Goal Handler 30 | - Server Goal Handler 31 | - Go code generation from action definitons 32 | 33 | ### To Be Added 34 | 35 | - Tests 36 | - Documentation 37 | - Fix for golint issues 38 | - Go mod 39 | 40 | ## How To Use 41 | 42 | Examples of client and server usage can be found in `rosgo/test` folder. 43 | -------------------------------------------------------------------------------- /actionlib/action.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | "std_msgs" 6 | 7 | "github.com/fetchrobotics/rosgo/ros" 8 | ) 9 | 10 | type ActionType interface { 11 | MD5Sum() string 12 | Name() string 13 | GoalType() ros.MessageType 14 | FeedbackType() ros.MessageType 15 | ResultType() ros.MessageType 16 | NewAction() Action 17 | } 18 | 19 | type Action interface { 20 | GetActionGoal() ActionGoal 21 | GetActionFeedback() ActionFeedback 22 | GetActionResult() ActionResult 23 | } 24 | 25 | type ActionGoal interface { 26 | ros.Message 27 | GetHeader() std_msgs.Header 28 | GetGoalId() actionlib_msgs.GoalID 29 | GetGoal() ros.Message 30 | SetHeader(std_msgs.Header) 31 | SetGoalId(actionlib_msgs.GoalID) 32 | SetGoal(ros.Message) 33 | } 34 | 35 | type ActionFeedback interface { 36 | ros.Message 37 | GetHeader() std_msgs.Header 38 | GetStatus() actionlib_msgs.GoalStatus 39 | GetFeedback() ros.Message 40 | SetHeader(std_msgs.Header) 41 | SetStatus(actionlib_msgs.GoalStatus) 42 | SetFeedback(ros.Message) 43 | } 44 | 45 | type ActionResult interface { 46 | ros.Message 47 | GetHeader() std_msgs.Header 48 | GetStatus() actionlib_msgs.GoalStatus 49 | GetResult() ros.Message 50 | SetHeader(std_msgs.Header) 51 | SetStatus(actionlib_msgs.GoalStatus) 52 | SetResult(ros.Message) 53 | } 54 | -------------------------------------------------------------------------------- /actionlib/action_client.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | "fmt" 6 | "std_msgs" 7 | "sync" 8 | 9 | "github.com/fetchrobotics/rosgo/ros" 10 | ) 11 | 12 | type defaultActionClient struct { 13 | started bool 14 | node ros.Node 15 | action string 16 | actionType ActionType 17 | actionResult ros.MessageType 18 | actionResultType ros.MessageType 19 | actionFeedback ros.MessageType 20 | actionGoal ros.MessageType 21 | goalPub ros.Publisher 22 | cancelPub ros.Publisher 23 | resultSub ros.Subscriber 24 | feedbackSub ros.Subscriber 25 | statusSub ros.Subscriber 26 | logger ros.Logger 27 | handlers []*clientGoalHandler 28 | handlersMutex sync.RWMutex 29 | goalIDGen *goalIDGenerator 30 | statusReceived bool 31 | callerID string 32 | } 33 | 34 | func newDefaultActionClient(node ros.Node, action string, actType ActionType) *defaultActionClient { 35 | ac := &defaultActionClient{ 36 | node: node, 37 | action: action, 38 | actionType: actType, 39 | actionResult: actType.ResultType(), 40 | actionFeedback: actType.FeedbackType(), 41 | actionGoal: actType.GoalType(), 42 | logger: node.Logger(), 43 | statusReceived: false, 44 | goalIDGen: newGoalIDGenerator(node.Name()), 45 | } 46 | 47 | ac.goalPub = node.NewPublisher(fmt.Sprintf("%s/goal", action), actType.GoalType()) 48 | ac.cancelPub = node.NewPublisher(fmt.Sprintf("%s/cancel", action), actionlib_msgs.MsgGoalID) 49 | ac.resultSub = node.NewSubscriber(fmt.Sprintf("%s/result", action), actType.ResultType(), ac.internalResultCallback) 50 | ac.feedbackSub = node.NewSubscriber(fmt.Sprintf("%s/feedback", action), actType.FeedbackType(), ac.internalFeedbackCallback) 51 | ac.statusSub = node.NewSubscriber(fmt.Sprintf("%s/status", action), actionlib_msgs.MsgGoalStatusArray, ac.internalStatusCallback) 52 | 53 | return ac 54 | } 55 | 56 | func (ac *defaultActionClient) SendGoal(goal ros.Message, transitionCb, feedbackCb interface{}) ClientGoalHandler { 57 | if !ac.started { 58 | ac.logger.Error("[ActionClient] Trying to send a goal on an inactive ActionClient") 59 | } 60 | 61 | ag := ac.actionType.GoalType().NewMessage().(ActionGoal) 62 | goalID := actionlib_msgs.GoalID{Id: ac.goalIDGen.generateID(), Stamp: ros.Now()} 63 | header := std_msgs.Header{Stamp: ros.Now()} 64 | 65 | ag.SetGoal(goal) 66 | ag.SetGoalId(goalID) 67 | ag.SetHeader(header) 68 | ac.PublishActionGoal(ag) 69 | 70 | handler := newClientGoalHandler(ac, ag, transitionCb, feedbackCb) 71 | 72 | ac.handlersMutex.Lock() 73 | ac.handlers = append(ac.handlers, handler) 74 | ac.handlersMutex.Unlock() 75 | 76 | return handler 77 | } 78 | 79 | func (ac *defaultActionClient) CancelAllGoals() { 80 | if !ac.started { 81 | ac.logger.Error("[ActionClient] Trying to cancel goals on an inactive ActionClient") 82 | return 83 | } 84 | 85 | ac.cancelPub.Publish(&actionlib_msgs.GoalID{}) 86 | } 87 | 88 | func (ac *defaultActionClient) CancelAllGoalsBeforeTime(stamp ros.Time) { 89 | if !ac.started { 90 | ac.logger.Error("[ActionClient] Trying to cancel goals on an inactive ActionClient") 91 | return 92 | } 93 | 94 | cancelMsg := &actionlib_msgs.GoalID{Stamp: stamp} 95 | ac.cancelPub.Publish(cancelMsg) 96 | } 97 | 98 | func (ac *defaultActionClient) Shutdown() { 99 | ac.handlersMutex.Lock() 100 | defer ac.handlersMutex.Unlock() 101 | 102 | ac.started = false 103 | for _, h := range ac.handlers { 104 | h.Shutdown(false) 105 | } 106 | 107 | ac.handlers = nil 108 | ac.node.Shutdown() 109 | } 110 | 111 | func (ac *defaultActionClient) PublishActionGoal(ag ActionGoal) { 112 | if ac.started { 113 | ac.goalPub.Publish(ag) 114 | } 115 | } 116 | 117 | func (ac *defaultActionClient) PublishCancel(cancel *actionlib_msgs.GoalID) { 118 | if ac.started { 119 | ac.cancelPub.Publish(cancel) 120 | } 121 | } 122 | 123 | func (ac *defaultActionClient) WaitForServer(timeout ros.Duration) bool { 124 | started := false 125 | ac.logger.Info("[ActionClient] Waiting action server to start") 126 | rate := ros.CycleTime(ros.NewDuration(0, 10000000)) 127 | waitStart := ros.Now() 128 | 129 | LOOP: 130 | for !started { 131 | gSubs := ac.goalPub.GetNumSubscribers() 132 | cSubs := ac.cancelPub.GetNumSubscribers() 133 | fPubs := ac.feedbackSub.GetNumPublishers() 134 | rPubs := ac.resultSub.GetNumPublishers() 135 | sPubs := ac.statusSub.GetNumPublishers() 136 | started = (gSubs > 0 && cSubs > 0 && fPubs > 0 && rPubs > 0 && sPubs > 0) 137 | 138 | now := ros.Now() 139 | diff := now.Diff(waitStart) 140 | if !timeout.IsZero() && diff.Cmp(timeout) >= 0 { 141 | break LOOP 142 | } 143 | 144 | rate.Sleep() 145 | } 146 | 147 | if started { 148 | ac.started = started 149 | } 150 | 151 | return started 152 | } 153 | 154 | func (ac *defaultActionClient) DeleteGoalHandler(gh *clientGoalHandler) { 155 | ac.handlersMutex.Lock() 156 | defer ac.handlersMutex.Unlock() 157 | 158 | for i, h := range ac.handlers { 159 | if h == gh { 160 | ac.handlers[i] = ac.handlers[len(ac.handlers)-1] 161 | ac.handlers[len(ac.handlers)-1] = nil 162 | ac.handlers = ac.handlers[:len(ac.handlers)-1] 163 | } 164 | } 165 | } 166 | 167 | func (ac *defaultActionClient) internalResultCallback(result ActionResult, event ros.MessageEvent) { 168 | ac.handlersMutex.RLock() 169 | defer ac.handlersMutex.RUnlock() 170 | 171 | for _, h := range ac.handlers { 172 | if err := h.updateResult(result); err != nil { 173 | ac.logger.Error(err) 174 | } 175 | } 176 | } 177 | 178 | func (ac *defaultActionClient) internalFeedbackCallback(feedback ActionFeedback, event ros.MessageEvent) { 179 | ac.handlersMutex.RLock() 180 | defer ac.handlersMutex.RUnlock() 181 | 182 | for _, h := range ac.handlers { 183 | h.updateFeedback(feedback) 184 | } 185 | } 186 | 187 | func (ac *defaultActionClient) internalStatusCallback(statusArr *actionlib_msgs.GoalStatusArray, event ros.MessageEvent) { 188 | ac.handlersMutex.RLock() 189 | defer ac.handlersMutex.RUnlock() 190 | 191 | if !ac.statusReceived { 192 | ac.statusReceived = true 193 | ac.logger.Debug("Recieved first status message from action server ") 194 | } else if ac.callerID != event.PublisherName { 195 | ac.logger.Debug("Previously received status from %s, now from %s. Did the action server change", ac.callerID, event.PublisherName) 196 | } 197 | 198 | ac.callerID = event.PublisherName 199 | for _, h := range ac.handlers { 200 | if err := h.updateStatus(statusArr); err != nil { 201 | ac.logger.Error(err) 202 | } 203 | } 204 | } 205 | -------------------------------------------------------------------------------- /actionlib/action_server.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | "fmt" 6 | "reflect" 7 | "std_msgs" 8 | "sync" 9 | "time" 10 | 11 | "github.com/fetchrobotics/rosgo/ros" 12 | ) 13 | 14 | type defaultActionServer struct { 15 | node ros.Node 16 | autoStart bool 17 | started bool 18 | action string 19 | actionType ActionType 20 | actionResult ros.MessageType 21 | actionResultType ros.MessageType 22 | actionFeedback ros.MessageType 23 | actionGoal ros.MessageType 24 | statusMutex sync.RWMutex 25 | statusFrequency ros.Rate 26 | statusTimer *time.Ticker 27 | handlers map[string]*serverGoalHandler 28 | handlersTimeout ros.Duration 29 | handlersMutex sync.Mutex 30 | goalCallback interface{} 31 | cancelCallback interface{} 32 | lastCancel ros.Time 33 | pubQueueSize int 34 | subQueueSize int 35 | goalSub ros.Subscriber 36 | cancelSub ros.Subscriber 37 | resultPub ros.Publisher 38 | feedbackPub ros.Publisher 39 | statusPub ros.Publisher 40 | statusPubChan chan struct{} 41 | goalIDGen *goalIDGenerator 42 | shutdownChan chan struct{} 43 | } 44 | 45 | func newDefaultActionServer(node ros.Node, action string, actType ActionType, goalCb interface{}, cancelCb interface{}, start bool) *defaultActionServer { 46 | return &defaultActionServer{ 47 | node: node, 48 | autoStart: start, 49 | started: false, 50 | action: action, 51 | actionType: actType, 52 | actionResult: actType.ResultType(), 53 | actionFeedback: actType.FeedbackType(), 54 | actionGoal: actType.GoalType(), 55 | handlersTimeout: ros.NewDuration(60, 0), 56 | goalCallback: goalCb, 57 | cancelCallback: cancelCb, 58 | lastCancel: ros.Now(), 59 | } 60 | } 61 | 62 | func (as *defaultActionServer) init() { 63 | as.statusPubChan = make(chan struct{}, 10) 64 | as.shutdownChan = make(chan struct{}, 10) 65 | 66 | // setup goal id generator and goal handlers 67 | as.goalIDGen = newGoalIDGenerator(as.node.Name()) 68 | as.handlers = map[string]*serverGoalHandler{} 69 | 70 | // setup action result type so that we can create default result messages 71 | res := as.actionResult.NewMessage().(ActionResult).GetResult() 72 | as.actionResultType = res.Type() 73 | 74 | // get frequency from ros params 75 | as.statusFrequency = ros.NewRate(5.0) 76 | 77 | // get queue sizes from ros params 78 | // queue sizes not implemented by ros.Node yet 79 | as.pubQueueSize = 50 80 | as.subQueueSize = 50 81 | 82 | as.goalSub = as.node.NewSubscriber(fmt.Sprintf("%s/goal", as.action), as.actionType.GoalType(), as.internalGoalCallback) 83 | as.cancelSub = as.node.NewSubscriber(fmt.Sprintf("%s/cancel", as.action), actionlib_msgs.MsgGoalID, as.internalCancelCallback) 84 | as.resultPub = as.node.NewPublisher(fmt.Sprintf("%s/result", as.action), as.actionType.ResultType()) 85 | as.feedbackPub = as.node.NewPublisher(fmt.Sprintf("%s/feedback", as.action), as.actionType.FeedbackType()) 86 | as.statusPub = as.node.NewPublisher(fmt.Sprintf("%s/status", as.action), actionlib_msgs.MsgGoalStatusArray) 87 | } 88 | 89 | func (as *defaultActionServer) Start() { 90 | logger := as.node.Logger() 91 | defer func() { 92 | logger.Debug("defaultActionServer.start exit") 93 | as.started = false 94 | }() 95 | 96 | // initialize subscribers and publishers 97 | as.init() 98 | 99 | // start status publish ticker that notifies at 5hz 100 | as.statusTimer = time.NewTicker(time.Second / 5.0) 101 | defer as.statusTimer.Stop() 102 | 103 | as.started = true 104 | 105 | for { 106 | select { 107 | case <-as.shutdownChan: 108 | return 109 | 110 | case <-as.statusTimer.C: 111 | as.PublishStatus() 112 | 113 | case <-as.statusPubChan: 114 | arr := as.getStatus() 115 | as.statusPub.Publish(arr) 116 | } 117 | } 118 | } 119 | 120 | // PublishResult publishes action result message 121 | func (as *defaultActionServer) PublishResult(status actionlib_msgs.GoalStatus, result ros.Message) { 122 | msg := as.actionResult.NewMessage().(ActionResult) 123 | msg.SetHeader(std_msgs.Header{Stamp: ros.Now()}) 124 | msg.SetStatus(status) 125 | msg.SetResult(result) 126 | as.resultPub.Publish(msg) 127 | } 128 | 129 | // PublishFeedback publishes action feedback messages 130 | func (as *defaultActionServer) PublishFeedback(status actionlib_msgs.GoalStatus, feedback ros.Message) { 131 | msg := as.actionFeedback.NewMessage().(ActionFeedback) 132 | msg.SetHeader(std_msgs.Header{Stamp: ros.Now()}) 133 | msg.SetStatus(status) 134 | msg.SetFeedback(feedback) 135 | as.feedbackPub.Publish(msg) 136 | } 137 | 138 | func (as *defaultActionServer) getStatus() *actionlib_msgs.GoalStatusArray { 139 | as.handlersMutex.Lock() 140 | defer as.handlersMutex.Unlock() 141 | var statusList []actionlib_msgs.GoalStatus 142 | 143 | if as.node.OK() { 144 | for id, gh := range as.handlers { 145 | handlerTime := gh.GetHandlerDestructionTime() 146 | destroyTime := handlerTime.Add(as.handlersTimeout) 147 | 148 | if !handlerTime.IsZero() && destroyTime.Cmp(ros.Now()) <= 0 { 149 | delete(as.handlers, id) 150 | continue 151 | } 152 | 153 | statusList = append(statusList, gh.GetGoalStatus()) 154 | } 155 | } 156 | 157 | goalStatus := &actionlib_msgs.GoalStatusArray{} 158 | goalStatus.Header.Stamp = ros.Now() 159 | goalStatus.StatusList = statusList 160 | return goalStatus 161 | } 162 | 163 | func (as *defaultActionServer) PublishStatus() { 164 | as.statusPubChan <- struct{}{} 165 | } 166 | 167 | // internalCancelCallback recieves cancel message from client 168 | func (as *defaultActionServer) internalCancelCallback(goalID *actionlib_msgs.GoalID, event ros.MessageEvent) { 169 | as.handlersMutex.Lock() 170 | defer as.handlersMutex.Unlock() 171 | 172 | goalFound := false 173 | logger := as.node.Logger() 174 | logger.Debug("Action server has received a new cancel request") 175 | 176 | for id, gh := range as.handlers { 177 | cancelAll := (goalID.Id == "" && goalID.Stamp.IsZero()) 178 | cancelCurrent := (goalID.Id == id) 179 | 180 | st := gh.GetGoalStatus() 181 | cancelBeforeStamp := (!goalID.Stamp.IsZero() && st.GoalId.Stamp.Cmp(goalID.Stamp) <= 0) 182 | 183 | if cancelAll || cancelCurrent || cancelBeforeStamp { 184 | if goalID.Id == st.GoalId.Id { 185 | goalFound = true 186 | } 187 | 188 | if gh.SetCancelRequested() { 189 | args := []reflect.Value{reflect.ValueOf(goalID)} 190 | fun := reflect.ValueOf(as.cancelCallback) 191 | numArgsNeeded := fun.Type().NumIn() 192 | 193 | if numArgsNeeded <= 1 { 194 | fun.Call(args[0:numArgsNeeded]) 195 | } 196 | } 197 | } 198 | } 199 | 200 | if goalID.Id != "" && !goalFound { 201 | gh := newServerGoalHandlerWithGoalId(as, goalID) 202 | as.handlers[goalID.Id] = gh 203 | gh.SetHandlerDestructionTime(ros.Now()) 204 | } 205 | 206 | if goalID.Stamp.Cmp(as.lastCancel) > 0 { 207 | as.lastCancel = goalID.Stamp 208 | } 209 | } 210 | 211 | // internalGoalCallback recieves the goals from client and checks if 212 | // the goalID already exists in the status list. If not, it will call 213 | // server's goalCallback with goal that was recieved from the client. 214 | func (as *defaultActionServer) internalGoalCallback(goal ActionGoal, event ros.MessageEvent) { 215 | as.handlersMutex.Lock() 216 | defer as.handlersMutex.Unlock() 217 | 218 | logger := as.node.Logger() 219 | goalID := goal.GetGoalId() 220 | 221 | for id, gh := range as.handlers { 222 | if goalID.Id == id { 223 | st := gh.GetGoalStatus() 224 | logger.Debugf("Goal %s was already in the status list with status %+v", goalID.Id, st.Status) 225 | if st.Status == actionlib_msgs.RECALLING { 226 | st.Status = actionlib_msgs.RECALLED 227 | result := as.actionResultType.NewMessage() 228 | as.PublishResult(st, result) 229 | } 230 | 231 | gh.SetHandlerDestructionTime(ros.Now()) 232 | return 233 | } 234 | } 235 | 236 | id := goalID.Id 237 | if len(id) == 0 { 238 | id = as.goalIDGen.generateID() 239 | goal.SetGoalId(actionlib_msgs.GoalID{ 240 | Id: id, 241 | Stamp: goalID.Stamp, 242 | }) 243 | } 244 | 245 | gh := newServerGoalHandlerWithGoal(as, goal) 246 | as.handlers[id] = gh 247 | if !goalID.Stamp.IsZero() && goalID.Stamp.Cmp(as.lastCancel) <= 0 { 248 | gh.SetCancelled(nil, "timestamp older than last goal cancel") 249 | return 250 | } 251 | 252 | args := []reflect.Value{reflect.ValueOf(goal), reflect.ValueOf(event)} 253 | fun := reflect.ValueOf(as.goalCallback) 254 | numArgsNeeded := fun.Type().NumIn() 255 | 256 | if numArgsNeeded <= 1 { 257 | fun.Call(args[0:numArgsNeeded]) 258 | } 259 | } 260 | 261 | func (as *defaultActionServer) getHandler(id string) *serverGoalHandler { 262 | handler := as.handlers[id] 263 | return handler 264 | } 265 | 266 | // RegisterGoalCallback replaces existing goal callback function with newly 267 | // provided goal callback function. 268 | func (as *defaultActionServer) RegisterGoalCallback(goalCb interface{}) { 269 | as.goalCallback = goalCb 270 | } 271 | 272 | func (as *defaultActionServer) RegisterCancelCallback(cancelCb interface{}) { 273 | as.cancelCallback = cancelCb 274 | } 275 | 276 | func (as *defaultActionServer) Shutdown() { 277 | as.shutdownChan <- struct{}{} 278 | } 279 | -------------------------------------------------------------------------------- /actionlib/actionlib.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | 6 | "github.com/fetchrobotics/rosgo/ros" 7 | ) 8 | 9 | func NewActionClient(node ros.Node, action string, actionType ActionType) ActionClient { 10 | return newDefaultActionClient(node, action, actionType) 11 | } 12 | 13 | func NewActionServer(node ros.Node, action string, actionType ActionType, goalCb, cancelCb interface{}, autoStart bool) ActionServer { 14 | return newDefaultActionServer(node, action, actionType, goalCb, cancelCb, autoStart) 15 | } 16 | 17 | func NewSimpleActionClient(node ros.Node, action string, actionType ActionType) SimpleActionClient { 18 | return newSimpleActionClient(node, action, actionType) 19 | } 20 | 21 | func NewSimpleActionServer(node ros.Node, action string, actionType ActionType, executeCb interface{}, autoStart bool) SimpleActionServer { 22 | return newSimpleActionServer(node, action, actionType, executeCb, autoStart) 23 | } 24 | 25 | func NewServerGoalHandlerWithGoal(as ActionServer, goal ActionGoal) ServerGoalHandler { 26 | return newServerGoalHandlerWithGoal(as, goal) 27 | } 28 | 29 | func NewServerGoalHandlerWithGoalId(as ActionServer, goalID *actionlib_msgs.GoalID) ServerGoalHandler { 30 | return newServerGoalHandlerWithGoalId(as, goalID) 31 | } 32 | 33 | type ActionClient interface { 34 | WaitForServer(timeout ros.Duration) bool 35 | SendGoal(goal ros.Message, transitionCallback interface{}, feedbackCallback interface{}) ClientGoalHandler 36 | CancelAllGoals() 37 | CancelAllGoalsBeforeTime(stamp ros.Time) 38 | } 39 | 40 | type ActionServer interface { 41 | Start() 42 | Shutdown() 43 | PublishResult(status actionlib_msgs.GoalStatus, result ros.Message) 44 | PublishFeedback(status actionlib_msgs.GoalStatus, feedback ros.Message) 45 | PublishStatus() 46 | RegisterGoalCallback(interface{}) 47 | RegisterCancelCallback(interface{}) 48 | } 49 | 50 | type SimpleActionClient interface { 51 | SendGoal(goal ros.Message, doneCb, activeCb, feedbackCb interface{}) 52 | SendGoalAndWait(goal ros.Message, executeTimeout, preeptTimeout ros.Duration) (uint8, error) 53 | WaitForServer(timeout ros.Duration) bool 54 | WaitForResult(timeout ros.Duration) bool 55 | GetResult() (ros.Message, error) 56 | GetState() (uint8, error) 57 | GetGoalStatusText() (string, error) 58 | CancelAllGoals() 59 | CancelAllGoalsBeforeTime(stamp ros.Time) 60 | CancelGoal() error 61 | StopTrackingGoal() 62 | } 63 | 64 | type SimpleActionServer interface { 65 | Start() 66 | IsNewGoalAvailable() bool 67 | IsPreemptRequested() bool 68 | IsActive() bool 69 | SetSucceeded(result ros.Message, text string) error 70 | SetAborted(result ros.Message, text string) error 71 | SetPreempted(result ros.Message, text string) error 72 | AcceptNewGoal() (ros.Message, error) 73 | PublishFeedback(feedback ros.Message) 74 | GetDefaultResult() ros.Message 75 | RegisterGoalCallback(callback interface{}) error 76 | RegisterPreemptCallback(callback interface{}) 77 | } 78 | 79 | type ClientGoalHandler interface { 80 | IsExpired() bool 81 | GetCommState() (CommState, error) 82 | GetGoalStatus() (uint8, error) 83 | GetGoalStatusText() (string, error) 84 | GetTerminalState() (uint8, error) 85 | GetResult() (ros.Message, error) 86 | Resend() error 87 | Cancel() error 88 | } 89 | 90 | type ServerGoalHandler interface { 91 | SetAccepted(string) error 92 | SetCancelled(ros.Message, string) error 93 | SetRejected(ros.Message, string) error 94 | SetAborted(ros.Message, string) error 95 | SetSucceeded(ros.Message, string) error 96 | SetCancelRequested() bool 97 | PublishFeedback(ros.Message) 98 | GetGoal() ros.Message 99 | GetGoalId() actionlib_msgs.GoalID 100 | GetGoalStatus() actionlib_msgs.GoalStatus 101 | Equal(ServerGoalHandler) bool 102 | NotEqual(ServerGoalHandler) bool 103 | Hash() uint32 104 | GetHandlerDestructionTime() ros.Time 105 | SetHandlerDestructionTime(ros.Time) 106 | } 107 | -------------------------------------------------------------------------------- /actionlib/client_goal_handler.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | "fmt" 6 | "reflect" 7 | 8 | "github.com/fetchrobotics/rosgo/ros" 9 | ) 10 | 11 | type clientGoalHandler struct { 12 | actionClient *defaultActionClient 13 | stateMachine *clientStateMachine 14 | actionGoal ActionGoal 15 | actionGoalID string 16 | transitionCb interface{} 17 | feedbackCb interface{} 18 | logger ros.Logger 19 | } 20 | 21 | func newClientGoalHandler(ac *defaultActionClient, ag ActionGoal, transitionCb, feedbackCb interface{}) *clientGoalHandler { 22 | return &clientGoalHandler{ 23 | actionClient: ac, 24 | stateMachine: newClientStateMachine(), 25 | actionGoal: ag, 26 | actionGoalID: ag.GetGoalId().Id, 27 | transitionCb: transitionCb, 28 | feedbackCb: feedbackCb, 29 | logger: ac.logger, 30 | } 31 | } 32 | 33 | func findGoalStatus(statusArr *actionlib_msgs.GoalStatusArray, id string) *actionlib_msgs.GoalStatus { 34 | var status actionlib_msgs.GoalStatus 35 | for _, st := range statusArr.StatusList { 36 | if st.GoalId.Id == id { 37 | status = st 38 | break 39 | } 40 | } 41 | 42 | return &status 43 | } 44 | 45 | func (gh *clientGoalHandler) GetCommState() (CommState, error) { 46 | if gh.stateMachine == nil { 47 | return Lost, fmt.Errorf("trying to get state on an inactive ClientGoalHandler") 48 | } 49 | 50 | return gh.stateMachine.getState(), nil 51 | } 52 | 53 | func (gh *clientGoalHandler) GetGoalStatus() (uint8, error) { 54 | if gh.stateMachine == nil { 55 | return actionlib_msgs.LOST, fmt.Errorf("trying to get goal status on an inactive ClientGoalHandler") 56 | } 57 | 58 | return gh.stateMachine.getGoalStatus().Status, nil 59 | } 60 | 61 | func (gh *clientGoalHandler) GetGoalStatusText() (string, error) { 62 | if gh.stateMachine == nil { 63 | return "", fmt.Errorf("trying to get goal status text on an inactive ClientGoalHandler") 64 | } 65 | 66 | return gh.stateMachine.getGoalStatus().Text, nil 67 | } 68 | 69 | func (gh *clientGoalHandler) GetTerminalState() (uint8, error) { 70 | if gh.stateMachine == nil { 71 | return 0, fmt.Errorf("trying to get goal status on inactive clientGoalHandler") 72 | } 73 | 74 | if gh.stateMachine.state != Done { 75 | gh.actionClient.logger.Warnf("Asking for terminal state when we are in state %v", gh.stateMachine.state) 76 | } 77 | 78 | // implement get status 79 | goalStatus := gh.stateMachine.getGoalStatus().Status 80 | if goalStatus == actionlib_msgs.PREEMPTED || 81 | goalStatus == actionlib_msgs.SUCCEEDED || 82 | goalStatus == actionlib_msgs.ABORTED || 83 | goalStatus == actionlib_msgs.REJECTED || 84 | goalStatus == actionlib_msgs.RECALLED || 85 | goalStatus == actionlib_msgs.LOST { 86 | 87 | return goalStatus, nil 88 | } 89 | 90 | gh.actionClient.logger.Warnf("Asking for terminal state when latest goal is in %v", goalStatus) 91 | return actionlib_msgs.LOST, nil 92 | } 93 | 94 | func (gh *clientGoalHandler) GetResult() (ros.Message, error) { 95 | if gh.stateMachine == nil { 96 | return nil, fmt.Errorf("trying to get goal status on inactive clientGoalHandler") 97 | } 98 | 99 | result := gh.stateMachine.getGoalResult() 100 | 101 | if result == nil { 102 | return nil, fmt.Errorf("trying to get result when no result has been recieved") 103 | } 104 | 105 | return result.GetResult(), nil 106 | } 107 | 108 | func (gh *clientGoalHandler) Resend() error { 109 | if gh.stateMachine == nil { 110 | return fmt.Errorf("trying to call resend on inactive client goal hanlder") 111 | } 112 | 113 | gh.actionClient.goalPub.Publish(gh.actionGoal) 114 | return nil 115 | } 116 | 117 | func (gh *clientGoalHandler) IsExpired() bool { 118 | return gh.stateMachine == nil 119 | } 120 | 121 | func (gh *clientGoalHandler) Cancel() error { 122 | if gh.stateMachine == nil { 123 | return fmt.Errorf("trying to call cancel on inactive client goal hanlder") 124 | } 125 | 126 | cancelMsg := &actionlib_msgs.GoalID{ 127 | Stamp: ros.Now(), 128 | Id: gh.actionGoalID} 129 | 130 | gh.actionClient.cancelPub.Publish(cancelMsg) 131 | gh.stateMachine.transitionTo(WaitingForCancelAck, gh, gh.transitionCb) 132 | return nil 133 | } 134 | 135 | func (gh *clientGoalHandler) Shutdown(deleteFromManager bool) { 136 | gh.stateMachine = nil 137 | if deleteFromManager { 138 | gh.actionClient.DeleteGoalHandler(gh) 139 | } 140 | } 141 | 142 | func (gh *clientGoalHandler) updateFeedback(af ActionFeedback) { 143 | if gh.actionGoalID != af.GetStatus().GoalId.Id { 144 | return 145 | } 146 | 147 | if gh.feedbackCb != nil && gh.stateMachine.getState() != Done { 148 | fun := reflect.ValueOf(gh.feedbackCb) 149 | args := []reflect.Value{reflect.ValueOf(gh), reflect.ValueOf(af.GetFeedback())} 150 | numArgsNeeded := fun.Type().NumIn() 151 | 152 | if numArgsNeeded == 2 { 153 | fun.Call(args) 154 | } 155 | } 156 | } 157 | 158 | func (gh *clientGoalHandler) updateResult(result ActionResult) error { 159 | if gh.actionGoalID != result.GetStatus().GoalId.Id { 160 | return nil 161 | } 162 | 163 | status := result.GetStatus() 164 | state := gh.stateMachine.getState() 165 | 166 | gh.stateMachine.setGoalStatus(status.GoalId, status.Status, status.Text) 167 | gh.stateMachine.setGoalResult(result) 168 | 169 | if state == WaitingForGoalAck || 170 | state == WaitingForCancelAck || 171 | state == Pending || 172 | state == Active || 173 | state == WaitingForResult || 174 | state == Recalling || 175 | state == Preempting { 176 | 177 | statusArr := new(actionlib_msgs.GoalStatusArray) 178 | statusArr.StatusList = append(statusArr.StatusList, result.GetStatus()) 179 | if err := gh.updateStatus(statusArr); err != nil { 180 | return err 181 | } 182 | 183 | gh.stateMachine.transitionTo(Done, gh, gh.transitionCb) 184 | return nil 185 | } else if state == Done { 186 | return fmt.Errorf("got a result when we are in the `DONE` state") 187 | } else { 188 | return fmt.Errorf("unknown state %v", state) 189 | } 190 | } 191 | 192 | func (gh *clientGoalHandler) updateStatus(statusArr *actionlib_msgs.GoalStatusArray) error { 193 | state := gh.stateMachine.getState() 194 | if state == Done { 195 | return nil 196 | } 197 | 198 | status := findGoalStatus(statusArr, gh.actionGoalID) 199 | if status == nil { 200 | if state != WaitingForGoalAck && 201 | state != WaitingForResult && 202 | state != Done { 203 | 204 | gh.logger.Warn("Transitioning goal to `Lost`") 205 | gh.stateMachine.setAsLost() 206 | gh.stateMachine.transitionTo(Done, gh, gh.transitionCb) 207 | } 208 | return nil 209 | } 210 | 211 | gh.stateMachine.setGoalStatus(status.GoalId, status.Status, status.Text) 212 | nextStates, err := gh.stateMachine.getTransitions(*status) 213 | if err != nil { 214 | return fmt.Errorf("error getting transitions: %v", err) 215 | } 216 | 217 | for e := nextStates.Front(); e != nil; e = e.Next() { 218 | gh.stateMachine.transitionTo(e.Value.(CommState), gh, gh.transitionCb) 219 | } 220 | 221 | return nil 222 | } 223 | -------------------------------------------------------------------------------- /actionlib/client_state_machine.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | "container/list" 6 | "fmt" 7 | "reflect" 8 | "sync" 9 | ) 10 | 11 | type CommState uint8 12 | 13 | const ( 14 | WaitingForGoalAck CommState = iota 15 | Pending 16 | Active 17 | WaitingForResult 18 | WaitingForCancelAck 19 | Recalling 20 | Preempting 21 | Done 22 | Lost 23 | ) 24 | 25 | func (cs CommState) String() string { 26 | switch cs { 27 | case WaitingForGoalAck: 28 | return "WAITING_FOR_GOAL_ACK" 29 | case Pending: 30 | return "PENDING" 31 | case Active: 32 | return "ACTIVE" 33 | case WaitingForResult: 34 | return "WAITING_FOR_RESULT" 35 | case WaitingForCancelAck: 36 | return "WAITING_FOR_CANCEL_ACK" 37 | case Recalling: 38 | return "RECALLING" 39 | case Preempting: 40 | return "PREEMPTING" 41 | case Done: 42 | return "DONE" 43 | case Lost: 44 | return "LOST" 45 | default: 46 | return "UNKNOWN" 47 | } 48 | } 49 | 50 | type clientStateMachine struct { 51 | state CommState 52 | goalStatus actionlib_msgs.GoalStatus 53 | goalResult ActionResult 54 | mutex sync.RWMutex 55 | } 56 | 57 | func newClientStateMachine() *clientStateMachine { 58 | return &clientStateMachine{ 59 | state: WaitingForGoalAck, 60 | goalStatus: actionlib_msgs.GoalStatus{Status: actionlib_msgs.PENDING}, 61 | } 62 | } 63 | 64 | func (sm *clientStateMachine) getState() CommState { 65 | sm.mutex.RLock() 66 | defer sm.mutex.RUnlock() 67 | 68 | return sm.state 69 | } 70 | 71 | func (sm *clientStateMachine) getGoalStatus() actionlib_msgs.GoalStatus { 72 | sm.mutex.RLock() 73 | defer sm.mutex.RUnlock() 74 | 75 | return sm.goalStatus 76 | } 77 | 78 | func (sm *clientStateMachine) getGoalResult() ActionResult { 79 | sm.mutex.RLock() 80 | defer sm.mutex.RUnlock() 81 | 82 | return sm.goalResult 83 | } 84 | 85 | func (sm *clientStateMachine) setState(state CommState) { 86 | sm.mutex.Lock() 87 | defer sm.mutex.Unlock() 88 | 89 | sm.state = state 90 | } 91 | 92 | func (sm *clientStateMachine) setGoalStatus(id actionlib_msgs.GoalID, status uint8, text string) { 93 | sm.mutex.Lock() 94 | defer sm.mutex.Unlock() 95 | 96 | sm.goalStatus.GoalId = id 97 | sm.goalStatus.Status = status 98 | sm.goalStatus.Text = text 99 | } 100 | 101 | func (sm *clientStateMachine) setGoalResult(result ActionResult) { 102 | sm.mutex.Lock() 103 | defer sm.mutex.Unlock() 104 | 105 | sm.goalResult = result 106 | } 107 | 108 | func (sm *clientStateMachine) setAsLost() { 109 | sm.mutex.Lock() 110 | defer sm.mutex.Unlock() 111 | 112 | sm.goalStatus.Status = uint8(Lost) 113 | } 114 | 115 | func (sm *clientStateMachine) transitionTo(state CommState, gh ClientGoalHandler, callback interface{}) { 116 | sm.setState(state) 117 | if callback != nil { 118 | fun := reflect.ValueOf(callback) 119 | args := []reflect.Value{reflect.ValueOf(gh)} 120 | numArgsNeeded := fun.Type().NumIn() 121 | 122 | if numArgsNeeded <= 1 { 123 | fun.Call(args[:numArgsNeeded]) 124 | } 125 | } 126 | } 127 | 128 | func (sm *clientStateMachine) getTransitions(goalStatus actionlib_msgs.GoalStatus) (stateList list.List, err error) { 129 | sm.mutex.RLock() 130 | defer sm.mutex.RUnlock() 131 | 132 | status := goalStatus.Status 133 | 134 | switch sm.state { 135 | case WaitingForGoalAck: 136 | switch status { 137 | case actionlib_msgs.PENDING: 138 | stateList.PushBack(Pending) 139 | break 140 | case actionlib_msgs.ACTIVE: 141 | stateList.PushBack(Active) 142 | break 143 | case actionlib_msgs.REJECTED: 144 | stateList.PushBack(Pending) 145 | stateList.PushBack(WaitingForCancelAck) 146 | break 147 | case actionlib_msgs.RECALLING: 148 | stateList.PushBack(Pending) 149 | stateList.PushBack(Recalling) 150 | break 151 | case actionlib_msgs.RECALLED: 152 | stateList.PushBack(Pending) 153 | stateList.PushBack(WaitingForResult) 154 | break 155 | case actionlib_msgs.PREEMPTED: 156 | stateList.PushBack(Active) 157 | stateList.PushBack(Preempting) 158 | stateList.PushBack(WaitingForResult) 159 | break 160 | case actionlib_msgs.SUCCEEDED: 161 | stateList.PushBack(Active) 162 | stateList.PushBack(WaitingForResult) 163 | break 164 | case actionlib_msgs.ABORTED: 165 | stateList.PushBack(Active) 166 | stateList.PushBack(WaitingForResult) 167 | break 168 | case actionlib_msgs.PREEMPTING: 169 | stateList.PushBack(Active) 170 | stateList.PushBack(Preempting) 171 | break 172 | } 173 | break 174 | 175 | case Pending: 176 | switch status { 177 | case actionlib_msgs.PENDING: 178 | break 179 | case actionlib_msgs.ACTIVE: 180 | stateList.PushBack(Active) 181 | break 182 | case actionlib_msgs.REJECTED: 183 | stateList.PushBack(WaitingForResult) 184 | break 185 | case actionlib_msgs.RECALLING: 186 | stateList.PushBack(Recalling) 187 | break 188 | case actionlib_msgs.RECALLED: 189 | stateList.PushBack(Recalling) 190 | stateList.PushBack(WaitingForResult) 191 | break 192 | case actionlib_msgs.PREEMPTED: 193 | stateList.PushBack(Active) 194 | stateList.PushBack(Preempting) 195 | stateList.PushBack(WaitingForResult) 196 | break 197 | case actionlib_msgs.SUCCEEDED: 198 | stateList.PushBack(Active) 199 | stateList.PushBack(WaitingForResult) 200 | break 201 | case actionlib_msgs.ABORTED: 202 | stateList.PushBack(Active) 203 | stateList.PushBack(WaitingForResult) 204 | break 205 | case actionlib_msgs.PREEMPTING: 206 | stateList.PushBack(Active) 207 | stateList.PushBack(Preempting) 208 | break 209 | } 210 | break 211 | case Active: 212 | switch status { 213 | case actionlib_msgs.PENDING: 214 | err = fmt.Errorf("invalid transition from Active to Pending") 215 | break 216 | case actionlib_msgs.ACTIVE: 217 | break 218 | case actionlib_msgs.REJECTED: 219 | err = fmt.Errorf("invalid transition from Active to Rejected") 220 | break 221 | case actionlib_msgs.RECALLING: 222 | err = fmt.Errorf("invalid transition from Active to Recalling") 223 | break 224 | case actionlib_msgs.RECALLED: 225 | err = fmt.Errorf("invalid transition from Active to Recalled") 226 | break 227 | case actionlib_msgs.PREEMPTED: 228 | stateList.PushBack(Preempting) 229 | stateList.PushBack(WaitingForResult) 230 | break 231 | case actionlib_msgs.SUCCEEDED: 232 | stateList.PushBack(WaitingForResult) 233 | break 234 | case actionlib_msgs.ABORTED: 235 | stateList.PushBack(WaitingForResult) 236 | break 237 | case actionlib_msgs.PREEMPTING: 238 | stateList.PushBack(Preempting) 239 | break 240 | } 241 | break 242 | case WaitingForResult: 243 | switch status { 244 | case actionlib_msgs.PENDING: 245 | err = fmt.Errorf("invalid transition from WaitingForResult to Pending") 246 | break 247 | case actionlib_msgs.ACTIVE: 248 | break 249 | case actionlib_msgs.REJECTED: 250 | break 251 | case actionlib_msgs.RECALLING: 252 | err = fmt.Errorf("invalid transition from WaitingForResult to Recalling") 253 | break 254 | case actionlib_msgs.RECALLED: 255 | break 256 | case actionlib_msgs.PREEMPTED: 257 | break 258 | case actionlib_msgs.SUCCEEDED: 259 | break 260 | case actionlib_msgs.ABORTED: 261 | break 262 | case actionlib_msgs.PREEMPTING: 263 | err = fmt.Errorf("invalid transition from WaitingForResult to Preempting") 264 | break 265 | } 266 | break 267 | case WaitingForCancelAck: 268 | switch status { 269 | case actionlib_msgs.PENDING: 270 | break 271 | case actionlib_msgs.ACTIVE: 272 | break 273 | case actionlib_msgs.REJECTED: 274 | stateList.PushBack(WaitingForResult) 275 | break 276 | case actionlib_msgs.RECALLING: 277 | stateList.PushBack(Recalling) 278 | break 279 | case actionlib_msgs.RECALLED: 280 | stateList.PushBack(Recalling) 281 | stateList.PushBack(WaitingForResult) 282 | break 283 | case actionlib_msgs.PREEMPTED: 284 | stateList.PushBack(Preempting) 285 | stateList.PushBack(WaitingForResult) 286 | break 287 | case actionlib_msgs.SUCCEEDED: 288 | stateList.PushBack(Recalling) 289 | stateList.PushBack(WaitingForResult) 290 | break 291 | case actionlib_msgs.ABORTED: 292 | stateList.PushBack(Recalling) 293 | stateList.PushBack(WaitingForResult) 294 | break 295 | case actionlib_msgs.PREEMPTING: 296 | stateList.PushBack(Preempting) 297 | break 298 | } 299 | break 300 | case Recalling: 301 | switch status { 302 | case actionlib_msgs.PENDING: 303 | err = fmt.Errorf("invalid transition from Recalling to Pending") 304 | break 305 | case actionlib_msgs.ACTIVE: 306 | err = fmt.Errorf("invalid transition from Recalling to Active") 307 | break 308 | case actionlib_msgs.REJECTED: 309 | stateList.PushBack(WaitingForResult) 310 | break 311 | case actionlib_msgs.RECALLING: 312 | break 313 | case actionlib_msgs.RECALLED: 314 | stateList.PushBack(WaitingForResult) 315 | break 316 | case actionlib_msgs.PREEMPTED: 317 | stateList.PushBack(Preempting) 318 | stateList.PushBack(WaitingForResult) 319 | break 320 | case actionlib_msgs.SUCCEEDED: 321 | stateList.PushBack(Preempting) 322 | stateList.PushBack(WaitingForResult) 323 | break 324 | case actionlib_msgs.ABORTED: 325 | stateList.PushBack(Preempting) 326 | stateList.PushBack(WaitingForResult) 327 | break 328 | case actionlib_msgs.PREEMPTING: 329 | stateList.PushBack(Preempting) 330 | break 331 | } 332 | break 333 | case Preempting: 334 | switch status { 335 | case actionlib_msgs.PENDING: 336 | err = fmt.Errorf("invalid transition from Preempting to Pending") 337 | break 338 | case actionlib_msgs.ACTIVE: 339 | err = fmt.Errorf("invalid transition from Preempting to Active") 340 | break 341 | case actionlib_msgs.REJECTED: 342 | err = fmt.Errorf("invalid transition from Preempting to Rejected") 343 | break 344 | case actionlib_msgs.RECALLING: 345 | err = fmt.Errorf("invalid transition from Preempting to Recalling") 346 | break 347 | case actionlib_msgs.RECALLED: 348 | err = fmt.Errorf("invalid transition from Preempting to Recalled") 349 | break 350 | case actionlib_msgs.PREEMPTED: 351 | stateList.PushBack(WaitingForResult) 352 | break 353 | case actionlib_msgs.SUCCEEDED: 354 | stateList.PushBack(WaitingForResult) 355 | break 356 | case actionlib_msgs.ABORTED: 357 | stateList.PushBack(WaitingForResult) 358 | break 359 | case actionlib_msgs.PREEMPTING: 360 | break 361 | } 362 | break 363 | case Done: 364 | switch status { 365 | case actionlib_msgs.PENDING: 366 | err = fmt.Errorf("invalid transition from Done to Pending") 367 | break 368 | case actionlib_msgs.ACTIVE: 369 | err = fmt.Errorf("invalid transition from Done to Active") 370 | break 371 | case actionlib_msgs.REJECTED: 372 | break 373 | case actionlib_msgs.RECALLING: 374 | err = fmt.Errorf("invalid transition from Done to Recalling") 375 | break 376 | case actionlib_msgs.RECALLED: 377 | break 378 | case actionlib_msgs.PREEMPTED: 379 | break 380 | case actionlib_msgs.SUCCEEDED: 381 | break 382 | case actionlib_msgs.ABORTED: 383 | break 384 | case actionlib_msgs.PREEMPTING: 385 | err = fmt.Errorf("invalid transition from Done to Preempting") 386 | break 387 | } 388 | break 389 | } 390 | 391 | return 392 | } 393 | -------------------------------------------------------------------------------- /actionlib/goal_id_gen.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "fmt" 5 | "sync" 6 | 7 | "github.com/fetchrobotics/rosgo/ros" 8 | ) 9 | 10 | type goalIDGenerator struct { 11 | goals int 12 | goalsMutex sync.RWMutex 13 | nodeName string 14 | } 15 | 16 | func newGoalIDGenerator(nodeName string) *goalIDGenerator { 17 | return &goalIDGenerator{ 18 | nodeName: nodeName, 19 | } 20 | } 21 | 22 | func (g *goalIDGenerator) generateID() string { 23 | g.goalsMutex.Lock() 24 | defer g.goalsMutex.Unlock() 25 | 26 | g.goals++ 27 | 28 | timeNow := ros.Now() 29 | return fmt.Sprintf("%s-%d-%d-%d", g.nodeName, g.goals, timeNow.Sec, timeNow.NSec) 30 | } 31 | -------------------------------------------------------------------------------- /actionlib/server_goal_handler.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | "fmt" 6 | "hash/fnv" 7 | "sync" 8 | 9 | "github.com/fetchrobotics/rosgo/ros" 10 | ) 11 | 12 | type serverGoalHandler struct { 13 | as ActionServer 14 | sm *serverStateMachine 15 | goal ActionGoal 16 | handlerDestructionTime ros.Time 17 | handlerMutex sync.RWMutex 18 | } 19 | 20 | func newServerGoalHandlerWithGoal(as ActionServer, goal ActionGoal) *serverGoalHandler { 21 | return &serverGoalHandler{ 22 | as: as, 23 | sm: newServerStateMachine(goal.GetGoalId()), 24 | goal: goal, 25 | } 26 | } 27 | 28 | func newServerGoalHandlerWithGoalId(as ActionServer, goalID *actionlib_msgs.GoalID) *serverGoalHandler { 29 | return &serverGoalHandler{ 30 | as: as, 31 | sm: newServerStateMachine(*goalID), 32 | } 33 | } 34 | 35 | func (gh *serverGoalHandler) GetHandlerDestructionTime() ros.Time { 36 | gh.handlerMutex.RLock() 37 | defer gh.handlerMutex.RUnlock() 38 | 39 | return gh.handlerDestructionTime 40 | } 41 | 42 | func (gh *serverGoalHandler) SetHandlerDestructionTime(t ros.Time) { 43 | gh.handlerMutex.Lock() 44 | defer gh.handlerMutex.Unlock() 45 | 46 | gh.handlerDestructionTime = t 47 | } 48 | 49 | func (gh *serverGoalHandler) SetAccepted(text string) error { 50 | if gh.goal == nil { 51 | return fmt.Errorf("attempt to set handler on an uninitialized handler") 52 | } 53 | 54 | if status, err := gh.sm.transition(Accept, text); err != nil { 55 | return fmt.Errorf("to transition to an active state, the goal must be in a pending"+ 56 | "or recalling state, it is currently in state: %d", status.Status) 57 | } 58 | 59 | gh.as.PublishStatus() 60 | 61 | return nil 62 | } 63 | 64 | func (gh *serverGoalHandler) SetCancelled(result ros.Message, text string) error { 65 | if gh.goal == nil { 66 | return fmt.Errorf("attempt to set handler on an uninitialized handler handler") 67 | } 68 | 69 | status, err := gh.sm.transition(Cancel, text) 70 | if err != nil { 71 | return fmt.Errorf("to transition to an Canceled state, the goal must be in a pending"+ 72 | " or recalling state, it is currently in state: %d", status.Status) 73 | } 74 | 75 | gh.SetHandlerDestructionTime(ros.Now()) 76 | gh.as.PublishResult(status, result) 77 | 78 | return nil 79 | } 80 | 81 | func (gh *serverGoalHandler) SetRejected(result ros.Message, text string) error { 82 | if gh.goal == nil { 83 | return fmt.Errorf("attempt to set handler on an uninitialized handler handler") 84 | } 85 | 86 | status, err := gh.sm.transition(Reject, text) 87 | if err != nil { 88 | return fmt.Errorf("to transition to an Rejected state, the goal must be in a pending"+ 89 | "or recalling state, it is currently in state: %d", status.Status) 90 | } 91 | 92 | gh.SetHandlerDestructionTime(ros.Now()) 93 | gh.as.PublishResult(status, result) 94 | 95 | return nil 96 | } 97 | 98 | func (gh *serverGoalHandler) SetAborted(result ros.Message, text string) error { 99 | if gh.goal == nil { 100 | return fmt.Errorf("attempt to set handler on an uninitialized handler handler") 101 | } 102 | 103 | status, err := gh.sm.transition(Abort, text) 104 | if err != nil { 105 | return fmt.Errorf("to transition to an Aborted state, the goal must be in a pending"+ 106 | "or recalling state, it is currently in state: %d", status.Status) 107 | } 108 | 109 | gh.SetHandlerDestructionTime(ros.Now()) 110 | gh.as.PublishResult(status, result) 111 | 112 | return nil 113 | } 114 | 115 | func (gh *serverGoalHandler) SetSucceeded(result ros.Message, text string) error { 116 | if gh.goal == nil { 117 | return fmt.Errorf("attempt to set handler on an uninitialized handler handler") 118 | } 119 | 120 | status, err := gh.sm.transition(Succeed, text) 121 | if err != nil { 122 | return fmt.Errorf("to transition to an Succeeded state, the goal must be in a pending"+ 123 | "or recalling state, it is currently in state: %d", status.Status) 124 | } 125 | 126 | gh.SetHandlerDestructionTime(ros.Now()) 127 | gh.as.PublishResult(status, result) 128 | 129 | return nil 130 | } 131 | 132 | func (gh *serverGoalHandler) SetCancelRequested() bool { 133 | if gh.goal == nil { 134 | return false 135 | } 136 | 137 | if _, err := gh.sm.transition(CancelRequest, "Cancel requested"); err != nil { 138 | return false 139 | } 140 | 141 | gh.SetHandlerDestructionTime(ros.Now()) 142 | return true 143 | } 144 | 145 | func (gh *serverGoalHandler) PublishFeedback(feedback ros.Message) { 146 | gh.as.PublishFeedback(gh.sm.getStatus(), feedback) 147 | } 148 | 149 | func (gh *serverGoalHandler) GetGoal() ros.Message { 150 | if gh.goal == nil { 151 | return nil 152 | } 153 | 154 | return gh.goal.GetGoal() 155 | } 156 | 157 | func (gh *serverGoalHandler) GetGoalId() actionlib_msgs.GoalID { 158 | if gh.goal == nil { 159 | return actionlib_msgs.GoalID{} 160 | } 161 | 162 | return gh.goal.GetGoalId() 163 | } 164 | 165 | func (gh *serverGoalHandler) GetGoalStatus() actionlib_msgs.GoalStatus { 166 | status := gh.sm.getStatus() 167 | if status.Status != 0 && gh.goal != nil && gh.goal.GetGoalId().Id != "" { 168 | return status 169 | } 170 | 171 | return actionlib_msgs.GoalStatus{} 172 | } 173 | 174 | func (gh *serverGoalHandler) Equal(other ServerGoalHandler) bool { 175 | if gh.goal == nil || other == nil { 176 | return false 177 | } 178 | 179 | return gh.goal.GetGoalId().Id == other.GetGoalId().Id 180 | } 181 | 182 | func (gh *serverGoalHandler) NotEqual(other ServerGoalHandler) bool { 183 | return !gh.Equal(other) 184 | } 185 | 186 | func (gh *serverGoalHandler) Hash() uint32 { 187 | id := gh.goal.GetGoalId().Id 188 | hs := fnv.New32a() 189 | hs.Write([]byte(id)) 190 | 191 | return hs.Sum32() 192 | } 193 | -------------------------------------------------------------------------------- /actionlib/server_state_machine.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | "fmt" 6 | "sync" 7 | ) 8 | 9 | type Event uint8 10 | 11 | const ( 12 | CancelRequest Event = iota + 1 13 | Cancel 14 | Reject 15 | Accept 16 | Succeed 17 | Abort 18 | ) 19 | 20 | func (e Event) String() string { 21 | switch e { 22 | case CancelRequest: 23 | return "CANCEL_REQUEST" 24 | case Cancel: 25 | return "CANCEL" 26 | case Reject: 27 | return "REJECT" 28 | case Accept: 29 | return "ACCEPT" 30 | case Succeed: 31 | return "SUCCEED" 32 | case Abort: 33 | return "ABORT" 34 | default: 35 | return "UNKNOWN" 36 | } 37 | } 38 | 39 | type serverStateMachine struct { 40 | goalStatus actionlib_msgs.GoalStatus 41 | mutex sync.RWMutex 42 | } 43 | 44 | func newServerStateMachine(goalID actionlib_msgs.GoalID) *serverStateMachine { 45 | return &serverStateMachine{ 46 | goalStatus: actionlib_msgs.GoalStatus{ 47 | GoalId: goalID, 48 | Status: actionlib_msgs.PENDING, 49 | }, 50 | } 51 | } 52 | 53 | func (sm *serverStateMachine) transition(event Event, text string) (actionlib_msgs.GoalStatus, error) { 54 | sm.mutex.Lock() 55 | defer sm.mutex.Unlock() 56 | 57 | nextState := sm.goalStatus.Status 58 | 59 | switch sm.goalStatus.Status { 60 | case actionlib_msgs.PENDING: 61 | switch event { 62 | case Reject: 63 | nextState = actionlib_msgs.REJECTED 64 | break 65 | case CancelRequest: 66 | nextState = actionlib_msgs.RECALLING 67 | break 68 | case Cancel: 69 | nextState = actionlib_msgs.RECALLED 70 | break 71 | case Accept: 72 | nextState = actionlib_msgs.ACTIVE 73 | break 74 | default: 75 | return sm.goalStatus, fmt.Errorf("invalid transition Event") 76 | } 77 | 78 | case actionlib_msgs.RECALLING: 79 | switch event { 80 | case Reject: 81 | nextState = actionlib_msgs.REJECTED 82 | break 83 | case Cancel: 84 | nextState = actionlib_msgs.RECALLED 85 | break 86 | case Accept: 87 | nextState = actionlib_msgs.PREEMPTING 88 | break 89 | default: 90 | return sm.goalStatus, fmt.Errorf("invalid transition Event") 91 | } 92 | 93 | case actionlib_msgs.ACTIVE: 94 | switch event { 95 | case Succeed: 96 | nextState = actionlib_msgs.SUCCEEDED 97 | break 98 | case CancelRequest: 99 | nextState = actionlib_msgs.PREEMPTING 100 | break 101 | case Cancel: 102 | nextState = actionlib_msgs.PREEMPTED 103 | break 104 | case Abort: 105 | nextState = actionlib_msgs.ABORTED 106 | break 107 | default: 108 | return sm.goalStatus, fmt.Errorf("invalid transition Event") 109 | } 110 | 111 | case actionlib_msgs.PREEMPTING: 112 | switch event { 113 | case Succeed: 114 | nextState = actionlib_msgs.SUCCEEDED 115 | break 116 | case Cancel: 117 | nextState = actionlib_msgs.PREEMPTED 118 | break 119 | case Abort: 120 | nextState = actionlib_msgs.ABORTED 121 | break 122 | default: 123 | return sm.goalStatus, fmt.Errorf("invalid transition Event") 124 | } 125 | case actionlib_msgs.REJECTED: 126 | break 127 | case actionlib_msgs.RECALLED: 128 | break 129 | case actionlib_msgs.SUCCEEDED: 130 | break 131 | case actionlib_msgs.PREEMPTED: 132 | break 133 | case actionlib_msgs.ABORTED: 134 | break 135 | default: 136 | return sm.goalStatus, fmt.Errorf("invalid state") 137 | } 138 | 139 | sm.goalStatus.Status = nextState 140 | sm.goalStatus.Text = text 141 | 142 | return sm.goalStatus, nil 143 | } 144 | 145 | func (sm *serverStateMachine) getStatus() actionlib_msgs.GoalStatus { 146 | sm.mutex.RLock() 147 | defer sm.mutex.RUnlock() 148 | 149 | return sm.goalStatus 150 | } 151 | -------------------------------------------------------------------------------- /actionlib/simple_action_client.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | "fmt" 6 | "reflect" 7 | "time" 8 | 9 | "github.com/fetchrobotics/rosgo/ros" 10 | ) 11 | 12 | const ( 13 | SimpleStatePending uint8 = 0 14 | SimpleStateActive uint8 = 1 15 | SimpleStateDone uint8 = 2 16 | ) 17 | 18 | type simpleActionClient struct { 19 | ac *defaultActionClient 20 | simpleState uint8 21 | gh ClientGoalHandler 22 | doneCb interface{} 23 | activeCb interface{} 24 | feedbackCb interface{} 25 | doneChan chan struct{} 26 | logger ros.Logger 27 | } 28 | 29 | func newSimpleActionClient(node ros.Node, action string, actionType ActionType) *simpleActionClient { 30 | return &simpleActionClient{ 31 | ac: newDefaultActionClient(node, action, actionType), 32 | simpleState: SimpleStateDone, 33 | doneChan: make(chan struct{}, 10), 34 | logger: node.Logger(), 35 | } 36 | } 37 | 38 | func (sc *simpleActionClient) SendGoal(goal ros.Message, doneCb, activeCb, feedbackCb interface{}) { 39 | sc.StopTrackingGoal() 40 | sc.doneCb = doneCb 41 | sc.activeCb = activeCb 42 | sc.feedbackCb = feedbackCb 43 | 44 | sc.setSimpleState(SimpleStatePending) 45 | sc.gh = sc.ac.SendGoal(goal, sc.transitionHandler, sc.feedbackHandler) 46 | } 47 | 48 | func (sc *simpleActionClient) SendGoalAndWait(goal ros.Message, executeTimeout, preeptTimeout ros.Duration) (uint8, error) { 49 | sc.SendGoal(goal, nil, nil, nil) 50 | if !sc.WaitForResult(executeTimeout) { 51 | sc.logger.Debug("Cancelling goal") 52 | sc.CancelGoal() 53 | if sc.WaitForResult(preeptTimeout) { 54 | sc.logger.Debug("Preempt finished within specified timeout") 55 | } else { 56 | sc.logger.Debug("Preempt did not finish within specified timeout") 57 | } 58 | } 59 | 60 | return sc.GetState() 61 | } 62 | 63 | func (sc *simpleActionClient) WaitForServer(timeout ros.Duration) bool { 64 | return sc.ac.WaitForServer(timeout) 65 | } 66 | 67 | func (sc *simpleActionClient) WaitForResult(timeout ros.Duration) bool { 68 | if sc.gh == nil { 69 | sc.logger.Errorf("[SimpleActionClient] Called WaitForResult when no goal exists") 70 | return false 71 | } 72 | 73 | waitStart := ros.Now() 74 | waitStart = waitStart.Add(timeout) 75 | 76 | LOOP: 77 | for { 78 | select { 79 | case <-sc.doneChan: 80 | break LOOP 81 | case <-time.After(100 * time.Millisecond): 82 | } 83 | 84 | if !timeout.IsZero() && waitStart.Cmp(ros.Now()) <= 0 { 85 | break LOOP 86 | } 87 | } 88 | 89 | return sc.simpleState == SimpleStateDone 90 | } 91 | 92 | func (sc *simpleActionClient) GetResult() (ros.Message, error) { 93 | if sc.gh == nil { 94 | return nil, fmt.Errorf("called get result when no goal running") 95 | } 96 | 97 | return sc.gh.GetResult() 98 | } 99 | 100 | func (sc *simpleActionClient) GetState() (uint8, error) { 101 | if sc.gh == nil { 102 | return actionlib_msgs.LOST, fmt.Errorf("called get state when no goal running") 103 | } 104 | 105 | status, err := sc.gh.GetGoalStatus() 106 | if err != nil { 107 | return actionlib_msgs.LOST, err 108 | } 109 | 110 | if status == actionlib_msgs.RECALLING { 111 | status = actionlib_msgs.PENDING 112 | } else if status == actionlib_msgs.PREEMPTING { 113 | status = actionlib_msgs.ACTIVE 114 | } 115 | 116 | return status, nil 117 | } 118 | 119 | func (sc *simpleActionClient) GetGoalStatusText() (string, error) { 120 | if sc.gh == nil { 121 | return "", fmt.Errorf("called GetGoalStatusText when no goal is running") 122 | } 123 | 124 | return sc.gh.GetGoalStatusText() 125 | } 126 | 127 | func (sc *simpleActionClient) CancelAllGoals() { 128 | sc.ac.CancelAllGoals() 129 | } 130 | 131 | func (sc *simpleActionClient) CancelAllGoalsBeforeTime(stamp ros.Time) { 132 | sc.ac.CancelAllGoalsBeforeTime(stamp) 133 | } 134 | 135 | func (sc *simpleActionClient) CancelGoal() error { 136 | if sc.gh == nil { 137 | return nil 138 | } 139 | 140 | return sc.gh.Cancel() 141 | } 142 | 143 | func (sc *simpleActionClient) StopTrackingGoal() { 144 | sc.gh = nil 145 | } 146 | 147 | func (sc *simpleActionClient) transitionHandler(gh ClientGoalHandler) { 148 | commState, err := gh.GetCommState() 149 | if err != nil { 150 | sc.logger.Errorf("Error getting CommState: %v", err) 151 | return 152 | } 153 | 154 | errMsg := fmt.Errorf("received comm state %s when in simple state %d with SimpleActionClient in NS %s", 155 | commState, sc.simpleState, sc.ac.node.Name()) 156 | 157 | var callbackType string 158 | var args []reflect.Value 159 | switch commState { 160 | case Active: 161 | switch sc.simpleState { 162 | case SimpleStatePending: 163 | sc.setSimpleState(SimpleStateActive) 164 | callbackType = "active" 165 | 166 | case SimpleStateDone: 167 | sc.logger.Errorf("[SimpleActionClient] %v", errMsg) 168 | } 169 | 170 | case Recalling: 171 | switch sc.simpleState { 172 | case SimpleStateActive, SimpleStateDone: 173 | sc.logger.Errorf("[SimpleActionClient] %v", errMsg) 174 | } 175 | 176 | case Preempting: 177 | switch sc.simpleState { 178 | case SimpleStatePending: 179 | sc.setSimpleState(SimpleStateActive) 180 | callbackType = "active" 181 | 182 | case SimpleStateDone: 183 | sc.logger.Errorf("[SimpleActionClient] %v", errMsg) 184 | } 185 | 186 | case Done: 187 | switch sc.simpleState { 188 | case SimpleStatePending, SimpleStateActive: 189 | sc.setSimpleState(SimpleStateDone) 190 | sc.sendDone() 191 | 192 | if sc.doneCb == nil { 193 | break 194 | } 195 | 196 | status, err := gh.GetGoalStatus() 197 | if err != nil { 198 | sc.logger.Errorf("[SimpleActionClient] Error getting status: %v", err) 199 | break 200 | } 201 | 202 | result, err := gh.GetResult() 203 | if err != nil { 204 | sc.logger.Errorf("[SimpleActionClient] Error getting result: %v", err) 205 | break 206 | } 207 | 208 | callbackType = "done" 209 | args = append(args, reflect.ValueOf(status)) 210 | args = append(args, reflect.ValueOf(result)) 211 | 212 | case SimpleStateDone: 213 | sc.logger.Errorf("[SimpleActionClient] received DONE twice") 214 | } 215 | } 216 | 217 | if len(callbackType) > 0 { 218 | sc.runCallback(callbackType, args) 219 | } 220 | } 221 | 222 | func (sc *simpleActionClient) sendDone() { 223 | select { 224 | case sc.doneChan <- struct{}{}: 225 | default: 226 | sc.logger.Errorf("[SimpleActionClient] Error sending done notification. Channel full.") 227 | } 228 | } 229 | 230 | func (sc *simpleActionClient) feedbackHandler(gh ClientGoalHandler, msg ros.Message) { 231 | if sc.gh == nil || sc.gh != gh { 232 | return 233 | } 234 | 235 | sc.runCallback("feedback", []reflect.Value{reflect.ValueOf(msg)}) 236 | } 237 | 238 | func (sc *simpleActionClient) setSimpleState(state uint8) { 239 | sc.logger.Debugf("[SimpleActionClient] Transitioning from %d to %d", sc.simpleState, state) 240 | sc.simpleState = state 241 | } 242 | 243 | func (sc *simpleActionClient) runCallback(cbType string, args []reflect.Value) { 244 | var callback interface{} 245 | switch cbType { 246 | case "active": 247 | callback = sc.activeCb 248 | case "feedback": 249 | callback = sc.feedbackCb 250 | case "done": 251 | callback = sc.doneCb 252 | default: 253 | sc.logger.Errorf("[SimpleActionClient] Unknown callback %s", cbType) 254 | } 255 | 256 | if callback == nil { 257 | return 258 | } 259 | 260 | fun := reflect.ValueOf(callback) 261 | numArgsNeeded := fun.Type().NumIn() 262 | 263 | if numArgsNeeded > len(args) { 264 | sc.logger.Errorf("[SimpleActionClient] Unexpected arguments:"+ 265 | "callback %s expects %d arguments but %d arguments provided", cbType, numArgsNeeded, len(args)) 266 | return 267 | } 268 | 269 | sc.logger.Debugf("[SimpleActionClient] Calling %s callback with %d arguments", cbType, len(args)) 270 | 271 | fun.Call(args[0:numArgsNeeded]) 272 | } 273 | -------------------------------------------------------------------------------- /actionlib/simple_action_server.go: -------------------------------------------------------------------------------- 1 | package actionlib 2 | 3 | import ( 4 | "actionlib_msgs" 5 | "fmt" 6 | "reflect" 7 | "sync" 8 | "time" 9 | 10 | "github.com/fetchrobotics/rosgo/ros" 11 | ) 12 | 13 | type simpleActionServer struct { 14 | actionServer *defaultActionServer 15 | currentGoal *serverGoalHandler 16 | nextGoal *serverGoalHandler 17 | goalMutex sync.RWMutex 18 | newGoal bool 19 | preemptRequest bool 20 | newGoalPreemptRequest bool 21 | logger ros.Logger 22 | goalCallback interface{} 23 | preemptCallback interface{} 24 | executeCb interface{} 25 | executorCh chan struct{} 26 | } 27 | 28 | func newSimpleActionServer(node ros.Node, action string, actType ActionType, executeCb interface{}, start bool) *simpleActionServer { 29 | s := new(simpleActionServer) 30 | s.actionServer = newDefaultActionServer(node, action, actType, s.internalGoalCallback, s.internalPreemptCallback, start) 31 | s.newGoal = false 32 | s.preemptRequest = false 33 | s.newGoalPreemptRequest = false 34 | s.executeCb = executeCb 35 | s.logger = node.Logger() 36 | s.executorCh = make(chan struct{}, 100) 37 | return s 38 | } 39 | 40 | func (s *simpleActionServer) Start() { 41 | if s.executeCb != nil { 42 | go s.goalExecutor() 43 | } 44 | 45 | go s.actionServer.Start() 46 | } 47 | 48 | func (s *simpleActionServer) IsNewGoalAvailable() bool { 49 | s.goalMutex.Lock() 50 | defer s.goalMutex.Unlock() 51 | 52 | return s.newGoal 53 | } 54 | 55 | func (s *simpleActionServer) IsPreemptRequested() bool { 56 | s.goalMutex.Lock() 57 | defer s.goalMutex.Unlock() 58 | 59 | return s.preemptRequest 60 | } 61 | 62 | func (s *simpleActionServer) AcceptNewGoal() (ros.Message, error) { 63 | s.goalMutex.Lock() 64 | defer s.goalMutex.Unlock() 65 | 66 | if !s.newGoal || s.nextGoal == nil { 67 | return nil, fmt.Errorf("attempting to accept the next goal when a new goal is not available") 68 | } 69 | 70 | // check if we need to send a preempted message for the goal that we're currently pursuing 71 | if s.IsActive() && s.currentGoal != nil && s.currentGoal.NotEqual(s.nextGoal) { 72 | s.currentGoal.SetCancelled(s.GetDefaultResult(), 73 | "This goal was canceled because another goal was received by the simple action server") 74 | } 75 | 76 | s.logger.Debug("Accepting a new goal") 77 | 78 | // accept the next goal 79 | s.currentGoal = s.nextGoal 80 | s.newGoal = false 81 | 82 | // set preempt to request to equal the preempt state of the new goal 83 | s.preemptRequest = s.newGoalPreemptRequest 84 | s.newGoalPreemptRequest = false 85 | 86 | // set the status of the current goal to be active 87 | s.currentGoal.SetAccepted("This goal has been accepted by the simple action server") 88 | 89 | return s.currentGoal.GetGoal(), nil 90 | } 91 | 92 | func (s *simpleActionServer) IsActive() bool { 93 | if s.currentGoal == nil || s.currentGoal.GetGoalId().Id == "" { 94 | return false 95 | } 96 | 97 | status := s.currentGoal.GetGoalStatus().Status 98 | if status == actionlib_msgs.ACTIVE || status == actionlib_msgs.PREEMPTING { 99 | return true 100 | } 101 | 102 | return false 103 | } 104 | 105 | func (s *simpleActionServer) SetSucceeded(result ros.Message, text string) error { 106 | s.goalMutex.Lock() 107 | defer s.goalMutex.Unlock() 108 | 109 | if result == nil { 110 | result = s.GetDefaultResult() 111 | } 112 | 113 | return s.currentGoal.SetSucceeded(result, text) 114 | } 115 | 116 | func (s *simpleActionServer) SetAborted(result ros.Message, text string) error { 117 | s.goalMutex.Lock() 118 | defer s.goalMutex.Unlock() 119 | 120 | if result == nil { 121 | result = s.GetDefaultResult() 122 | } 123 | 124 | return s.currentGoal.SetAborted(result, text) 125 | } 126 | 127 | func (s *simpleActionServer) SetPreempted(result ros.Message, text string) error { 128 | s.goalMutex.Lock() 129 | defer s.goalMutex.Unlock() 130 | 131 | if result == nil { 132 | result = s.GetDefaultResult() 133 | } 134 | 135 | return s.currentGoal.SetCancelled(result, text) 136 | } 137 | 138 | func (s *simpleActionServer) PublishFeedback(feedback ros.Message) { 139 | s.goalMutex.Lock() 140 | defer s.goalMutex.Unlock() 141 | 142 | s.currentGoal.PublishFeedback(feedback) 143 | } 144 | 145 | func (s *simpleActionServer) GetDefaultResult() ros.Message { 146 | return s.actionServer.actionResultType.NewMessage() 147 | } 148 | 149 | func (s *simpleActionServer) RegisterGoalCallback(cb interface{}) error { 150 | if s.executeCb != nil { 151 | return fmt.Errorf("execute callback if present: not registering goal callback") 152 | } 153 | 154 | s.goalCallback = cb 155 | 156 | return nil 157 | } 158 | 159 | func (s *simpleActionServer) RegisterPreemptCallback(cb interface{}) { 160 | s.preemptCallback = cb 161 | } 162 | 163 | func (s *simpleActionServer) internalGoalCallback(ag ActionGoal) { 164 | goalHandler := s.actionServer.getHandler(ag.GetGoalId().Id) 165 | s.logger.Infof("[SimpleActionServer] Server received new goal with id %s", goalHandler.GetGoalId().Id) 166 | 167 | var goalStamp, nextGoalStamp ros.Time 168 | goalStamp = goalHandler.GetGoalId().Stamp 169 | if s.nextGoal != nil { 170 | nextGoalStamp = s.nextGoal.GetGoalId().Stamp 171 | } 172 | 173 | s.goalMutex.Lock() 174 | defer s.goalMutex.Unlock() 175 | 176 | if (s.currentGoal == nil || goalStamp.Cmp(s.currentGoal.GetGoalId().Stamp) >= 0) && 177 | (s.nextGoal == nil || nextGoalStamp.Cmp(s.currentGoal.GetGoalId().Stamp) >= 0) { 178 | 179 | if (s.nextGoal != nil) && 180 | (s.currentGoal == nil || s.nextGoal.NotEqual(s.currentGoal)) { 181 | s.nextGoal.SetCancelled(s.GetDefaultResult(), 182 | "This goal was canceled because another goal was received by the simple action server") 183 | } 184 | 185 | s.nextGoal = goalHandler 186 | s.newGoal = true 187 | s.newGoalPreemptRequest = false 188 | goal := goalHandler.GetGoal() 189 | args := []reflect.Value{reflect.ValueOf(goal)} 190 | 191 | if s.IsActive() { 192 | s.preemptRequest = true 193 | if err := s.runCallback("preempt", args); err != nil { 194 | s.logger.Error(err) 195 | } 196 | } 197 | 198 | if err := s.runCallback("goal", args); err != nil { 199 | s.logger.Error(err) 200 | } 201 | 202 | // notify executor that a new goal is available 203 | select { 204 | case s.executorCh <- struct{}{}: 205 | default: 206 | s.logger.Error("[SimpleActionServer] Exectuor new goal notification error: Channel full.") 207 | } 208 | } else { 209 | goalHandler.SetCancelled(s.GetDefaultResult(), 210 | "This goal was canceled because another goal was received by the simple action server") 211 | } 212 | } 213 | 214 | func (s *simpleActionServer) internalPreemptCallback(gID *actionlib_msgs.GoalID) { 215 | s.goalMutex.Lock() 216 | defer s.goalMutex.Unlock() 217 | 218 | goalHandler := s.actionServer.getHandler(gID.Id) 219 | s.logger.Infof("[SimpleActionServer] Server received preempt call for goal with id %s", 220 | goalHandler.GetGoalId().Id) 221 | 222 | if goalHandler.GetGoalId().Id == s.currentGoal.GetGoalId().Id { 223 | s.preemptRequest = true 224 | goal := goalHandler.GetGoal() 225 | args := []reflect.Value{reflect.ValueOf(goal)} 226 | if err := s.runCallback("preempt", args); err != nil { 227 | s.logger.Error(err) 228 | } 229 | } else { 230 | s.newGoalPreemptRequest = true 231 | } 232 | } 233 | 234 | func (s *simpleActionServer) goalExecutor() { 235 | intervalCh := time.NewTicker(1 * time.Second) 236 | defer intervalCh.Stop() 237 | 238 | for s.actionServer.node.OK() { 239 | select { 240 | case <-s.executorCh: 241 | if err := s.execute(); err != nil { 242 | s.logger.Error(err) 243 | return 244 | } 245 | 246 | case <-intervalCh.C: 247 | if err := s.execute(); err != nil { 248 | s.logger.Error(err) 249 | return 250 | } 251 | } 252 | } 253 | } 254 | 255 | func (s *simpleActionServer) execute() error { 256 | if s.IsActive() { 257 | return fmt.Errorf("should never reach this code with an active goal") 258 | } 259 | 260 | if s.IsNewGoalAvailable() { 261 | goal, err := s.AcceptNewGoal() 262 | if err != nil { 263 | return err 264 | } 265 | 266 | if s.executeCb == nil { 267 | return fmt.Errorf("execute callback must exist. This is a bug in SimpleActionServer") 268 | } 269 | 270 | args := []reflect.Value{reflect.ValueOf(goal)} 271 | if err := s.runCallback("execute", args); err != nil { 272 | return err 273 | } 274 | 275 | if s.IsActive() { 276 | s.logger.Warn("Your executeCallback did not set the goal to a terminal status." + 277 | "This is a bug in your ActionServer implementation. Fix your code!" + 278 | "For now, the ActionServer will set this goal to aborted") 279 | if err := s.SetAborted(nil, ""); err != nil { 280 | return err 281 | } 282 | } 283 | } 284 | 285 | return nil 286 | } 287 | 288 | func (s *simpleActionServer) runCallback(cbType string, args []reflect.Value) error { 289 | var callback interface{} 290 | switch cbType { 291 | case "goal": 292 | callback = s.goalCallback 293 | case "preempt": 294 | callback = s.preemptCallback 295 | case "execute": 296 | callback = s.executeCb 297 | default: 298 | return fmt.Errorf("unknown callback type called") 299 | } 300 | 301 | if callback == nil { 302 | return nil 303 | } 304 | 305 | fun := reflect.ValueOf(callback) 306 | numArgsNeeded := fun.Type().NumIn() 307 | 308 | if numArgsNeeded <= 1 { 309 | fun.Call(args[0:numArgsNeeded]) 310 | } else { 311 | return fmt.Errorf("unexepcted number of arguments for callback") 312 | } 313 | 314 | return nil 315 | } 316 | -------------------------------------------------------------------------------- /gengo/generate_test.go: -------------------------------------------------------------------------------- 1 | // Copyright 2018, Akio Ochiai All rights reserved 2 | package main 3 | 4 | import ( 5 | // "fmt" 6 | // "math" 7 | "os" 8 | "strings" 9 | "testing" 10 | ) 11 | 12 | func TestGenerateBadAction(t *testing.T) { 13 | const text string = ` 14 | # Comment 15 | bool B = 1 16 | int8 I8 = -128 17 | int16 I16 = -32768 18 | int32 I32 = -2147483648 19 | int64 I64 = -9223372036854775808 20 | uint8 U8 = 255 21 | uint16 U16 = 65535 22 | uint32 U32 = 4294967295 23 | uint64 U64 = 18446744073709551615 24 | string S = Lorem Ipsum # Comment is ignored 25 | 26 | bool b 27 | int8 i8 28 | uint8 u8 29 | int16 i16 30 | uint16 u16 31 | int32 i32 32 | uint32 u32 33 | int64 i64 34 | uint64 u64 35 | float32 f32 36 | float64 f64 37 | --- 38 | string s 39 | time t 40 | duration d 41 | string[] sva 42 | string[42] sfa 43 | ` 44 | rosPkgPath := os.Getenv("ROS_PACKAGE_PATH") 45 | ctx, e := NewMsgContext(strings.Split(rosPkgPath, ":")) 46 | if e != nil { 47 | t.Errorf("Failed to create MsgContext.") 48 | } 49 | 50 | // var spec *ActionSpec 51 | _, e = ctx.LoadActionFromString(text, "foo/Foo") 52 | if e == nil { 53 | t.Errorf("Successfully parse bad action %v", e) 54 | } 55 | } 56 | 57 | func TestGenerateAction(t *testing.T) { 58 | const text string = ` 59 | # Comment 60 | bool B = 1 61 | int8 I8 = -128 62 | int16 I16 = -32768 63 | int32 I32 = -2147483648 64 | int64 I64 = -9223372036854775808 65 | uint8 U8 = 255 66 | uint16 U16 = 65535 67 | uint32 U32 = 4294967295 68 | uint64 U64 = 18446744073709551615 69 | string S = Lorem Ipsum # Comment is ignored 70 | 71 | bool b 72 | int8 i8 73 | uint8 u8 74 | int16 i16 75 | uint16 u16 76 | int32 i32 77 | uint32 u32 78 | int64 i64 79 | uint64 u64 80 | float32 f32 81 | float64 f64 82 | --- 83 | string s 84 | time t 85 | duration d 86 | string[] sva 87 | string[42] sfa 88 | --- 89 | std_msgs/Empty e 90 | std_msgs/Empty[] eva 91 | std_msgs/Empty[42] efa 92 | Bar x 93 | Bar[] xva 94 | Bar[42] xfa 95 | ` 96 | rosPkgPath := os.Getenv("ROS_PACKAGE_PATH") 97 | ctx, e := NewMsgContext(strings.Split(rosPkgPath, ":")) 98 | if e != nil { 99 | t.Errorf("Failed to create MsgContext.") 100 | } 101 | 102 | var spec *ActionSpec 103 | spec, e = ctx.LoadActionFromString(text, "foo/Foo") 104 | if e != nil { 105 | t.Errorf("Failed to parse: %v", e) 106 | } 107 | 108 | // action, _, _, _, err := GenerateAction(ctx, spec) 109 | _, _, _, _, _, _, _, err := GenerateAction(ctx, spec) 110 | if err != nil { 111 | t.Errorf("Failed to generate message: %v", err) 112 | } 113 | } 114 | 115 | func TestGenerateMessage(t *testing.T) { 116 | const text string = ` 117 | # Comment 118 | bool B = 1 119 | int8 I8 = -128 120 | int16 I16 = -32768 121 | int32 I32 = -2147483648 122 | int64 I64 = -9223372036854775808 123 | uint8 U8 = 255 124 | uint16 U16 = 65535 125 | uint32 U32 = 4294967295 126 | uint64 U64 = 18446744073709551615 127 | string S = Lorem Ipsum # Comment is ignored 128 | 129 | Header header 130 | bool b 131 | int8 i8 132 | uint8 u8 133 | int16 i16 134 | uint16 u16 135 | int32 i32 136 | uint32 u32 137 | int64 i64 138 | uint64 u64 139 | float32 f32 140 | float64 f64 141 | string s 142 | time t 143 | duration d 144 | string[] sva 145 | string[42] sfa 146 | std_msgs/Empty e 147 | std_msgs/Empty[] eva 148 | std_msgs/Empty[42] efa 149 | Bar x 150 | Bar[] xva 151 | Bar[42] xfa 152 | ` 153 | rosPkgPath := os.Getenv("ROS_PACKAGE_PATH") 154 | ctx, e := NewMsgContext(strings.Split(rosPkgPath, ":")) 155 | if e != nil { 156 | t.Errorf("Failed to create MsgContext.") 157 | } 158 | 159 | var spec *MsgSpec 160 | spec, e = ctx.LoadMsgFromString(text, "foo/Foo") 161 | if e != nil { 162 | t.Errorf("Failed to parse: %v", e) 163 | } 164 | 165 | _, err := GenerateMessage(ctx, spec) 166 | if err != nil { 167 | t.Errorf("Failed to generate message: %v", err) 168 | } 169 | } 170 | -------------------------------------------------------------------------------- /gengo/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "flag" 5 | "fmt" 6 | "go/format" 7 | "io/ioutil" 8 | "os" 9 | "path/filepath" 10 | "strings" 11 | ) 12 | 13 | var ( 14 | out = flag.String("out", "vendor", "Directory to generate files in") 15 | importPath = flag.String("import_path", "", "Specify import path/prefix for nested types") 16 | ) 17 | 18 | func writeCode(fullname string, code string) error { 19 | nameComponents := strings.Split(fullname, "/") 20 | pkgDir := filepath.Join(*out, nameComponents[0]) 21 | if _, err := os.Stat(pkgDir); os.IsNotExist(err) { 22 | err = os.MkdirAll(pkgDir, os.ModeDir|os.FileMode(0775)) 23 | if err != nil { 24 | return err 25 | } 26 | } 27 | filename := filepath.Join(pkgDir, nameComponents[1]+".go") 28 | 29 | res, err := format.Source([]byte(code)) 30 | if err != nil { 31 | return fmt.Errorf("Error formatting generated code: %+v", err) 32 | } 33 | 34 | return ioutil.WriteFile(filename, res, os.FileMode(0664)) 35 | } 36 | 37 | func main() { 38 | flag.Parse() 39 | if _, err := os.Stat(*out); os.IsNotExist(err) { 40 | err = os.MkdirAll(*out, os.ModeDir|os.FileMode(0775)) 41 | if err != nil { 42 | fmt.Println(err) 43 | os.Exit(-1) 44 | } 45 | } 46 | 47 | if flag.NArg() < 2 { 48 | fmt.Println("USAGE: gengo [-out=] [-import_path=] msg|srv|action []") 49 | os.Exit(-1) 50 | } 51 | 52 | rosPkgPath := os.Getenv("ROS_PACKAGE_PATH") 53 | 54 | context, err := NewMsgContext(strings.Split(rosPkgPath, ":")) 55 | if err != nil { 56 | fmt.Println(err) 57 | os.Exit(-1) 58 | } 59 | mode := flag.Arg(0) 60 | fullname := flag.Arg(1) 61 | 62 | fmt.Printf("Generating %v...", fullname) 63 | 64 | if mode == "msg" { 65 | var spec *MsgSpec 66 | var err error 67 | if flag.NArg() == 2 { 68 | spec, err = context.LoadMsg(fullname) 69 | } else { 70 | spec, err = context.LoadMsgFromFile(flag.Arg(2), fullname) 71 | } 72 | if err != nil { 73 | fmt.Println(err) 74 | os.Exit(-1) 75 | } 76 | var code string 77 | code, err = GenerateMessage(context, spec, false) 78 | if err != nil { 79 | fmt.Println(err) 80 | os.Exit(-1) 81 | } 82 | err = writeCode(fullname, code) 83 | if err != nil { 84 | fmt.Println(err) 85 | os.Exit(-1) 86 | } 87 | } else if mode == "srv" { 88 | var spec *SrvSpec 89 | var err error 90 | if flag.NArg() == 2 { 91 | spec, err = context.LoadSrv(fullname) 92 | } else { 93 | spec, err = context.LoadSrvFromFile(flag.Arg(2), fullname) 94 | } 95 | if err != nil { 96 | fmt.Println(err) 97 | os.Exit(-1) 98 | } 99 | srvCode, reqCode, resCode, err := GenerateService(context, spec) 100 | if err != nil { 101 | fmt.Println(err) 102 | os.Exit(-1) 103 | } 104 | 105 | err = writeCode(fullname, srvCode) 106 | if err != nil { 107 | fmt.Println(err) 108 | os.Exit(-1) 109 | } 110 | 111 | err = writeCode(spec.Request.FullName, reqCode) 112 | if err != nil { 113 | fmt.Println(err) 114 | os.Exit(-1) 115 | } 116 | 117 | err = writeCode(spec.Response.FullName, resCode) 118 | if err != nil { 119 | fmt.Println(err) 120 | os.Exit(-1) 121 | } 122 | } else if mode == "action" { 123 | var spec *ActionSpec 124 | var err error 125 | 126 | if len(os.Args) == 3 { 127 | spec, err = context.LoadAction(fullname) 128 | } else { 129 | spec, err = context.LoadActionFromFile(os.Args[3], fullname) 130 | } 131 | if err != nil { 132 | fmt.Println(err) 133 | os.Exit(-1) 134 | } 135 | 136 | actionCode, codeMap, err := GenerateAction(context, spec) 137 | if err != nil { 138 | fmt.Println(err) 139 | os.Exit(-1) 140 | } 141 | 142 | err = writeCode(fullname, actionCode) 143 | if err != nil { 144 | fmt.Println(err) 145 | os.Exit(-1) 146 | } 147 | 148 | for name, code := range codeMap { 149 | err = writeCode(name, code) 150 | if err != nil { 151 | fmt.Println(err) 152 | os.Exit(-1) 153 | } 154 | } 155 | 156 | } else { 157 | fmt.Println("USAGE: gengo ") 158 | os.Exit(-1) 159 | } 160 | fmt.Println("Done") 161 | } 162 | -------------------------------------------------------------------------------- /gengo/msgs.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "bytes" 5 | "crypto/md5" 6 | "encoding/hex" 7 | "fmt" 8 | "regexp" 9 | "strconv" 10 | "strings" 11 | "unicode" 12 | ) 13 | 14 | const ( 15 | HeaderType = "Header" 16 | TimeType = "time" 17 | DurationType = "duration" 18 | HeaderFullName = "std_msgs/Header" 19 | TimeMsg = "uint32 secs\nuint32 nsecs" 20 | DurationMsg = "uint32 secs\nuint32 nsecs" 21 | ) 22 | 23 | var PrimitiveTypes = []string{ 24 | "int8", 25 | "uint8", "int16", "uint16", "int32", "uint32", "int64", "uint64", "float32", "float64", 26 | "string", 27 | "bool", 28 | // deprecated: 29 | "char", "byte", 30 | } 31 | 32 | var BuiltinTypes = append([]string{TimeType, DurationType}, PrimitiveTypes...) 33 | 34 | var ResourceNameLegalCharsPattern = regexp.MustCompile(`^[A-Za-z][\w_\/]*$`) 35 | 36 | var BaseResourceNameLegalCharsPattern = regexp.MustCompile(`"[A-Za-z][\w_]*$`) 37 | 38 | func isValidConsantType(t string) bool { 39 | for _, e := range PrimitiveTypes { 40 | if e == t { 41 | return true 42 | } 43 | } 44 | return false 45 | } 46 | 47 | func isValidMsgFieldName(name string) bool { 48 | return isLegalResourceBaseName(name) 49 | } 50 | 51 | func isLegalResourceBaseName(name string) bool { 52 | if strings.Contains(name, "//") { 53 | return false 54 | } 55 | return ResourceNameLegalCharsPattern.MatchString(name) 56 | } 57 | 58 | func isLegalResourceName(name string) bool { 59 | return BaseResourceNameLegalCharsPattern.MatchString(name) 60 | } 61 | 62 | func isPrimitiveType(name string) bool { 63 | for _, t := range PrimitiveTypes { 64 | if t == name { 65 | return true 66 | } 67 | } 68 | return false 69 | } 70 | 71 | func isBuiltinType(name string) bool { 72 | for _, t := range BuiltinTypes { 73 | if t == name { 74 | return true 75 | } 76 | } 77 | return false 78 | } 79 | 80 | func baseMsgType(t string) string { 81 | index := strings.Index(t, "[") 82 | if index < 0 { 83 | return t 84 | } else { 85 | return t[:index] 86 | } 87 | } 88 | 89 | func splitType(t string) (string, string) { 90 | components := strings.Split(t, "/") 91 | if len(components) == 1 { 92 | return "", t 93 | } else { 94 | return components[0], components[1] 95 | } 96 | } 97 | 98 | func parseType(msgType string) (pkg string, baseType string, isArray bool, arrayLen int, err error) { 99 | index := strings.Index(msgType, "[") 100 | if index < 0 { 101 | pkg, name := splitType(msgType) 102 | return pkg, name, false, 0, nil 103 | } else { 104 | if msgType[len(msgType)-1] == ']' { 105 | base := msgType[:index] 106 | rest := msgType[index:] 107 | pkg, name := splitType(base) 108 | if rest == "[]" { 109 | return pkg, name, true, -1, nil 110 | } else { 111 | value64, err := strconv.ParseInt(rest[1:len(rest)-1], 10, 32) 112 | if err != nil { 113 | return pkg, name, false, 0, err 114 | } 115 | value := int(value64) 116 | return pkg, name, true, value, nil 117 | } 118 | } else { 119 | return "", msgType, false, 0, fmt.Errorf("missing ']'") 120 | } 121 | } 122 | } 123 | 124 | func isValidMsgType(t string) bool { 125 | if t != strings.TrimSpace(t) { 126 | return false 127 | } 128 | base := baseMsgType(t) 129 | if !isLegalResourceBaseName(base) { 130 | return false 131 | } 132 | 133 | x := t[len(base):] 134 | state := 0 135 | for _, c := range x { 136 | if state == 0 { 137 | if c != '[' { 138 | return false 139 | } 140 | state = 1 141 | } else if state == 1 { 142 | if c == ']' { 143 | state = 0 144 | } else if !unicode.IsDigit(c) { 145 | return false 146 | } 147 | } 148 | } 149 | return state == 0 150 | } 151 | 152 | func isValidConstantType(t string) bool { 153 | for _, pt := range PrimitiveTypes { 154 | if t == pt { 155 | return true 156 | } 157 | } 158 | return false 159 | } 160 | 161 | func isHeaderType(name string) bool { 162 | patterns := map[string]bool{ 163 | HeaderType: true, 164 | HeaderFullName: true, 165 | "roslib/Header": true, 166 | } 167 | return patterns[name] 168 | } 169 | 170 | type Constant struct { 171 | Type string 172 | Name string 173 | Value interface{} 174 | ValueText string 175 | GoName string 176 | } 177 | 178 | func ToGoType(pkg string, typeName string) string { 179 | var goType string 180 | switch typeName { 181 | case "int8": 182 | goType = "int8" 183 | case "uint8": 184 | goType = "uint8" 185 | case "int16": 186 | goType = "int16" 187 | case "uint16": 188 | goType = "uint16" 189 | case "int32": 190 | goType = "int32" 191 | case "uint32": 192 | goType = "uint32" 193 | case "int64": 194 | goType = "int64" 195 | case "uint64": 196 | goType = "uint64" 197 | case "float32": 198 | goType = "float32" 199 | case "float64": 200 | goType = "float64" 201 | case "string": 202 | goType = "string" 203 | case "bool": 204 | goType = "bool" 205 | case "char": 206 | goType = "uint8" 207 | case "byte": 208 | goType = "uint8" 209 | case "time": 210 | goType = "ros.Time" 211 | case "duration": 212 | goType = "ros.Duration" 213 | default: 214 | goType = pkg + "." + typeName 215 | } 216 | return goType 217 | } 218 | 219 | func ToGoName(name string, constant bool) string { 220 | if constant { 221 | return strings.ToUpper(name) 222 | } 223 | 224 | var buffer []string 225 | words := strings.Split(name, "_") 226 | for _, word := range words { 227 | head := strings.ToUpper(word[:1]) 228 | tail := "" 229 | if len(word) > 1 { 230 | tail = word[1:] 231 | } 232 | buffer = append(append(buffer, head), tail) 233 | } 234 | return strings.Join(buffer, "") 235 | } 236 | 237 | func GetZeroValue(pkg string, typeName string) string { 238 | var zeroValue string 239 | switch typeName { 240 | case "int8": 241 | zeroValue = "0" 242 | case "uint8": 243 | zeroValue = "0" 244 | case "int16": 245 | zeroValue = "0" 246 | case "uint16": 247 | zeroValue = "0" 248 | case "int32": 249 | zeroValue = "0" 250 | case "uint32": 251 | zeroValue = "0" 252 | case "int64": 253 | zeroValue = "0" 254 | case "uint64": 255 | zeroValue = "0" 256 | case "float32": 257 | zeroValue = "0.0" 258 | case "float64": 259 | zeroValue = "0.0" 260 | case "string": 261 | zeroValue = "\"\"" 262 | case "bool": 263 | zeroValue = "false" 264 | case "char": 265 | zeroValue = "0" 266 | case "byte": 267 | zeroValue = "0" 268 | case "time": 269 | zeroValue = "ros.Time{}" 270 | case "duration": 271 | zeroValue = "ros.Duration{}" 272 | default: 273 | zeroValue = pkg + "." + typeName + "{}" 274 | } 275 | return zeroValue 276 | } 277 | 278 | func NewConstant(fieldType string, name string, value interface{}, valueText string) *Constant { 279 | goName := ToGoName(name, true) 280 | return &Constant{fieldType, name, value, valueText, goName} 281 | } 282 | 283 | func (c *Constant) String() string { 284 | return fmt.Sprintf("%s %s = %v", c.Type, c.Name, c.Value) 285 | } 286 | 287 | type Field struct { 288 | Package string 289 | Type string 290 | Name string 291 | IsBuiltin bool 292 | IsArray bool 293 | ArrayLen int 294 | GoName string 295 | GoType string 296 | ZeroValue string 297 | } 298 | 299 | func NewField(pkg string, fieldType string, name string, isArray bool, arrayLen int) *Field { 300 | goType := ToGoType(pkg, fieldType) 301 | goName := ToGoName(name, false) 302 | zeroValue := GetZeroValue(pkg, fieldType) 303 | isBuiltin := isBuiltinType(fieldType) 304 | return &Field{pkg, fieldType, name, isBuiltin, isArray, arrayLen, goName, goType, zeroValue} 305 | } 306 | 307 | func (f *Field) String() string { 308 | if f.IsArray && f.ArrayLen > -1 { 309 | return fmt.Sprintf("%s[%d] %s", f.Type, f.ArrayLen, f.Name) 310 | } else if f.IsArray { 311 | return fmt.Sprintf("%s[] %s", f.Type, f.Name) 312 | } else { 313 | return fmt.Sprintf("%s %s", f.Type, f.Name) 314 | } 315 | } 316 | 317 | type MsgSpec struct { 318 | Fields []Field 319 | Constants []Constant 320 | Text string 321 | MD5Sum string 322 | FullName string 323 | ShortName string 324 | Package string 325 | } 326 | 327 | type SrvSpec struct { 328 | Package string 329 | ShortName string 330 | FullName string 331 | Text string 332 | MD5Sum string 333 | Request *MsgSpec 334 | Response *MsgSpec 335 | } 336 | 337 | type ActionSpec struct { 338 | Package string 339 | ShortName string 340 | FullName string 341 | Text string 342 | MD5Sum string 343 | Goal *MsgSpec 344 | Feedback *MsgSpec 345 | Result *MsgSpec 346 | ActionGoal *MsgSpec 347 | ActionFeedback *MsgSpec 348 | ActionResult *MsgSpec 349 | } 350 | 351 | type OptionMsgSpec func(*MsgSpec) error 352 | 353 | func OptionPackageName(name string) func(*MsgSpec) error { 354 | return func(spec *MsgSpec) error { 355 | spec.Package = name 356 | return nil 357 | } 358 | } 359 | 360 | func OptionShortName(name string) func(*MsgSpec) error { 361 | return func(spec *MsgSpec) error { 362 | spec.ShortName = name 363 | return nil 364 | } 365 | } 366 | 367 | func NewMsgSpec(fields []Field, constants []Constant, text string, fullName string, options ...OptionMsgSpec) (*MsgSpec, error) { 368 | spec := &MsgSpec{} 369 | 370 | spec.Fields = fields 371 | spec.Constants = constants 372 | spec.Text = text 373 | spec.FullName = fullName 374 | 375 | for _, opt := range options { 376 | err := opt(spec) 377 | if err != nil { 378 | return nil, err 379 | } 380 | } 381 | return spec, nil 382 | } 383 | 384 | // Implements Stringer interface 385 | func (s *MsgSpec) String() string { 386 | lines := []string{} 387 | 388 | lines = append(lines, fmt.Sprintf("msg %s {", s.FullName)) 389 | 390 | for _, c := range s.Constants { 391 | lines = append(lines, fmt.Sprintf("\t%s", c.String())) 392 | } 393 | lines = append(lines, "") 394 | for _, f := range s.Fields { 395 | lines = append(lines, fmt.Sprintf("\t%s", f.String())) 396 | } 397 | 398 | lines = append(lines, fmt.Sprintf("}")) 399 | return strings.Join(lines, "\n") 400 | } 401 | 402 | func (s *MsgSpec) ComputeMD5(msgContext *MsgContext) (string, error) { 403 | thisPkgName := s.Package 404 | var buffer bytes.Buffer 405 | for _, c := range s.Constants { 406 | buffer.WriteString(fmt.Sprintf("%v %v=%v\n", c.Type, c.Name, c.ValueText)) 407 | } 408 | for _, f := range s.Fields { 409 | msgType := baseMsgType(f.Type) 410 | if isBuiltinType(f.Type) { 411 | buffer.WriteString(fmt.Sprintf("%v %v\n", f.Type, f.Name)) 412 | } else { 413 | pkgName, baseType, err := packageResourceName(msgType) 414 | if err != nil { 415 | return "", err 416 | } 417 | // If no package name, it should be a messge in the current package 418 | if len(pkgName) == 0 { 419 | pkgName = thisPkgName 420 | } 421 | fullMsgName := pkgName + "/" + baseType 422 | if msgSpec, err := msgContext.LoadMsg(fullMsgName); err != nil { 423 | subMD5, err := msgSpec.ComputeMD5(msgContext) 424 | if err != nil { 425 | return "", err 426 | } 427 | buffer.WriteString(fmt.Sprintf("%v %v\n", subMD5, f.Name)) 428 | } else { 429 | return "", fmt.Errorf("Message '%s' was not found", fullMsgName) 430 | } 431 | } 432 | } 433 | data := buffer.Bytes() 434 | hash := md5.New() 435 | sum := hash.Sum(data) 436 | return hex.EncodeToString(sum), nil 437 | } 438 | -------------------------------------------------------------------------------- /gengo/parser.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "fmt" 5 | "strconv" 6 | "strings" 7 | "unicode" 8 | ) 9 | 10 | const ( 11 | Sep = "/" 12 | MsgDir = "msg" 13 | SrvDir = "srv" 14 | ExtMsg = ".msg" 15 | ExtSrv = ".msg" 16 | 17 | ConstChar = "=" 18 | CommentChar = "#" 19 | IoDelim = "---" 20 | ) 21 | 22 | type SyntaxError struct { 23 | FullName string 24 | Line int 25 | Message string 26 | } 27 | 28 | func NewSyntaxError(fullName string, line int, message string) *SyntaxError { 29 | self := &SyntaxError{} 30 | self.FullName = fullName 31 | self.Line = line 32 | self.Message = message 33 | return self 34 | } 35 | 36 | func (e *SyntaxError) Error() string { 37 | return fmt.Sprintf("[%s@%d] %s", e.FullName, e.Line, e.Message) 38 | } 39 | 40 | /// Convert constant literal to a Go object 41 | /// Original implementation (genmsg) depends on Python's literal syntax. 42 | func convertConstantValue(fieldType string, valueLiteral string) (interface{}, error) { 43 | switch fieldType { 44 | case "float32": 45 | result, e := strconv.ParseFloat(valueLiteral, 32) 46 | return float32(result), e 47 | case "float64": 48 | return strconv.ParseFloat(valueLiteral, 64) 49 | case "string": 50 | return strings.TrimSpace(valueLiteral), nil 51 | case "byte": 52 | result, e := strconv.ParseInt(valueLiteral, 0, 8) 53 | return int8(result), e 54 | case "int8": 55 | result, e := strconv.ParseInt(valueLiteral, 0, 8) 56 | return int8(result), e 57 | case "int16": 58 | result, e := strconv.ParseInt(valueLiteral, 0, 16) 59 | return int16(result), e 60 | case "int32": 61 | result, e := strconv.ParseInt(valueLiteral, 0, 32) 62 | return int32(result), e 63 | case "int64": 64 | return strconv.ParseInt(valueLiteral, 0, 64) 65 | case "char": 66 | result, e := strconv.ParseUint(valueLiteral, 0, 8) 67 | return uint8(result), e 68 | case "uint8": 69 | result, e := strconv.ParseUint(valueLiteral, 0, 8) 70 | return uint8(result), e 71 | case "uint16": 72 | result, e := strconv.ParseUint(valueLiteral, 0, 16) 73 | return uint16(result), e 74 | case "uint32": 75 | result, e := strconv.ParseUint(valueLiteral, 0, 32) 76 | return uint32(result), e 77 | case "uint64": 78 | return strconv.ParseUint(valueLiteral, 0, 64) 79 | case "bool": 80 | // The spec of ROS message doesn't specify boolean literal exactly. 81 | // genmsg implementation determines true/false based Python's eval() and accepts any valid Python expression. 82 | if valueLiteral == "None" || valueLiteral == "False" { 83 | return false, nil 84 | } else if valueLiteral == "True" { 85 | return true, nil 86 | } else if val, e := strconv.ParseUint(valueLiteral, 10, 0); e == nil { 87 | return val != 0, nil 88 | } else { 89 | return nil, fmt.Errorf("Inavalid constant literal for bool: [%s]", valueLiteral) 90 | } 91 | default: 92 | return nil, fmt.Errorf("Invalid constant type: [%s]", fieldType) 93 | } 94 | } 95 | 96 | func packageResourceName(name string) (string, string, error) { 97 | const Separator = "/" 98 | if strings.Contains(name, Separator) { 99 | components := strings.Split(name, Separator) 100 | if len(components) == 2 { 101 | return components[0], components[1], nil 102 | } else { 103 | return "", "", fmt.Errorf("Invalid name %s", name) 104 | } 105 | } else { 106 | return "", name, nil 107 | } 108 | } 109 | 110 | func stripComment(line string) string { 111 | return strings.TrimSpace(strings.Split(line, CommentChar)[0]) 112 | } 113 | 114 | func loadConstantLine(line string) (*Constant, error) { 115 | cleanLine := stripComment(line) 116 | sepIndex := strings.IndexFunc(cleanLine, unicode.IsSpace) 117 | if sepIndex < 0 { 118 | return nil, fmt.Errorf("Could not find a constant name after the type name") 119 | } 120 | 121 | fieldType := cleanLine[:sepIndex] 122 | if !isValidConsantType(fieldType) { 123 | return nil, fmt.Errorf("[%s] is not a legal constant type", fieldType) 124 | } 125 | 126 | var name, valueText string 127 | if fieldType == "string" { 128 | // Strings contain anything to the right of the equal sign, no comments allowd. 129 | sepIndex := strings.IndexFunc(line, unicode.IsSpace) 130 | if sepIndex < 0 { 131 | return nil, fmt.Errorf("Could not find a constant name after the type name") 132 | } 133 | keyValue := line[sepIndex:] 134 | kvSplits := strings.SplitN(keyValue, "=", 2) 135 | if len(kvSplits) != 2 { 136 | return nil, fmt.Errorf("A constant definition requires its value") 137 | } 138 | name = strings.TrimSpace(kvSplits[0]) 139 | valueText = strings.TrimLeftFunc(kvSplits[1], unicode.IsSpace) 140 | } else { 141 | keyValue := strings.TrimSpace(cleanLine[sepIndex:]) 142 | kvSplits := strings.SplitN(keyValue, "=", 2) 143 | if len(kvSplits) != 2 { 144 | return nil, fmt.Errorf("A constant definition requires its value") 145 | } 146 | name = strings.TrimSpace(kvSplits[0]) 147 | valueText = strings.TrimSpace(kvSplits[1]) 148 | } 149 | 150 | value, e := convertConstantValue(fieldType, valueText) 151 | if e != nil { 152 | return nil, e 153 | } 154 | return NewConstant(fieldType, name, value, valueText), nil 155 | } 156 | 157 | func loadFieldLine(line string, packageName string) (*Field, error) { 158 | cleanLine := stripComment(line) 159 | lineSplits := strings.SplitN(cleanLine, " ", 2) 160 | if len(lineSplits) != 2 { 161 | return nil, fmt.Errorf("Invalid declaration: %s", line) 162 | } 163 | fieldType := strings.TrimSpace(lineSplits[0]) 164 | name := strings.TrimSpace(lineSplits[1]) 165 | if !isValidMsgFieldName(name) { 166 | return nil, fmt.Errorf("%s is not a legal message field name", name) 167 | } 168 | if !isValidMsgType(fieldType) { 169 | return nil, fmt.Errorf("%s is not a legal message field type", fieldType) 170 | } 171 | if len(packageName) > 0 && !strings.Contains(fieldType, Sep) { 172 | if fieldType == HeaderType { 173 | fieldType = HeaderFullName 174 | } else if !isBuiltinType(baseMsgType(fieldType)) { 175 | fieldType = fmt.Sprintf("%s/%s", packageName, fieldType) 176 | } 177 | } else if fieldType == HeaderType { 178 | fieldType = HeaderFullName 179 | } 180 | pkg, baseType, isArray, arrayLen, err := parseType(fieldType) 181 | if err != nil { 182 | return nil, err 183 | } 184 | 185 | return NewField(pkg, baseType, name, isArray, arrayLen), nil 186 | } 187 | -------------------------------------------------------------------------------- /gengo/parser_test.go: -------------------------------------------------------------------------------- 1 | // Copyright 2018, Akio Ochiai All rights reserved 2 | package main 3 | 4 | import ( 5 | // "math" 6 | "os" 7 | "reflect" 8 | "strings" 9 | "testing" 10 | ) 11 | 12 | func TestConvertConstantValue(t *testing.T) { 13 | var tests = []struct { 14 | fieldType string 15 | valueLiteral string 16 | expected interface{} 17 | expectError bool 18 | }{ 19 | {"bool", "0", false, false}, 20 | {"bool", "1", true, false}, 21 | {"bool", "2", true, false}, 22 | {"bool", "-2", true, true}, 23 | {"bool", "True", true, false}, 24 | {"bool", "False", false, false}, 25 | {"bool", "None", false, false}, 26 | {"float32", "2.72", float32(2.72), false}, 27 | {"float64", "-3.14", float64(-3.14), false}, 28 | {"int8", "-129", 0, true}, 29 | {"int8", "-128", int8(-128), false}, 30 | {"int8", "127", int8(127), false}, 31 | {"int8", "128", 0, true}, 32 | {"int16", "-32769", 0, true}, 33 | {"int16", "-32768", int16(-32768), false}, 34 | {"int16", "32767", int16(32767), false}, 35 | {"int16", "32768", 0, true}, 36 | {"int32", "-2147483649", 0, true}, 37 | {"int32", "-2147483648", int32(-2147483648), false}, 38 | {"int32", "2147483647", int32(2147483647), false}, 39 | {"int32", "2147483648", 0, true}, 40 | {"int64", "-9223372036854775809", 0, true}, 41 | {"int64", "-9223372036854775808", int64(-9223372036854775808), false}, 42 | {"int64", "9223372036854775807", int64(9223372036854775807), false}, 43 | {"int64", "9223372036854775808", 0, true}, 44 | {"uint8", "-1", 0, true}, 45 | {"uint8", "0", uint8(0), false}, 46 | {"uint8", "255", uint8(255), false}, 47 | {"uint8", "256", 0, true}, 48 | {"uint16", "-1", 0, true}, 49 | {"uint16", "0", uint16(0), false}, 50 | {"uint16", "65535", uint16(65535), false}, 51 | {"uint16", "65536", 0, true}, 52 | {"uint32", "-1", 0, true}, 53 | {"uint32", "0", uint32(0), false}, 54 | {"uint32", "4294967295", uint32(4294967295), false}, 55 | {"uint32", "4294967296", 0, true}, 56 | {"uint64", "-1", 0, true}, 57 | {"uint64", "0", uint64(0), false}, 58 | {"uint64", "18446744073709551615", uint64(18446744073709551615), false}, 59 | {"uint64", "18446744073709551616", 0, true}, 60 | {"string", "Lorem Ipsum", "Lorem Ipsum", false}, 61 | } 62 | 63 | for _, test := range tests { 64 | result, e := convertConstantValue(test.fieldType, test.valueLiteral) 65 | if test.expectError { 66 | if e == nil { 67 | t.Errorf("INPUT(%s : %s) | should fail but succeeded", test.valueLiteral, test.fieldType) 68 | } 69 | } else { 70 | if e != nil { 71 | t.Errorf("INPUT(%s : %s) | %s", test.valueLiteral, test.fieldType, e.Error()) 72 | 73 | } else if result != test.expected { 74 | format := "INPUT(%s : %s) | Expected: [%v: %v], Actual: [%v : %v]" 75 | t.Errorf(format, test.valueLiteral, test.fieldType, test.expected, reflect.TypeOf(test.expected), result, reflect.TypeOf(result)) 76 | } 77 | } 78 | } 79 | } 80 | 81 | func TestParseMessage(t *testing.T) { 82 | const text string = ` 83 | # Comment 84 | bool B = 1 85 | int8 I8 = -128 86 | int16 I16 = -32768 87 | int32 I32 = -2147483648 88 | int64 I64 = -9223372036854775808 89 | uint8 U8 = 255 90 | uint16 U16 = 65535 91 | uint32 U32 = 4294967295 92 | uint64 U64 = 18446744073709551615 93 | string S = Lorem Ipsum # Comment is ignored 94 | 95 | Header header 96 | bool b 97 | int8 i8 98 | uint8 u8 99 | int16 i16 100 | uint16 u16 101 | int32 i32 102 | uint32 u32 103 | int64 i64 104 | uint64 u64 105 | float32 f32 106 | float64 f64 107 | string s 108 | time t 109 | duration d 110 | string[] sva 111 | string[42] sfa 112 | std_msgs/Empty e 113 | std_msgs/Empty[] eva 114 | std_msgs/Empty[42] efa 115 | Bar x 116 | Bar[] xva 117 | Bar[42] xfa 118 | ` 119 | 120 | rosPkgPath := os.Getenv("ROS_PACKAGE_PATH") 121 | ctx, e := NewMsgContext(strings.Split(rosPkgPath, ":")) 122 | if e != nil { 123 | t.Errorf("Failed to create MsgContext.") 124 | } 125 | // var spec *MsgSpec 126 | _, e = ctx.LoadMsgFromString(text, "foo/Foo") 127 | if e != nil { 128 | t.Errorf("Failed to parse: %v", e) 129 | } 130 | // fmt.Println("---") 131 | // fmt.Println(spec.String()) 132 | } 133 | 134 | func TestMD5_std_msgs(t *testing.T) { 135 | var std_msgs = map[string]string{ 136 | "std_msgs/Bool": "8b94c1b53db61fb6aed406028ad6332a", 137 | "std_msgs/Byte": "ad736a2e8818154c487bb80fe42ce43b", 138 | "std_msgs/ByteMultiArray": "70ea476cbcfd65ac2f68f3cda1e891fe", 139 | "std_msgs/Char": "1bf77f25acecdedba0e224b162199717", 140 | "std_msgs/ColorRGBA": "a29a96539573343b1310c73607334b00", 141 | "std_msgs/Duration": "3e286caf4241d664e55f3ad380e2ae46", 142 | "std_msgs/Empty": "d41d8cd98f00b204e9800998ecf8427e", 143 | "std_msgs/Float32": "73fcbf46b49191e672908e50842a83d4", 144 | "std_msgs/Float32MultiArray": "6a40e0ffa6a17a503ac3f8616991b1f6", 145 | "std_msgs/Float64": "fdb28210bfa9d7c91146260178d9a584", 146 | "std_msgs/Float64MultiArray": "4b7d974086d4060e7db4613a7e6c3ba4", 147 | "std_msgs/Header": "2176decaecbce78abc3b96ef049fabed", 148 | "std_msgs/Int16": "8524586e34fbd7cb1c08c5f5f1ca0e57", 149 | "std_msgs/Int16MultiArray": "d9338d7f523fcb692fae9d0a0e9f067c", 150 | "std_msgs/Int32": "da5909fbe378aeaf85e547e830cc1bb7", 151 | "std_msgs/Int32MultiArray": "1d99f79f8b325b44fee908053e9c945b", 152 | "std_msgs/Int64": "34add168574510e6e17f5d23ecc077ef", 153 | "std_msgs/Int64MultiArray": "54865aa6c65be0448113a2afc6a49270", 154 | "std_msgs/Int8": "27ffa0c9c4b8fb8492252bcad9e5c57b", 155 | "std_msgs/Int8MultiArray": "d7c1af35a1b4781bbe79e03dd94b7c13", 156 | "std_msgs/MultiArrayDimension": "4cd0c83a8683deae40ecdac60e53bfa8", 157 | "std_msgs/MultiArrayLayout": "0fed2a11c13e11c5571b4e2a995a91a3", 158 | "std_msgs/String": "992ce8a1687cec8c8bd883ec73ca41d1", 159 | "std_msgs/Time": "cd7166c74c552c311fbcc2fe5a7bc289", 160 | "std_msgs/UInt16": "1df79edf208b629fe6b81923a544552d", 161 | "std_msgs/UInt16MultiArray": "52f264f1c973c4b73790d384c6cb4484", 162 | "std_msgs/UInt32": "304a39449588c7f8ce2df6e8001c5fce", 163 | "std_msgs/UInt32MultiArray": "4d6a180abc9be191b96a7eda6c8a233d", 164 | "std_msgs/UInt64": "1b2a79973e8bf53d7b53acb71299cb57", 165 | "std_msgs/UInt64MultiArray": "6088f127afb1d6c72927aa1247e945af", 166 | "std_msgs/UInt8": "7c8164229e7d2c17eb95e9231617fdee", 167 | } 168 | 169 | rosPkgPath := os.Getenv("ROS_PACKAGE_PATH") 170 | ctx, e := NewMsgContext(strings.Split(rosPkgPath, ":")) 171 | if e != nil { 172 | t.Errorf("Failed to create MsgContext.") 173 | } else { 174 | for fullname, md5 := range std_msgs { 175 | _, shortName, _ := packageResourceName(fullname) 176 | 177 | t.Run(shortName, func(t *testing.T) { 178 | var spec *MsgSpec 179 | spec, e := ctx.LoadMsg(fullname) 180 | if e != nil { 181 | t.Errorf("Failed to parse: %v", e) 182 | } else { 183 | assertEqual(t, spec.MD5Sum, md5) 184 | } 185 | }) 186 | } 187 | 188 | } 189 | } 190 | 191 | func TestMD5_sensor_msgs(t *testing.T) { 192 | var sensor_msgs = map[string]string{ 193 | "sensor_msgs/BatteryState": "476f837fa6771f6e16e3bf4ef96f8770", 194 | "sensor_msgs/CameraInfo": "c9a58c1b0b154e0e6da7578cb991d214", 195 | "sensor_msgs/ChannelFloat32": "3d40139cdd33dfedcb71ffeeeb42ae7f", 196 | "sensor_msgs/CompressedImage": "8f7a12909da2c9d3332d540a0977563f", 197 | "sensor_msgs/FluidPressure": "804dc5cea1c5306d6a2eb80b9833befe", 198 | "sensor_msgs/Illuminance": "8cf5febb0952fca9d650c3d11a81a188", 199 | "sensor_msgs/Image": "060021388200f6f0f447d0fcd9c64743", 200 | "sensor_msgs/Imu": "6a62c6daae103f4ff57a132d6f95cec2", 201 | "sensor_msgs/JointState": "3066dcd76a6cfaef579bd0f34173e9fd", 202 | "sensor_msgs/Joy": "5a9ea5f83505693b71e785041e67a8bb", 203 | "sensor_msgs/JoyFeedback": "f4dcd73460360d98f36e55ee7f2e46f1", 204 | "sensor_msgs/JoyFeedbackArray": "cde5730a895b1fc4dee6f91b754b213d", 205 | "sensor_msgs/LaserEcho": "8bc5ae449b200fba4d552b4225586696", 206 | "sensor_msgs/LaserScan": "90c7ef2dc6895d81024acba2ac42f369", 207 | "sensor_msgs/MagneticField": "2f3b0b43eed0c9501de0fa3ff89a45aa", 208 | "sensor_msgs/MultiDOFJointState": "690f272f0640d2631c305eeb8301e59d", 209 | "sensor_msgs/MultiEchoLaserScan": "6fefb0c6da89d7c8abe4b339f5c2f8fb", 210 | "sensor_msgs/NavSatFix": "2d3a8cd499b9b4a0249fb98fd05cfa48", 211 | "sensor_msgs/NavSatStatus": "331cdbddfa4bc96ffc3b9ad98900a54c", 212 | "sensor_msgs/PointCloud": "d8e9c3f5afbdd8a130fd1d2763945fca", 213 | "sensor_msgs/PointCloud2": "1158d486dd51d683ce2f1be655c3c181", 214 | "sensor_msgs/PointField": "268eacb2962780ceac86cbd17e328150", 215 | "sensor_msgs/Range": "c005c34273dc426c67a020a87bc24148", 216 | "sensor_msgs/RegionOfInterest": "bdb633039d588fcccb441a4d43ccfe09", 217 | "sensor_msgs/RelativeHumidity": "8730015b05955b7e992ce29a2678d90f", 218 | "sensor_msgs/Temperature": "ff71b307acdbe7c871a5a6d7ed359100", 219 | "sensor_msgs/TimeReference": "fded64a0265108ba86c3d38fb11c0c16", 220 | } 221 | 222 | rosPkgPath := os.Getenv("ROS_PACKAGE_PATH") 223 | ctx, e := NewMsgContext(strings.Split(rosPkgPath, ":")) 224 | if e != nil { 225 | t.Errorf("Failed to create MsgContext.") 226 | } 227 | 228 | for fullname, md5 := range sensor_msgs { 229 | _, shortName, _ := packageResourceName(fullname) 230 | 231 | t.Run(shortName, func(t *testing.T) { 232 | var spec *MsgSpec 233 | spec, e := ctx.LoadMsg(fullname) 234 | if e != nil { 235 | t.Errorf("Failed to parse: %v", e) 236 | } else { 237 | assertEqual(t, spec.MD5Sum, md5) 238 | } 239 | }) 240 | } 241 | } 242 | -------------------------------------------------------------------------------- /gengo/testing_util.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "testing" 5 | ) 6 | 7 | func assertEqual(t *testing.T, a interface{}, b interface{}) { 8 | if a != b { 9 | t.Fatalf("%s != %s", a, b) 10 | } 11 | } 12 | -------------------------------------------------------------------------------- /ros/duration.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "time" 5 | ) 6 | 7 | type Duration struct { 8 | temporal 9 | } 10 | 11 | func NewDuration(sec uint32, nsec uint32) Duration { 12 | sec, nsec = normalizeTemporal(int64(sec), int64(nsec)) 13 | return Duration{temporal{sec, nsec}} 14 | } 15 | 16 | func (d *Duration) Add(other Duration) Duration { 17 | sec, nsec := normalizeTemporal(int64(d.Sec)+int64(other.Sec), 18 | int64(d.NSec)+int64(other.NSec)) 19 | return Duration{temporal{sec, nsec}} 20 | } 21 | 22 | func (d *Duration) Sub(other Duration) Duration { 23 | sec, nsec := normalizeTemporal(int64(d.Sec)-int64(other.Sec), 24 | int64(d.NSec)-int64(other.NSec)) 25 | return Duration{temporal{sec, nsec}} 26 | } 27 | 28 | func (d *Duration) Cmp(other Duration) int { 29 | return cmpUint64(d.ToNSec(), other.ToNSec()) 30 | } 31 | 32 | func (d *Duration) Sleep() error { 33 | if !d.IsZero() { 34 | time.Sleep(time.Duration(d.ToNSec()) * time.Nanosecond) 35 | } 36 | return nil 37 | } 38 | -------------------------------------------------------------------------------- /ros/duration_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "testing" 5 | "time" 6 | ) 7 | 8 | func TestNewDuration(t *testing.T) { 9 | d := NewDuration(1, 2) 10 | if d.Sec != 1 { 11 | t.Fail() 12 | } 13 | if d.NSec != 2 { 14 | t.Fail() 15 | } 16 | } 17 | 18 | func TestDurationAdd(t *testing.T) { 19 | var d1, d2 Duration 20 | d1.FromNSec(500000000) 21 | d2.FromNSec(800000000) 22 | 23 | d3 := d1.Add(d2) 24 | if d3.Sec != 1 { 25 | t.Error(d3.Sec) 26 | } 27 | if d3.NSec != 300000000 { 28 | t.Error(d3.NSec) 29 | } 30 | } 31 | 32 | func TestDurationSub(t *testing.T) { 33 | var d1, d2 Duration 34 | d1.FromNSec(1300000000) 35 | d2.FromNSec(500000000) 36 | 37 | d3 := d1.Sub(d2) 38 | if d3.Sec != 0 { 39 | t.Error(d3.Sec) 40 | } 41 | if d3.NSec != 800000000 { 42 | t.Error(d3.NSec) 43 | } 44 | } 45 | 46 | func TestDurationSleep(t *testing.T) { 47 | d := NewDuration(1, 100000000) 48 | start := time.Now().UnixNano() 49 | d.Sleep() 50 | end := time.Now().UnixNano() 51 | // The jitter tolerance (5msec) doesn't have strong basis. 52 | const Tolerance int64 = 5000000 53 | elapsed := end - start 54 | delta := elapsed - int64(d.ToNSec()) 55 | if delta < 0 { 56 | delta = -delta 57 | } 58 | if delta > Tolerance { 59 | t.Errorf("expected: %d actual0: %d delta: %d", d.ToNSec(), elapsed, delta) 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /ros/header.go: -------------------------------------------------------------------------------- 1 | // Connection header 2 | package ros 3 | 4 | import ( 5 | "bytes" 6 | "encoding/binary" 7 | "fmt" 8 | "io" 9 | ) 10 | 11 | type header struct { 12 | key string 13 | value string 14 | } 15 | 16 | type connHeader struct { 17 | headers map[string]string 18 | } 19 | 20 | const BufferSize = 1024 21 | 22 | func readConnectionHeader(r io.Reader) ([]header, error) { 23 | buf := make([]byte, 4) 24 | _, err := io.ReadAtLeast(r, buf, 4) 25 | if err != nil { 26 | return nil, err 27 | } 28 | var headerSize uint32 29 | bufReader := bytes.NewBuffer(buf) 30 | err = binary.Read(bufReader, binary.LittleEndian, &headerSize) 31 | if err != nil { 32 | return nil, err 33 | } 34 | buf = make([]byte, int(headerSize)) 35 | _, err = io.ReadAtLeast(r, buf, int(headerSize)) 36 | if err != nil { 37 | return nil, err 38 | } 39 | 40 | var done uint32 = 0 41 | var headers []header 42 | bufReader = bytes.NewBuffer(buf) 43 | for { 44 | if done == headerSize { 45 | break 46 | } else if done > headerSize { 47 | return nil, fmt.Errorf("Header length overrrun") 48 | } 49 | var size uint32 50 | err := binary.Read(bufReader, binary.LittleEndian, &size) 51 | if err != nil { 52 | return nil, err 53 | } 54 | line := bufReader.Next(int(size)) 55 | sep := bytes.IndexByte(line, '=') 56 | key := string(line[0:sep]) 57 | value := string(line[sep+1:]) 58 | headers = append(headers, header{key, value}) 59 | done += 4 + size 60 | } 61 | return headers, nil 62 | } 63 | 64 | func writeConnectionHeader(headers []header, w io.Writer) error { 65 | //var buf bytes.Buffer 66 | var headerSize int 67 | var sizeList []int 68 | for _, h := range headers { 69 | size := len(h.key) + len(h.value) + 1 70 | sizeList = append(sizeList, size) 71 | headerSize += size + 4 72 | } 73 | if err := binary.Write(w, binary.LittleEndian, uint32(headerSize)); err != nil { 74 | return err 75 | } 76 | for i, h := range headers { 77 | err := binary.Write(w, binary.LittleEndian, uint32(sizeList[i])) 78 | if err != nil { 79 | return err 80 | } 81 | if _, err = w.Write([]byte(h.key)); err != nil { 82 | return err 83 | } 84 | if _, err = w.Write([]byte("=")); err != nil { 85 | return err 86 | } 87 | if _, err = w.Write([]byte(h.value)); err != nil { 88 | return err 89 | } 90 | } 91 | return nil 92 | } 93 | -------------------------------------------------------------------------------- /ros/header_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "bytes" 5 | "testing" 6 | ) 7 | 8 | func TestReadConnectionHeader(t *testing.T) { 9 | // example from ros.org 10 | data := []byte{ 11 | 0xb0, 0x00, 0x00, 0x00, 12 | 0x20, 0x00, 0x00, 0x00, 13 | 0x6d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65, 0x5f, 0x64, 0x65, 14 | 0x66, 0x69, 0x6e, 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x3d, 0x73, 15 | 0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x64, 0x61, 0x74, 0x61, 16 | 0x0a, 0x0a, 17 | 0x25, 0x00, 0x00, 0x00, 18 | 0x63, 0x61, 0x6c, 0x6c, 0x65, 0x72, 0x69, 0x64, 0x3d, 0x2f, 19 | 0x72, 0x6f, 0x73, 0x74, 0x6f, 0x70, 0x69, 0x63, 0x5f, 0x34, 20 | 0x37, 0x36, 0x37, 0x5f, 0x31, 0x33, 0x31, 0x36, 0x39, 0x31, 21 | 0x32, 0x37, 0x34, 0x31, 0x35, 0x35, 0x37, 22 | 0x0a, 0x00, 0x00, 0x00, 23 | 0x6c, 0x61, 0x74, 0x63, 0x68, 0x69, 0x6e, 0x67, 0x3d, 0x31, 24 | 0x27, 0x00, 0x00, 0x00, 25 | 0x6d, 0x64, 0x35, 0x73, 0x75, 0x6d, 0x3d, 0x39, 0x39, 0x32, 26 | 0x63, 0x65, 0x38, 0x61, 0x31, 0x36, 0x38, 0x37, 0x63, 0x65, 27 | 0x63, 0x38, 0x63, 0x38, 0x62, 0x64, 0x38, 0x38, 0x33, 0x65, 28 | 0x63, 0x37, 0x33, 0x63, 0x61, 0x34, 0x31, 0x64, 0x31, 29 | 0x0e, 0x00, 0x00, 0x00, 30 | 0x74, 0x6f, 0x70, 0x69, 0x63, 0x3d, 0x2f, 0x63, 0x68, 0x61, 31 | 0x74, 0x74, 0x65, 0x72, 32 | 0x14, 0x00, 0x00, 0x00, 33 | 0x74, 0x79, 0x70, 0x65, 0x3d, 0x73, 0x74, 0x64, 0x5f, 0x6d, 34 | 0x73, 0x67, 0x73, 0x2f, 0x53, 0x74, 0x72, 0x69, 0x6e, 0x67, 35 | 0x09, 0x00, 0x00, 0x00, 36 | 0x05, 0x00, 0x00, 0x00, 37 | 0x68, 0x65, 0x6c, 0x6c, 0x6f, 38 | } 39 | 40 | buf := bytes.NewReader(data) 41 | t.Log(buf.Len()) 42 | headers, err := readConnectionHeader(buf) 43 | if err != nil { 44 | t.Error(err) 45 | } 46 | 47 | if headers[0].key != "message_definition" { 48 | t.Error("") 49 | } 50 | if headers[0].value != "string data\n\n" { 51 | t.Error("") 52 | } 53 | 54 | if headers[1].key != "callerid" { 55 | t.Error("") 56 | } 57 | if headers[1].value != "/rostopic_4767_1316912741557" { 58 | t.Error("") 59 | } 60 | 61 | if headers[2].key != "latching" { 62 | t.Error("") 63 | } 64 | if headers[2].value != "1" { 65 | t.Error("") 66 | } 67 | 68 | if headers[3].key != "md5sum" { 69 | t.Error("") 70 | } 71 | if headers[3].value != "992ce8a1687cec8c8bd883ec73ca41d1" { 72 | t.Error("") 73 | } 74 | 75 | if headers[4].key != "topic" { 76 | t.Error("") 77 | } 78 | if headers[4].value != "/chatter" { 79 | t.Error("") 80 | } 81 | 82 | if headers[5].key != "type" { 83 | t.Error("") 84 | } 85 | if headers[5].value != "std_msgs/String" { 86 | t.Error("") 87 | } 88 | } 89 | 90 | func TestWriteConnectionHeader(t *testing.T) { 91 | headers := []header{ 92 | header{"message_definition", "string data\n\n"}, 93 | header{"callerid", "/rostopic_4767_1316912741557"}, 94 | header{"latching", "1"}, 95 | header{"md5sum", "992ce8a1687cec8c8bd883ec73ca41d1"}, 96 | header{"topic", "/chatter"}, 97 | header{"type", "std_msgs/String"}, 98 | } 99 | 100 | var buffer bytes.Buffer 101 | err := writeConnectionHeader(headers, &buffer) 102 | if err != nil { 103 | t.Error(err) 104 | } 105 | 106 | expected := []byte{ 107 | 0xb0, 0x00, 0x00, 0x00, 108 | 0x20, 0x00, 0x00, 0x00, 109 | 0x6d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65, 0x5f, 0x64, 0x65, 110 | 0x66, 0x69, 0x6e, 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x3d, 0x73, 111 | 0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x64, 0x61, 0x74, 0x61, 112 | 0x0a, 0x0a, 113 | 0x25, 0x00, 0x00, 0x00, 114 | 0x63, 0x61, 0x6c, 0x6c, 0x65, 0x72, 0x69, 0x64, 0x3d, 0x2f, 115 | 0x72, 0x6f, 0x73, 0x74, 0x6f, 0x70, 0x69, 0x63, 0x5f, 0x34, 116 | 0x37, 0x36, 0x37, 0x5f, 0x31, 0x33, 0x31, 0x36, 0x39, 0x31, 117 | 0x32, 0x37, 0x34, 0x31, 0x35, 0x35, 0x37, 118 | 0x0a, 0x00, 0x00, 0x00, 119 | 0x6c, 0x61, 0x74, 0x63, 0x68, 0x69, 0x6e, 0x67, 0x3d, 0x31, 120 | 0x27, 0x00, 0x00, 0x00, 121 | 0x6d, 0x64, 0x35, 0x73, 0x75, 0x6d, 0x3d, 0x39, 0x39, 0x32, 122 | 0x63, 0x65, 0x38, 0x61, 0x31, 0x36, 0x38, 0x37, 0x63, 0x65, 123 | 0x63, 0x38, 0x63, 0x38, 0x62, 0x64, 0x38, 0x38, 0x33, 0x65, 124 | 0x63, 0x37, 0x33, 0x63, 0x61, 0x34, 0x31, 0x64, 0x31, 125 | 0x0e, 0x00, 0x00, 0x00, 126 | 0x74, 0x6f, 0x70, 0x69, 0x63, 0x3d, 0x2f, 0x63, 0x68, 0x61, 127 | 0x74, 0x74, 0x65, 0x72, 128 | 0x14, 0x00, 0x00, 0x00, 129 | 0x74, 0x79, 0x70, 0x65, 0x3d, 0x73, 0x74, 0x64, 0x5f, 0x6d, 130 | 0x73, 0x67, 0x73, 0x2f, 0x53, 0x74, 0x72, 0x69, 0x6e, 0x67, 131 | } 132 | if !bytes.Equal(buffer.Bytes(), expected) { 133 | t.Fail() 134 | } 135 | } 136 | -------------------------------------------------------------------------------- /ros/log.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "fmt" 5 | "log" 6 | "os" 7 | ) 8 | 9 | type LogLevel int 10 | 11 | const ( 12 | LogLevelDebug LogLevel = iota 13 | LogLevelInfo 14 | LogLevelWarn 15 | LogLevelError 16 | LogLevelFatal 17 | ) 18 | 19 | type Logger interface { 20 | Severity() LogLevel 21 | SetSeverity(severity LogLevel) 22 | Debug(v ...interface{}) 23 | Debugf(format string, v ...interface{}) 24 | Info(v ...interface{}) 25 | Infof(format string, v ...interface{}) 26 | Warn(v ...interface{}) 27 | Warnf(format string, v ...interface{}) 28 | Error(v ...interface{}) 29 | Errorf(format string, v ...interface{}) 30 | Fatal(v ...interface{}) 31 | Fatalf(format string, v ...interface{}) 32 | } 33 | 34 | type defaultLogger struct { 35 | severity LogLevel 36 | } 37 | 38 | func NewDefaultLogger() *defaultLogger { 39 | logger := new(defaultLogger) 40 | logger.severity = LogLevelInfo 41 | return logger 42 | } 43 | 44 | func (logger *defaultLogger) Severity() LogLevel { 45 | return logger.severity 46 | } 47 | 48 | func (logger *defaultLogger) SetSeverity(severity LogLevel) { 49 | logger.severity = severity 50 | } 51 | 52 | func (logger *defaultLogger) Debug(v ...interface{}) { 53 | if int(logger.severity) <= int(LogLevelDebug) { 54 | msg := fmt.Sprintf("[DEBUG] %s", fmt.Sprint(v...)) 55 | log.Println(msg) 56 | } 57 | } 58 | 59 | func (logger *defaultLogger) Debugf(format string, v ...interface{}) { 60 | if int(logger.severity) <= int(LogLevelDebug) { 61 | log.Printf("[DEBUG] "+format, v...) 62 | } 63 | } 64 | 65 | func (logger *defaultLogger) Info(v ...interface{}) { 66 | if int(logger.severity) <= int(LogLevelInfo) { 67 | msg := fmt.Sprintf("[INFO] %s", fmt.Sprint(v...)) 68 | log.Println(msg) 69 | } 70 | } 71 | 72 | func (logger *defaultLogger) Infof(format string, v ...interface{}) { 73 | if int(logger.severity) <= int(LogLevelInfo) { 74 | log.Printf("[INFO] "+format, v...) 75 | } 76 | } 77 | 78 | func (logger *defaultLogger) Warn(v ...interface{}) { 79 | if int(logger.severity) <= int(LogLevelWarn) { 80 | msg := fmt.Sprintf("[WARN] %s", fmt.Sprint(v...)) 81 | log.Println(msg) 82 | } 83 | } 84 | 85 | func (logger *defaultLogger) Warnf(format string, v ...interface{}) { 86 | if int(logger.severity) <= int(LogLevelWarn) { 87 | log.Printf("[WARN] "+format, v...) 88 | } 89 | } 90 | 91 | func (logger *defaultLogger) Error(v ...interface{}) { 92 | if int(logger.severity) <= int(LogLevelError) { 93 | msg := fmt.Sprintf("[ERROR] %s", fmt.Sprint(v...)) 94 | log.Println(msg) 95 | } 96 | } 97 | 98 | func (logger *defaultLogger) Errorf(format string, v ...interface{}) { 99 | if int(logger.severity) <= int(LogLevelError) { 100 | log.Printf("[ERROR]"+format, v...) 101 | } 102 | } 103 | 104 | func (logger *defaultLogger) Fatal(v ...interface{}) { 105 | if int(logger.severity) <= int(LogLevelFatal) { 106 | msg := fmt.Sprintf("[FATAL] %s", fmt.Sprint(v...)) 107 | log.Println(msg) 108 | os.Exit(1) 109 | } 110 | } 111 | 112 | func (logger *defaultLogger) Fatalf(format string, v ...interface{}) { 113 | if int(logger.severity) <= int(LogLevelFatal) { 114 | log.Printf("[FATAL] "+format, v...) 115 | os.Exit(1) 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /ros/master.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "fmt" 5 | 6 | "github.com/fetchrobotics/rosgo/xmlrpc" 7 | ) 8 | 9 | func callRosAPI(calleeURI string, method string, args ...interface{}) (interface{}, error) { 10 | result, err := xmlrpc.Call(calleeURI, method, args...) 11 | if err != nil { 12 | return nil, err 13 | } 14 | 15 | var ok bool 16 | var xs []interface{} 17 | var code int32 18 | var message string 19 | var value interface{} 20 | if xs, ok = result.([]interface{}); !ok { 21 | return nil, fmt.Errorf("malformed ROS API result") 22 | } 23 | if len(xs) != 3 { 24 | err := fmt.Errorf("Malformed ROS API result. Length must be 3 but %d", len(xs)) 25 | return nil, err 26 | } 27 | if code, ok = xs[0].(int32); !ok { 28 | return nil, fmt.Errorf("status code is not int") 29 | } 30 | if message, ok = xs[1].(string); !ok { 31 | return nil, fmt.Errorf("message is not string") 32 | } 33 | value = xs[2] 34 | 35 | if code != APIStatusSuccess { 36 | err := fmt.Errorf("ROS Master API call failed with code %d: %s", code, message) 37 | return nil, err 38 | } 39 | return value, nil 40 | } 41 | 42 | // Build XMLRPC ready array from ROS API result triplet. 43 | func buildRosAPIResult(code int32, message string, value interface{}) interface{} { 44 | result := make([]interface{}, 3) 45 | result[0] = code 46 | result[1] = message 47 | result[2] = value 48 | return result 49 | } 50 | -------------------------------------------------------------------------------- /ros/message.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "bytes" 5 | ) 6 | 7 | type MessageType interface { 8 | Text() string 9 | MD5Sum() string 10 | Name() string 11 | NewMessage() Message 12 | } 13 | 14 | type Message interface { 15 | GetType() MessageType 16 | Serialize(buf *bytes.Buffer) error 17 | Deserialize(buf *Reader) error 18 | } 19 | -------------------------------------------------------------------------------- /ros/name.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "fmt" 5 | "regexp" 6 | "strings" 7 | ) 8 | 9 | const ( 10 | Sep = "/" 11 | GlobalNS = "/" 12 | PrivateNS = "~" 13 | ) 14 | 15 | type NameMap map[string]string 16 | 17 | func getNamespace(name string) string { 18 | if len(name) == 0 { 19 | return GlobalNS 20 | } else if name[len(name)-1] == '/' { 21 | name = name[:len(name)-1] 22 | } 23 | result := name[:strings.LastIndex(name, Sep)+1] 24 | if len(result) == 0 { 25 | return Sep 26 | } 27 | return result 28 | } 29 | 30 | func qualifyNodeName(nodeName string) (string, string, error) { 31 | if nodeName == "" { 32 | return "", "", fmt.Errorf("Empty node name") 33 | } 34 | if nodeName[:1] == PrivateNS { 35 | return "", "", fmt.Errorf("Node name should not contain '~'") 36 | } 37 | canonName := canonicalizeName(nodeName) 38 | 39 | var components []string 40 | for _, c := range strings.Split(canonName, Sep) { 41 | if len(c) > 0 { 42 | components = append(components, c) 43 | } 44 | } 45 | if len(components) == 1 { 46 | return GlobalNS, components[0], nil 47 | } else { 48 | namespace := GlobalNS + strings.Join(components[:len(components)-1], Sep) 49 | return namespace, components[len(components)-1], nil 50 | } 51 | } 52 | 53 | func isValidName(name string) bool { 54 | if len(name) == 0 { 55 | return true 56 | } 57 | if name == "/" || name == "~" { 58 | return true 59 | } 60 | if matched, _ := regexp.MatchString("^[~/]?([a-zA-Z]\\w*/)*[a-zA-Z]\\w*/?$", name); !matched { 61 | return false 62 | } 63 | return true 64 | } 65 | 66 | func isGlobalName(name string) bool { 67 | return len(name) > 0 && name[0:1] == GlobalNS 68 | } 69 | 70 | func isPrivateName(name string) bool { 71 | return len(name) > 0 && name[0:1] == PrivateNS 72 | } 73 | 74 | // Remove sequential seperater 75 | func canonicalizeName(name string) string { 76 | if name == GlobalNS { 77 | return name 78 | } else { 79 | components := []string{} 80 | for _, word := range strings.Split(name, Sep) { 81 | if len(word) > 0 { 82 | components = append(components, word) 83 | } 84 | } 85 | if name[0:1] == GlobalNS { 86 | return GlobalNS + strings.Join(components, Sep) 87 | } else { 88 | return strings.Join(components, Sep) 89 | } 90 | } 91 | } 92 | 93 | type NameResolver struct { 94 | nodeName string 95 | namespace string 96 | mapping NameMap 97 | resolvedMapping NameMap 98 | } 99 | 100 | func newNameResolver(namespace string, nodeName string, remapping NameMap) *NameResolver { 101 | n := new(NameResolver) 102 | 103 | n.nodeName = nodeName 104 | n.namespace = canonicalizeName(namespace) 105 | n.mapping = remapping 106 | n.resolvedMapping = make(NameMap) 107 | 108 | for k, v := range n.mapping { 109 | newKey := n.resolve(k) 110 | newValue := n.resolve(v) 111 | n.resolvedMapping[newKey] = newValue 112 | } 113 | 114 | return n 115 | } 116 | 117 | // Resolve a ROS name to global name 118 | func (n *NameResolver) resolve(name string) string { 119 | if len(name) == 0 { 120 | return n.namespace 121 | } 122 | 123 | var resolvedName string 124 | canonName := canonicalizeName(name) 125 | if isGlobalName(canonName) { 126 | resolvedName = canonName 127 | } else if isPrivateName(canonName) { 128 | resolvedName = canonicalizeName(n.namespace + Sep + n.nodeName + Sep + canonName[1:]) 129 | } else { 130 | resolvedName = canonicalizeName(n.namespace + Sep + canonName) 131 | } 132 | 133 | return resolvedName 134 | } 135 | 136 | // Resolve a ROS name with remapping 137 | func (n *NameResolver) remap(name string) string { 138 | key := n.resolve(name) 139 | if value, ok := n.resolvedMapping[key]; ok { 140 | return value 141 | } else { 142 | return key 143 | } 144 | } 145 | -------------------------------------------------------------------------------- /ros/name_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "testing" 5 | ) 6 | 7 | func TestNameValidation(t *testing.T) { 8 | // Positive testing 9 | positives := [...]string{ 10 | "", 11 | "/", 12 | "~", 13 | "foo", 14 | "foo/", 15 | "foo/bar", 16 | "foo/bar/", 17 | "foo_0/bar1_/", 18 | "/foo", 19 | "/foo/", 20 | "/foo/bar", 21 | "/foo/bar/", 22 | "~foo", 23 | "~foo/", 24 | "~foo/bar", 25 | "~foo/bar/", 26 | } 27 | for _, p := range positives { 28 | if !isValidName(p) { 29 | t.Error(p) 30 | } 31 | } 32 | 33 | // Negative testing 34 | negatives := [...]string{ 35 | "foo//bar", 36 | "^foo//bar", 37 | "//foo", 38 | "0foo", 39 | "_0foo", 40 | "foo/0bar", 41 | "foo/_bar", 42 | "foo/~bar", 43 | "foo bar", 44 | } 45 | for _, n := range negatives { 46 | if isValidName(n) { 47 | t.Error(n) 48 | } 49 | } 50 | } 51 | 52 | func TestCanonicalizeName(t *testing.T) { 53 | if canonicalizeName("/") != "/" { 54 | t.Fail() 55 | } 56 | 57 | if canonicalizeName("/foo//bar/") != "/foo/bar" { 58 | t.Fail() 59 | } 60 | 61 | if canonicalizeName("foo//bar///baz/") != "foo/bar/baz" { 62 | t.Fail() 63 | } 64 | 65 | if canonicalizeName("~foo//bar///baz/") != "~foo/bar/baz" { 66 | t.Fail() 67 | } 68 | } 69 | 70 | func TestSpecialNamespace(t *testing.T) { 71 | if !isGlobalName("/foo") { 72 | t.Fail() 73 | } 74 | if isGlobalName("~foo") { 75 | t.Fail() 76 | } 77 | if isGlobalName("foo") { 78 | t.Fail() 79 | } 80 | 81 | if isPrivateName("/foo") { 82 | t.Fail() 83 | } 84 | if !isPrivateName("~foo") { 85 | t.Fail() 86 | } 87 | if isPrivateName("foo") { 88 | t.Fail() 89 | } 90 | } 91 | 92 | func TestResolution1(t *testing.T) { 93 | remapping := NameMap{} 94 | resolver := newNameResolver("/", "node1", remapping) 95 | var result string 96 | 97 | result = resolver.resolve("bar") 98 | if result != "/bar" { 99 | t.Error(result) 100 | } 101 | 102 | result = resolver.resolve("/bar") 103 | if result != "/bar" { 104 | t.Error(result) 105 | } 106 | 107 | result = resolver.resolve("~bar") 108 | if result != "/node1/bar" { 109 | t.Error(result) 110 | } 111 | } 112 | 113 | func TestResolution2(t *testing.T) { 114 | remapping := NameMap{} 115 | resolver := newNameResolver("/go", "node2", remapping) 116 | var result string 117 | 118 | result = resolver.resolve("bar") 119 | if result != "/go/bar" { 120 | t.Error(result) 121 | } 122 | 123 | result = resolver.resolve("/bar") 124 | if result != "/bar" { 125 | t.Error(result) 126 | } 127 | 128 | result = resolver.resolve("~bar") 129 | if result != "/go/node2/bar" { 130 | t.Error(result) 131 | } 132 | } 133 | 134 | func TestResolution3(t *testing.T) { 135 | remapping := NameMap{} 136 | resolver := newNameResolver("/go", "node3", remapping) 137 | var result string 138 | 139 | result = resolver.resolve("foo/bar") 140 | if result != "/go/foo/bar" { 141 | t.Error(result) 142 | } 143 | 144 | result = resolver.resolve("/foo/bar") 145 | if result != "/foo/bar" { 146 | t.Error(result) 147 | } 148 | 149 | result = resolver.resolve("~foo/bar") 150 | if result != "/go/node3/foo/bar" { 151 | t.Error(result) 152 | } 153 | } 154 | 155 | func TestNameMap1(t *testing.T) { 156 | remapping := NameMap{ 157 | "foo": "bar", 158 | } 159 | 160 | resolver := newNameResolver("/", "mynode", remapping) 161 | var result string 162 | 163 | result = resolver.remap("foo") 164 | if result != "/bar" { 165 | t.Error(result) 166 | } 167 | 168 | result = resolver.remap("/foo") 169 | if result != "/bar" { 170 | t.Error(result) 171 | } 172 | } 173 | 174 | func TestNameMap2(t *testing.T) { 175 | remapping := NameMap{ 176 | "foo": "bar", 177 | } 178 | 179 | resolver := newNameResolver("/baz", "mynode", remapping) 180 | var result string 181 | 182 | result = resolver.remap("foo") 183 | if result != "/baz/bar" { 184 | t.Error(result) 185 | t.Error(resolver.mapping) 186 | } 187 | 188 | result = resolver.remap("/baz/foo") 189 | if result != "/baz/bar" { 190 | t.Error(result) 191 | } 192 | } 193 | 194 | func TestNameMap3(t *testing.T) { 195 | remapping := NameMap{ 196 | "/foo": "bar", 197 | } 198 | 199 | resolver := newNameResolver("/", "mynode", remapping) 200 | var result string 201 | 202 | result = resolver.remap("foo") 203 | if result != "/bar" { 204 | t.Error(result) 205 | } 206 | 207 | result = resolver.remap("/foo") 208 | if result != "/bar" { 209 | t.Error(result) 210 | } 211 | } 212 | 213 | func TestNameMap4(t *testing.T) { 214 | remapping := NameMap{ 215 | "/foo": "bar", 216 | } 217 | 218 | resolver := newNameResolver("/baz", "mynode", remapping) 219 | var result string 220 | 221 | result = resolver.remap("/foo") 222 | if result != "/baz/bar" { 223 | t.Error(resolver.mapping) 224 | t.Error(result) 225 | } 226 | } 227 | 228 | func TestNameMap5(t *testing.T) { 229 | remapping := NameMap{ 230 | "/foo": "/a/b/c/bar", 231 | } 232 | 233 | resolver := newNameResolver("/baz", "mynode", remapping) 234 | var result string 235 | 236 | result = resolver.remap("/foo") 237 | if result != "/a/b/c/bar" { 238 | t.Error(result) 239 | } 240 | } 241 | 242 | func TestGetNamespace(t *testing.T) { 243 | var ns string 244 | ns = getNamespace("") 245 | if ns != "/" { 246 | t.Error(ns) 247 | } 248 | 249 | ns = getNamespace("/") 250 | if ns != "/" { 251 | t.Error(ns) 252 | } 253 | 254 | ns = getNamespace("/foo") 255 | if ns != "/" { 256 | t.Error(ns) 257 | } 258 | 259 | ns = getNamespace("/foo/") 260 | if ns != "/" { 261 | t.Error(ns) 262 | } 263 | 264 | ns = getNamespace("/foo/bar") 265 | if ns != "/foo/" { 266 | t.Error(ns) 267 | } 268 | 269 | ns = getNamespace("/foo/bar/baz") 270 | if ns != "/foo/bar/" { 271 | t.Error(ns) 272 | } 273 | } 274 | 275 | func TestProcessArguments(t *testing.T) { 276 | args := []string{ 277 | "foo:=bar", 278 | "_param:=value", 279 | "__master:=http://localhost:11311", 280 | "foo", 281 | "42", 282 | } 283 | 284 | mapping, params, specials, rest := processArguments(args) 285 | if mapping["foo"] != "bar" { 286 | t.Fail() 287 | } 288 | if params["param"] != "value" { 289 | t.Fail() 290 | } 291 | if specials["__master"] != "http://localhost:11311" { 292 | t.Fail() 293 | } 294 | if len(rest) != 2 { 295 | t.Fail() 296 | } 297 | if rest[0] != "foo" || rest[1] != "42" { 298 | t.Fail() 299 | } 300 | } 301 | -------------------------------------------------------------------------------- /ros/network.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "net" 5 | "os" 6 | "strings" 7 | ) 8 | 9 | func determineHost() (string, bool) { 10 | // If the user set ROS_HOSTNAME, use it as is 11 | if rosHostname, ok := os.LookupEnv("ROS_HOSTNAME"); ok { 12 | return rosHostname, (rosHostname == "localhost") 13 | } 14 | 15 | // If the user set ROS_IP, use it as is 16 | if rosIP, ok := os.LookupEnv("ROS_IP"); ok { 17 | return rosIP, (rosIP == "::1" || strings.HasPrefix(rosIP, "127.")) 18 | } 19 | 20 | // Try using the hostname 21 | if osHostname, err := os.Hostname(); err == nil && osHostname != "localhost" { 22 | return osHostname, false 23 | } 24 | 25 | // Fall back on the interface IP 26 | if addrs, err := net.InterfaceAddrs(); err == nil { 27 | for _, addr := range addrs { 28 | if ipnet, ok := addr.(*net.IPNet); ok && !ipnet.IP.IsLoopback() { 29 | return ipnet.IP.String(), false 30 | } 31 | } 32 | } 33 | // Fall back to the loopback UP 34 | return "127.0.0.1", true 35 | } 36 | -------------------------------------------------------------------------------- /ros/network_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "os" 5 | "testing" 6 | ) 7 | 8 | func TestDetermineHost(t *testing.T) { 9 | os.Unsetenv("ROS_HOSTNAME") 10 | os.Unsetenv("ROS_IP") 11 | 12 | var host string 13 | var localOnly bool 14 | 15 | // ROS_HOSTNAME: localhost, ROS_IP: nil 16 | os.Setenv("ROS_HOSTNAME", "localhost") 17 | host, localOnly = determineHost() 18 | if host != "localhost" { 19 | t.Error("ROS_HOSTNAME is not addressed") 20 | } 21 | if localOnly != true { 22 | t.Errorf("localOnly flag is wrong for %s", host) 23 | } 24 | 25 | // ROS_HOSTNAME: hostname.in.env.var, ROS_IP: nil 26 | os.Setenv("ROS_HOSTNAME", "hostname.in.env.var") 27 | host, localOnly = determineHost() 28 | if host != "hostname.in.env.var" { 29 | t.Error("ROS_HOSTNAME is not addressed") 30 | } 31 | if localOnly != false { 32 | t.Errorf("localOnly flag is wrong for %s", host) 33 | } 34 | 35 | // ROS_HOSTNAME: hostname.in.env.var, ROS_IP: 1.2.3.4 36 | os.Setenv("ROS_IP", "1.2.3.4") 37 | host, localOnly = determineHost() 38 | if host != "hostname.in.env.var" { 39 | t.Error("ROS_HOSTNAME is not addressed when ROS_IP is set") 40 | } 41 | if localOnly != false { 42 | t.Errorf("localOnly flag is wrong for %s", host) 43 | } 44 | 45 | // ROS_HOSTNAME: nil, ROS_IP: 1.2.3.4 46 | os.Unsetenv("ROS_HOSTNAME") 47 | host, localOnly = determineHost() 48 | if host != "1.2.3.4" { 49 | t.Error("ROS_IP is not addressed") 50 | } 51 | if localOnly != false { 52 | t.Errorf("localOnly flag is wrong for %s", host) 53 | } 54 | 55 | // ROS_HOSTNAME: nil, ROS_IP: 127.0.0.1 56 | os.Setenv("ROS_IP", "127.0.0.1") 57 | host, localOnly = determineHost() 58 | if host != "127.0.0.1" { 59 | t.Error("ROS_HOSTNAME is not addressed when ROS_IP is set") 60 | } 61 | if localOnly != true { 62 | t.Errorf("localOnly flag is wrong for %s", host) 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /ros/node_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "testing" 5 | ) 6 | 7 | func TestLoadJsonFromString(t *testing.T) { 8 | value, err := loadParamFromString("42") 9 | if err != nil { 10 | t.Error(err) 11 | } 12 | i, ok := value.(float64) 13 | if !ok { 14 | t.Fail() 15 | } 16 | if i != 42.0 { 17 | t.Error(i) 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /ros/publisher.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "bytes" 5 | "encoding/binary" 6 | "encoding/hex" 7 | "errors" 8 | "fmt" 9 | "net" 10 | "sync" 11 | "time" 12 | ) 13 | 14 | type remoteSubscriberSessionError struct { 15 | session *remoteSubscriberSession 16 | err error 17 | } 18 | 19 | func (e *remoteSubscriberSessionError) Error() string { 20 | return fmt.Sprintf("remoteSubscriberSession: %s topic: %s error: %v", 21 | e.session.callerID, e.session.topic, e.err) 22 | } 23 | 24 | type defaultPublisher struct { 25 | node *defaultNode 26 | topic string 27 | msgType MessageType 28 | msgChan chan []byte 29 | shutdownChan chan struct{} 30 | sesssionIDCount int 31 | sessions map[int]*remoteSubscriberSession 32 | sessionChan chan *remoteSubscriberSession 33 | sessionErrorChan chan error 34 | listenerErrorChan chan error 35 | listener net.Listener 36 | connectCallback func(SingleSubscriberPublisher) 37 | disconnectCallback func(SingleSubscriberPublisher) 38 | } 39 | 40 | func newDefaultPublisher(node *defaultNode, 41 | topic string, msgType MessageType, 42 | connectCallback, disconnectCallback func(SingleSubscriberPublisher)) *defaultPublisher { 43 | pub := new(defaultPublisher) 44 | pub.node = node 45 | pub.topic = topic 46 | pub.msgType = msgType 47 | pub.shutdownChan = make(chan struct{}, 10) 48 | pub.sessions = make(map[int]*remoteSubscriberSession) 49 | pub.msgChan = make(chan []byte, 10) 50 | pub.listenerErrorChan = make(chan error, 10) 51 | pub.sessionChan = make(chan *remoteSubscriberSession, 10) 52 | pub.sessionErrorChan = make(chan error, 10) 53 | pub.connectCallback = connectCallback 54 | pub.disconnectCallback = disconnectCallback 55 | if listener, err := net.Listen("tcp", ":0"); err != nil { 56 | panic(err) 57 | } else { 58 | pub.listener = listener 59 | } 60 | return pub 61 | } 62 | 63 | func (pub *defaultPublisher) start(wg *sync.WaitGroup) { 64 | logger := pub.node.logger 65 | logger.Debugf("Publisher goroutine for %s started.", pub.topic) 66 | wg.Add(1) 67 | defer func() { 68 | logger.Debug("defaultPublisher.start exit") 69 | wg.Done() 70 | }() 71 | 72 | go pub.listenRemoteSubscriber() 73 | 74 | for { 75 | logger.Debug("defaultPublisher.start loop") 76 | select { 77 | case msg := <-pub.msgChan: 78 | logger.Debug("Receive msgChan") 79 | for _, s := range pub.sessions { 80 | session := s 81 | session.msgChan <- msg 82 | } 83 | 84 | case err := <-pub.listenerErrorChan: 85 | logger.Debug("Listener closed unexpectedly: %s", err) 86 | pub.listener.Close() 87 | return 88 | 89 | case s := <-pub.sessionChan: 90 | pub.sessions[s.id] = s 91 | go s.start() 92 | 93 | case err := <-pub.sessionErrorChan: 94 | logger.Error(err) 95 | if sessionError, ok := err.(*remoteSubscriberSessionError); ok { 96 | id := sessionError.session.id 97 | delete(pub.sessions, id) 98 | } 99 | 100 | case <-pub.shutdownChan: 101 | logger.Debug("defaultPublisher.start Receive shutdownChan") 102 | pub.listener.Close() 103 | logger.Debug("defaultPublisher.start closed listener") 104 | _, err := callRosAPI(pub.node.masterURI, "unregisterPublisher", pub.node.qualifiedName, pub.topic, pub.node.xmlrpcURI) 105 | if err != nil { 106 | logger.Warn(err) 107 | } 108 | 109 | for id, s := range pub.sessions { 110 | s.quitChan <- struct{}{} 111 | delete(pub.sessions, id) 112 | } 113 | return 114 | } 115 | } 116 | } 117 | 118 | func (pub *defaultPublisher) listenRemoteSubscriber() { 119 | logger := pub.node.logger 120 | logger.Debugf("Start listen %s.", pub.listener.Addr().String()) 121 | defer func() { 122 | logger.Debug("defaultPublisher.listenRemoteSubscriber exit") 123 | }() 124 | 125 | for { 126 | logger.Debug("defaultPublisher.listenRemoteSubscriber loop") 127 | conn, err := pub.listener.Accept() 128 | if err != nil { 129 | logger.Debugf("pub.listner.Accept() failed") 130 | pub.listenerErrorChan <- err 131 | close(pub.listenerErrorChan) 132 | logger.Debugf("defaultPublisher.listenRemoteSubscriber loop exit") 133 | return 134 | } 135 | 136 | logger.Debugf("Connected %s", conn.RemoteAddr().String()) 137 | id := pub.sesssionIDCount 138 | pub.sesssionIDCount++ 139 | session := newRemoteSubscriberSession(pub, id, conn) 140 | pub.sessionChan <- session 141 | } 142 | } 143 | 144 | func (pub *defaultPublisher) Publish(msg Message) { 145 | var buf bytes.Buffer 146 | _ = msg.Serialize(&buf) 147 | pub.msgChan <- buf.Bytes() 148 | } 149 | 150 | func (pub *defaultPublisher) GetNumSubscribers() int { 151 | return len(pub.sessions) 152 | } 153 | 154 | func (pub *defaultPublisher) Shutdown() { 155 | pub.shutdownChan <- struct{}{} 156 | } 157 | 158 | func (pub *defaultPublisher) hostAndPort() (string, string) { 159 | _, port, err := net.SplitHostPort(pub.listener.Addr().String()) 160 | if err != nil { 161 | // Not reached 162 | panic(err) 163 | } 164 | return pub.node.hostname, port 165 | } 166 | 167 | type remoteSubscriberSession struct { 168 | id int 169 | conn net.Conn 170 | nodeID string 171 | callerID string 172 | topic string 173 | typeText string 174 | md5sum string 175 | typeName string 176 | sizeBytesSent uint32 177 | msgBytesSent uint32 178 | numSent int64 179 | quitChan chan struct{} 180 | msgChan chan []byte 181 | errorChan chan error 182 | logger Logger 183 | connectCallback func(SingleSubscriberPublisher) 184 | disconnectCallback func(SingleSubscriberPublisher) 185 | } 186 | 187 | func newRemoteSubscriberSession(pub *defaultPublisher, id int, conn net.Conn) *remoteSubscriberSession { 188 | session := new(remoteSubscriberSession) 189 | session.id = id 190 | session.conn = conn 191 | session.nodeID = pub.node.qualifiedName 192 | session.topic = pub.topic 193 | session.typeText = pub.msgType.Text() 194 | session.md5sum = pub.msgType.MD5Sum() 195 | session.typeName = pub.msgType.Name() 196 | session.sizeBytesSent = 0 197 | session.msgBytesSent = 0 198 | session.numSent = 0 199 | session.quitChan = make(chan struct{}) 200 | session.msgChan = make(chan []byte, 10) 201 | session.errorChan = pub.sessionErrorChan 202 | session.logger = pub.node.logger 203 | session.connectCallback = pub.connectCallback 204 | session.disconnectCallback = pub.disconnectCallback 205 | return session 206 | } 207 | 208 | type singleSubPub struct { 209 | subName string 210 | topic string 211 | msgChan chan []byte 212 | } 213 | 214 | func (ssp *singleSubPub) Publish(msg Message) { 215 | var buf bytes.Buffer 216 | _ = msg.Serialize(&buf) 217 | ssp.msgChan <- buf.Bytes() 218 | } 219 | 220 | func (ssp *singleSubPub) GetSubscriberName() string { 221 | return ssp.subName 222 | } 223 | 224 | func (ssp *singleSubPub) GetTopic() string { 225 | return ssp.topic 226 | } 227 | 228 | func (session *remoteSubscriberSession) start() { 229 | logger := session.logger 230 | logger.Debug("remoteSubscriberSession.start enter") 231 | 232 | ssp := &singleSubPub{ 233 | topic: session.topic, 234 | msgChan: session.msgChan, 235 | // callerID is filled in after header gets read later in this function. 236 | } 237 | 238 | defer func() { 239 | logger.Debug("remoteSubscriberSession.start exit") 240 | 241 | if session.disconnectCallback != nil { 242 | session.disconnectCallback(ssp) 243 | } 244 | }() 245 | defer func() { 246 | if err := recover(); err != nil { 247 | if e, ok := err.(error); ok { 248 | session.errorChan <- &remoteSubscriberSessionError{session, e} 249 | } else { 250 | e = fmt.Errorf("Unkonwn error value") 251 | session.errorChan <- &remoteSubscriberSessionError{session, e} 252 | } 253 | } else { 254 | e := fmt.Errorf("Normal exit") 255 | session.errorChan <- &remoteSubscriberSessionError{session, e} 256 | } 257 | }() 258 | // 1. Read connection header 259 | headers, err := readConnectionHeader(session.conn) 260 | if err != nil { 261 | panic(errors.New("failed to read connection header")) 262 | } 263 | logger.Debug("TCPROS Connection Header:") 264 | headerMap := make(map[string]string) 265 | for _, h := range headers { 266 | headerMap[h.key] = h.value 267 | logger.Debugf(" `%s` = `%s`", h.key, h.value) 268 | } 269 | 270 | if headerMap["type"] != session.typeName && headerMap["type"] != "*" { 271 | panic(fmt.Errorf("incompatible message type: does not match for topic %s: %s vs %s", 272 | session.topic, session.typeName, headerMap["type"])) 273 | } 274 | 275 | if headerMap["md5sum"] != session.md5sum && headerMap["md5sum"] != "*" { 276 | panic(fmt.Errorf("incompatible message md5: does not match for topic %s: %s vs %s", 277 | session.topic, session.md5sum, headerMap["md5sum"])) 278 | } 279 | session.callerID = headerMap["callerid"] 280 | ssp.subName = headerMap["callerid"] 281 | if session.connectCallback != nil { 282 | go session.connectCallback(ssp) 283 | } 284 | 285 | // 2. Return reponse header 286 | var resHeaders []header 287 | resHeaders = append(resHeaders, header{"message_definition", session.typeText}) 288 | resHeaders = append(resHeaders, header{"callerid", session.nodeID}) 289 | resHeaders = append(resHeaders, header{"latching", "0"}) 290 | resHeaders = append(resHeaders, header{"md5sum", session.md5sum}) 291 | resHeaders = append(resHeaders, header{"topic", session.topic}) 292 | resHeaders = append(resHeaders, header{"type", session.typeName}) 293 | logger.Debug("TCPROS Response Header") 294 | for _, h := range resHeaders { 295 | logger.Debugf(" `%s` = `%s`", h.key, h.value) 296 | } 297 | err = writeConnectionHeader(resHeaders, session.conn) 298 | if err != nil { 299 | panic(errors.New("failed to write response header")) 300 | } 301 | 302 | // 3. Start sending message 303 | logger.Debug("Start sending messages...") 304 | queueMaxSize := 100 305 | queue := make(chan []byte, queueMaxSize) 306 | for { 307 | //logger.Debug("session.remoteSubscriberSession") 308 | select { 309 | case msg := <-session.msgChan: 310 | logger.Debug("Receive msgChan") 311 | if len(queue) == queueMaxSize { 312 | <-queue 313 | } 314 | queue <- msg 315 | 316 | case <-session.quitChan: 317 | logger.Debug("Receive quitChan") 318 | return 319 | 320 | case msg := <-queue: 321 | logger.Debug("writing") 322 | logger.Debug(hex.EncodeToString(msg)) 323 | session.conn.SetDeadline(time.Now().Add(10 * time.Millisecond)) 324 | size := uint32(len(msg)) 325 | if err := binary.Write(session.conn, binary.LittleEndian, size); err != nil { 326 | if neterr, ok := err.(net.Error); ok && neterr.Timeout() { 327 | logger.Debug("timeout") 328 | continue 329 | } else { 330 | logger.Error(err) 331 | panic(err) 332 | } 333 | } 334 | logger.Debug(len(msg)) 335 | session.conn.SetDeadline(time.Now().Add(10 * time.Millisecond)) 336 | if _, err := session.conn.Write(msg); err != nil { 337 | if neterr, ok := err.(net.Error); ok && neterr.Timeout() { 338 | logger.Debug("timeout") 339 | continue 340 | } else { 341 | logger.Error(err) 342 | panic(err) 343 | } 344 | } 345 | logger.Debug(hex.EncodeToString(msg)) 346 | } 347 | } 348 | } 349 | -------------------------------------------------------------------------------- /ros/rate.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | // Impelement Rate interface 4 | type Rate struct { 5 | actualCycleTime Duration 6 | expectedCycleTime Duration 7 | start Time 8 | } 9 | 10 | func NewRate(frequency float64) Rate { 11 | var actualCycleTime, expectedCycleTime Duration 12 | expectedCycleTime.FromSec(1.0 / frequency) 13 | start := Now() 14 | return Rate{actualCycleTime, expectedCycleTime, start} 15 | } 16 | 17 | func CycleTime(d Duration) Rate { 18 | var actualCycleTime Duration 19 | start := Now() 20 | return Rate{actualCycleTime, d, start} 21 | } 22 | 23 | func (r *Rate) CycleTime() Duration { 24 | return r.actualCycleTime 25 | } 26 | 27 | func (r *Rate) ExpectedCycleTime() Duration { 28 | return r.expectedCycleTime 29 | } 30 | 31 | func (r *Rate) Reset() { 32 | r.actualCycleTime = NewDuration(0, 0) 33 | r.start = Now() 34 | } 35 | 36 | func (r *Rate) Sleep() error { 37 | end := Now() 38 | diff := end.Diff(r.start) 39 | var remaining Duration 40 | if r.expectedCycleTime.Cmp(diff) >= 0 { 41 | remaining = r.expectedCycleTime.Sub(diff) 42 | } 43 | remaining.Sleep() 44 | now := Now() 45 | r.actualCycleTime = now.Diff(r.start) 46 | r.start = r.start.Add(r.expectedCycleTime) 47 | return nil 48 | } 49 | -------------------------------------------------------------------------------- /ros/rate_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "math/rand" 5 | "testing" 6 | "time" 7 | ) 8 | 9 | func TestNewRate(t *testing.T) { 10 | r := NewRate(100) 11 | if !r.actualCycleTime.IsZero() { 12 | t.Fail() 13 | } 14 | if r.expectedCycleTime.ToSec() != 0.01 { 15 | t.Fail() 16 | } 17 | } 18 | 19 | func TestCycleTime(t *testing.T) { 20 | const MeasureTolerance int64 = 1000000 21 | 22 | var d Duration 23 | d.FromSec(0.01) 24 | r := CycleTime(d) 25 | if !r.actualCycleTime.IsZero() { 26 | t.Fail() 27 | } 28 | if r.expectedCycleTime.ToSec() != 0.01 { 29 | t.Fail() 30 | } 31 | 32 | start := time.Now().UnixNano() 33 | r.Sleep() 34 | end := time.Now().UnixNano() 35 | 36 | actual := r.CycleTime() 37 | elapsed := end - start 38 | delta := int64(actual.ToNSec()) - elapsed 39 | if delta < 0 { 40 | delta = -delta 41 | } 42 | if delta > MeasureTolerance { 43 | t.Error(delta) 44 | } 45 | } 46 | 47 | func TestRateReset(t *testing.T) { 48 | r := NewRate(100) 49 | r.Sleep() 50 | 51 | if r.actualCycleTime.IsZero() { 52 | t.Fail() 53 | } 54 | r.Reset() 55 | if !r.actualCycleTime.IsZero() { 56 | t.Fail() 57 | } 58 | } 59 | 60 | func TestRateSleep(t *testing.T) { 61 | // The jitter tolerance (5msec) doesn't have strong basis. 62 | const JitterTolerance int64 = 5000000 63 | ct := NewDuration(0, 100000000) // 10msec 64 | r := CycleTime(ct) 65 | if ct.Cmp(r.ExpectedCycleTime()) != 0 { 66 | t.Fail() 67 | } 68 | for i := 0; i < 10; i++ { 69 | start := time.Now().UnixNano() 70 | time.Sleep(time.Duration(rand.Intn(10)) * time.Millisecond) 71 | r.Sleep() 72 | end := time.Now().UnixNano() 73 | 74 | elapsed := end - start 75 | delta := elapsed - int64(ct.ToNSec()) 76 | if delta < 0 { 77 | delta = -delta 78 | } 79 | if delta > JitterTolerance { 80 | actual := r.CycleTime() 81 | t.Errorf("expected: %d actual: %d measured: %d delta: %d", 82 | ct.ToNSec(), actual.ToNSec(), elapsed, delta) 83 | } 84 | } 85 | } 86 | -------------------------------------------------------------------------------- /ros/ros.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "time" 5 | ) 6 | 7 | // Node defines interface for a ros node 8 | type Node interface { 9 | 10 | // NewPublisher creates a publisher for specified topic and message type. 11 | NewPublisher(topic string, msgType MessageType) Publisher 12 | 13 | // NewPublisherWithCallbacks creates a publisher which gives you callbacks when subscribers 14 | // connect and disconnect. The callbacks are called in their own 15 | // goroutines, so they don't need to return immediately to let the 16 | // connection proceed. 17 | NewPublisherWithCallbacks(topic string, msgType MessageType, connectCallback, disconnectCallback func(SingleSubscriberPublisher)) Publisher 18 | 19 | // NewSubscriber creates a subscriber to specified topic, where 20 | // the messages are of a given type. callback should be a function 21 | // which takes 0, 1, or 2 arguments.If it takes 0 arguments, it will 22 | // simply be called without the message. 1-argument functions are 23 | // the normal case, and the argument should be of the generated message type. 24 | // If the function takes 2 arguments, the first argument should be of the 25 | // generated message type and the second argument should be of type MessageEvent. 26 | NewSubscriber(topic string, msgType MessageType, callback interface{}) Subscriber 27 | NewServiceClient(service string, srvType ServiceType, options ...ServiceClientOption) ServiceClient 28 | NewServiceServer(service string, srvType ServiceType, callback interface{}, options ...ServiceServerOption) ServiceServer 29 | 30 | OK() bool 31 | SpinOnce() 32 | Spin() 33 | Shutdown() 34 | 35 | GetParam(name string) (interface{}, error) 36 | SetParam(name string, value interface{}) error 37 | HasParam(name string) (bool, error) 38 | SearchParam(name string) (string, error) 39 | DeleteParam(name string) error 40 | 41 | Logger() Logger 42 | 43 | NonRosArgs() []string 44 | Name() string 45 | } 46 | 47 | // NodeOption allows to customize created nodes. 48 | type NodeOption func(n *defaultNode) 49 | 50 | // NodeServiceClientOptions specifies default options applied to the service clients created in this node. 51 | func NodeServiceClientOptions(opts ...ServiceClientOption) NodeOption { 52 | return func(n *defaultNode) { 53 | n.srvClientOpts = opts 54 | } 55 | } 56 | 57 | // NodeServiceServerOptions specifies default options applied to the service servers created in this node. 58 | func NodeServiceServerOptions(opts ...ServiceServerOption) NodeOption { 59 | return func(n *defaultNode) { 60 | n.srvServerOpts = opts 61 | } 62 | } 63 | 64 | func NewNode(name string, args []string, opts ...NodeOption) (Node, error) { 65 | return newDefaultNode(name, args, opts...) 66 | } 67 | 68 | type Publisher interface { 69 | Publish(msg Message) 70 | GetNumSubscribers() int 71 | Shutdown() 72 | } 73 | 74 | // SingleSubscriberPublisher is a publisher which only sends to one specific subscriber. 75 | // This is sent as an argument to the connect and disconnect callback 76 | // functions passed to Node.NewPublisherWithCallbacks(). 77 | type SingleSubscriberPublisher interface { 78 | Publish(msg Message) 79 | GetSubscriberName() string 80 | GetTopic() string 81 | } 82 | 83 | type Subscriber interface { 84 | GetNumPublishers() int 85 | Shutdown() 86 | } 87 | 88 | // MessageEvent is an optional second argument to a Subscriber callback. 89 | type MessageEvent struct { 90 | PublisherName string 91 | ReceiptTime time.Time 92 | ConnectionHeader map[string]string 93 | } 94 | 95 | type ServiceHandler interface{} 96 | 97 | type ServiceFactory interface { 98 | Name() string 99 | MD5Sum() string 100 | } 101 | 102 | type ServiceServer interface { 103 | Shutdown() 104 | } 105 | 106 | type ServiceClient interface { 107 | Call(srv Service) error 108 | Shutdown() 109 | } 110 | -------------------------------------------------------------------------------- /ros/serialization.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "io" 5 | ) 6 | 7 | // Reader implements io.Reader interface and provides a no-copy way to read N 8 | // bytes via Next() method. Reader is used by generated message to de-serialize 9 | // byte arrays ([]uint8) without/ copying underlying data. 10 | type Reader struct { 11 | s []byte 12 | i int 13 | } 14 | 15 | // NewReader creates new Reader and adopts the byte slice. 16 | // The caller must not modify the slice after this call. 17 | func NewReader(s []byte) *Reader { 18 | return &Reader{s, 0} 19 | } 20 | 21 | // Read implements the io.Reader interface. Like the other reader 22 | // implementations, this implementation copies data from the original slice 23 | // into "b". 24 | func (r *Reader) Read(b []byte) (n int, err error) { 25 | if r.i >= len(r.s) { 26 | return 0, io.EOF 27 | } 28 | n = copy(b, r.s[r.i:]) 29 | r.i += n 30 | return 31 | } 32 | 33 | // Next returns a slice containing the next n bytes from the buffer, advancing 34 | // the buffer as if the bytes had been returned by Read. The resulting slice is 35 | // a sub-slice of the original slice. 36 | // 37 | // Asking for more bytes than available would returns only the remaining bytes. 38 | // Calling Next on an empty buffer, or after the buffer has been exhausted, 39 | // returns an empty slice. 40 | func (r *Reader) Next(n int) []byte { 41 | m := len(r.s) - r.i 42 | if n > m { 43 | n = m 44 | } 45 | data := r.s[r.i : r.i+n] 46 | r.i += n 47 | return data 48 | } 49 | 50 | // Let implements the length remaining in the buffer 51 | func (r *Reader) Len() int { 52 | return len(r.s) - r.i 53 | } 54 | -------------------------------------------------------------------------------- /ros/serialization_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "io" 5 | "testing" 6 | 7 | "google3/third_party/golang/cmp/cmp" 8 | ) 9 | 10 | func TestReader(t *testing.T) { 11 | type nextOp struct { 12 | n int 13 | want []byte 14 | } 15 | type readOp struct { 16 | buffer []byte 17 | want []byte 18 | wantN int 19 | wantErr error 20 | } 21 | type op struct { 22 | read *readOp 23 | next *nextOp 24 | } 25 | 26 | for _, tc := range []struct { 27 | name string 28 | data []byte 29 | ops []op 30 | }{ 31 | { 32 | name: "ReadAll", 33 | data: []byte{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24}, 34 | ops: []op{ 35 | {read: &readOp{ 36 | buffer: make([]byte, 8), 37 | want: []byte{1, 2, 3, 4, 5, 6, 7, 8}, 38 | wantN: 8, 39 | }}, 40 | {read: &readOp{ 41 | buffer: make([]byte, 8), 42 | want: []byte{9, 10, 11, 12, 13, 14, 15, 16}, 43 | wantN: 8, 44 | }}, 45 | {read: &readOp{ 46 | buffer: make([]byte, 16), 47 | want: []byte{17, 18, 19, 20, 21, 22, 23, 24, 0, 0, 0, 0, 0, 0, 0, 0}, 48 | wantN: 8, 49 | }}, 50 | {read: &readOp{ 51 | buffer: make([]byte, 1), 52 | want: []byte{0}, 53 | wantN: 0, 54 | wantErr: io.EOF, 55 | }}, 56 | }, 57 | }, 58 | { 59 | name: "NextAll", 60 | data: []byte{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24}, 61 | ops: []op{ 62 | {next: &nextOp{ 63 | n: 8, 64 | want: []byte{1, 2, 3, 4, 5, 6, 7, 8}, 65 | }}, 66 | {next: &nextOp{ 67 | n: 8, 68 | want: []byte{9, 10, 11, 12, 13, 14, 15, 16}, 69 | }}, 70 | {next: &nextOp{ 71 | n: 16, 72 | want: []byte{17, 18, 19, 20, 21, 22, 23, 24}, 73 | }}, 74 | {next: &nextOp{ 75 | n: 8, 76 | want: []byte{}, 77 | }}, 78 | }, 79 | }, 80 | { 81 | name: "ReadAndNextAll", 82 | data: []byte{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24}, 83 | ops: []op{ 84 | {read: &readOp{ 85 | buffer: make([]byte, 8), 86 | want: []byte{1, 2, 3, 4, 5, 6, 7, 8}, 87 | wantN: 8, 88 | }}, 89 | {next: &nextOp{ 90 | n: 8, 91 | want: []byte{9, 10, 11, 12, 13, 14, 15, 16}, 92 | }}, 93 | {read: &readOp{ 94 | buffer: make([]byte, 16), 95 | want: []byte{17, 18, 19, 20, 21, 22, 23, 24, 0, 0, 0, 0, 0, 0, 0, 0}, 96 | wantN: 8, 97 | }}, 98 | {next: &nextOp{ 99 | n: 8, 100 | want: []byte{}, 101 | }}, 102 | }, 103 | }, 104 | { 105 | name: "ReadEmtpyBuffer", 106 | data: []byte{}, 107 | ops: []op{ 108 | {read: &readOp{ 109 | buffer: make([]byte, 1), 110 | want: []byte{0}, 111 | wantN: 0, 112 | wantErr: io.EOF, 113 | }}, 114 | }, 115 | }, 116 | { 117 | name: "ReadEmtpyBufferEmptyRead", 118 | data: []byte{}, 119 | ops: []op{ 120 | {read: &readOp{ 121 | buffer: make([]byte, 0), 122 | want: []byte{}, 123 | wantN: 0, 124 | wantErr: io.EOF, 125 | }}, 126 | }, 127 | }, 128 | { 129 | name: "NextEmtpyBuffer", 130 | data: []byte{}, 131 | ops: []op{ 132 | {next: &nextOp{ 133 | n: 1, 134 | want: []byte{}, 135 | }}, 136 | }, 137 | }, 138 | { 139 | name: "NextEmtpyBufferZeroBytes", 140 | data: []byte{}, 141 | ops: []op{ 142 | {next: &nextOp{ 143 | n: 0, 144 | want: []byte{}, 145 | }}, 146 | }, 147 | }, 148 | } { 149 | tc := tc 150 | t.Run(tc.name, func(t *testing.T) { 151 | r := NewReader(tc.data) 152 | for _, op := range tc.ops { 153 | if op.read != nil { 154 | n, err := r.Read(op.read.buffer) 155 | 156 | if diff := cmp.Diff(op.read.want, op.read.buffer); diff != "" { 157 | t.Errorf("Read(%v) read unexpected bytes: diff (-want +got): %v\nreader: %+v", op.read.buffer, diff, r) 158 | } 159 | 160 | if op.read.wantN != n { 161 | t.Errorf("Read(%v) read unexpected number of bytes: got %v, want %v\nreader: %+v", op.read.buffer, n, op.read.wantN, r) 162 | } 163 | 164 | if op.read.wantErr != err { 165 | t.Errorf("Read(%v) returned unexpected error: got %v, want %v\nreader: %+v", op.read.buffer, err, op.read.wantErr, r) 166 | } 167 | } else if op.next != nil { 168 | got := r.Next(op.next.n) 169 | if diff := cmp.Diff(op.next.want, got); diff != "" { 170 | t.Errorf("Next(%v) returned unexpected bytes: diff (-want +got): %v\nreader: %+v", op.next.n, diff, r) 171 | } 172 | } 173 | } 174 | }) 175 | } 176 | } 177 | -------------------------------------------------------------------------------- /ros/service.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | type ServiceType interface { 4 | MD5Sum() string 5 | Name() string 6 | RequestType() MessageType 7 | ResponseType() MessageType 8 | NewService() Service 9 | } 10 | 11 | type Service interface { 12 | ReqMessage() Message 13 | ResMessage() Message 14 | } 15 | -------------------------------------------------------------------------------- /ros/service_client.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "bytes" 5 | "encoding/binary" 6 | "errors" 7 | "fmt" 8 | "io" 9 | "net" 10 | "net/url" 11 | "time" 12 | ) 13 | 14 | type defaultServiceClient struct { 15 | logger Logger 16 | service string 17 | srvType ServiceType 18 | masterURI string 19 | nodeID string 20 | tcpTimeout time.Duration 21 | } 22 | 23 | func newDefaultServiceClient(logger Logger, nodeID string, masterURI string, service string, srvType ServiceType, options ...ServiceClientOption) *defaultServiceClient { 24 | client := new(defaultServiceClient) 25 | client.logger = logger 26 | client.service = service 27 | client.srvType = srvType 28 | client.masterURI = masterURI 29 | client.nodeID = nodeID 30 | client.tcpTimeout = 10 * time.Millisecond 31 | 32 | for _, option := range options { 33 | option(client) 34 | } 35 | return client 36 | } 37 | 38 | func (c *defaultServiceClient) Call(srv Service) error { 39 | logger := c.logger 40 | 41 | result, err := callRosAPI(c.masterURI, "lookupService", c.nodeID, c.service) 42 | if err != nil { 43 | return err 44 | } 45 | 46 | serviceRawURL, converted := result.(string) 47 | if !converted { 48 | return fmt.Errorf("Result of 'lookupService' is not a string") 49 | } 50 | var serviceURL *url.URL 51 | serviceURL, err = url.Parse(serviceRawURL) 52 | if err != nil { 53 | return err 54 | } 55 | 56 | var conn net.Conn 57 | conn, err = net.Dial("tcp", serviceURL.Host) 58 | if err != nil { 59 | return err 60 | } 61 | 62 | // 1. Write connection header 63 | var headers []header 64 | md5sum := c.srvType.MD5Sum() 65 | msgType := c.srvType.Name() 66 | headers = append(headers, header{"service", c.service}) 67 | headers = append(headers, header{"md5sum", md5sum}) 68 | headers = append(headers, header{"type", msgType}) 69 | headers = append(headers, header{"callerid", c.nodeID}) 70 | logger.Debug("TCPROS Connection Header") 71 | for _, h := range headers { 72 | logger.Debugf(" `%s` = `%s`", h.key, h.value) 73 | } 74 | conn.SetDeadline(time.Now().Add(c.tcpTimeout)) 75 | if err := writeConnectionHeader(headers, conn); err != nil { 76 | return err 77 | } 78 | 79 | // 2. Read reponse header 80 | conn.SetDeadline(time.Now().Add(c.tcpTimeout)) 81 | if resHeaders, err := readConnectionHeader(conn); err != nil { 82 | return err 83 | } else { 84 | logger.Debug("TCPROS Response Header:") 85 | resHeaderMap := make(map[string]string) 86 | for _, h := range resHeaders { 87 | resHeaderMap[h.key] = h.value 88 | logger.Debugf(" `%s` = `%s`", h.key, h.value) 89 | } 90 | if resHeaderMap["type"] != msgType || resHeaderMap["md5sum"] != md5sum { 91 | logger.Fatalf("Incompatible message type!") 92 | } 93 | logger.Debug("Start receiving messages...") 94 | } 95 | 96 | // 3. Send request 97 | var buf bytes.Buffer 98 | _ = srv.ReqMessage().Serialize(&buf) 99 | reqMsg := buf.Bytes() 100 | size := uint32(len(reqMsg)) 101 | conn.SetDeadline(time.Now().Add(c.tcpTimeout)) 102 | if err := binary.Write(conn, binary.LittleEndian, size); err != nil { 103 | return err 104 | } 105 | logger.Debug(len(reqMsg)) 106 | conn.SetDeadline(time.Now().Add(c.tcpTimeout)) 107 | if _, err := conn.Write(reqMsg); err != nil { 108 | return err 109 | } 110 | 111 | // 4. Read OK byte 112 | var ok byte 113 | conn.SetDeadline(time.Now().Add(c.tcpTimeout)) 114 | if err := binary.Read(conn, binary.LittleEndian, &ok); err != nil { 115 | return err 116 | } else { 117 | if ok == 0 { 118 | var size uint32 119 | conn.SetDeadline(time.Now().Add(c.tcpTimeout)) 120 | if err := binary.Read(conn, binary.LittleEndian, &size); err != nil { 121 | return err 122 | } 123 | errMsg := make([]byte, int(size)) 124 | conn.SetDeadline(time.Now().Add(c.tcpTimeout)) 125 | if _, err := io.ReadFull(conn, errMsg); err != nil { 126 | return err 127 | } else { 128 | return errors.New(string(errMsg)) 129 | } 130 | } 131 | } 132 | 133 | // 5. Receive response 134 | conn.SetDeadline(time.Now().Add(c.tcpTimeout)) 135 | //logger.Debug("Reading message size...") 136 | var msgSize uint32 137 | if err := binary.Read(conn, binary.LittleEndian, &msgSize); err != nil { 138 | return err 139 | } 140 | logger.Debugf(" %d", msgSize) 141 | resBuffer := make([]byte, int(msgSize)) 142 | //logger.Debug("Reading message body...") 143 | if _, err = io.ReadFull(conn, resBuffer); err != nil { 144 | return err 145 | } 146 | resReader := NewReader(resBuffer) 147 | if err := srv.ResMessage().Deserialize(resReader); err != nil { 148 | return err 149 | } 150 | return nil 151 | } 152 | 153 | func (*defaultServiceClient) Shutdown() {} 154 | -------------------------------------------------------------------------------- /ros/service_server.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "bytes" 5 | "container/list" 6 | "encoding/binary" 7 | "fmt" 8 | "io" 9 | "net" 10 | "reflect" 11 | "time" 12 | ) 13 | 14 | type serviceResult struct { 15 | srv Service 16 | err error 17 | } 18 | 19 | type remoteClientSessionCloseEvent struct { 20 | session *remoteClientSession 21 | err error 22 | } 23 | 24 | type defaultServiceServer struct { 25 | node *defaultNode 26 | service string 27 | srvType ServiceType 28 | handler interface{} 29 | listener *net.TCPListener 30 | rosrpcAddr string 31 | sessions *list.List 32 | shutdownChan chan struct{} 33 | sessionCloseChan chan *remoteClientSessionCloseEvent 34 | tcpTimeout time.Duration 35 | } 36 | 37 | func newDefaultServiceServer(node *defaultNode, service string, srvType ServiceType, handler interface{}, opts ...ServiceServerOption) *defaultServiceServer { 38 | logger := node.logger 39 | server := new(defaultServiceServer) 40 | if listener, err := net.Listen("tcp", ":0"); err != nil { 41 | panic(err) 42 | } else { 43 | if tcpListener, ok := listener.(*net.TCPListener); ok { 44 | server.listener = tcpListener 45 | } else { 46 | panic(fmt.Errorf("Server listener is not TCPListener")) 47 | } 48 | } 49 | server.node = node 50 | server.service = service 51 | server.srvType = srvType 52 | server.handler = handler 53 | server.tcpTimeout = 10 * time.Millisecond 54 | for _, option := range opts { 55 | option(server) 56 | } 57 | 58 | server.sessions = list.New() 59 | server.shutdownChan = make(chan struct{}, 10) 60 | server.sessionCloseChan = make(chan *remoteClientSessionCloseEvent, 10) 61 | _, port, err := net.SplitHostPort(server.listener.Addr().String()) 62 | if err != nil { 63 | // Not reached 64 | panic(err) 65 | } 66 | server.rosrpcAddr = fmt.Sprintf("rosrpc://%s:%s", node.hostname, port) 67 | logger.Debugf("ServiceServer listen %s", server.rosrpcAddr) 68 | _, err = callRosAPI(node.masterURI, "registerService", 69 | node.qualifiedName, 70 | service, 71 | server.rosrpcAddr, 72 | node.xmlrpcURI) 73 | if err != nil { 74 | logger.Errorf("Failed to register service %s", service) 75 | server.listener.Close() 76 | return nil 77 | } 78 | go server.start() 79 | return server 80 | } 81 | 82 | func (s *defaultServiceServer) Shutdown() { 83 | s.shutdownChan <- struct{}{} 84 | } 85 | 86 | // event loop 87 | func (s *defaultServiceServer) start() { 88 | logger := s.node.logger 89 | logger.Debugf("service server '%s' start listen %s.", s.service, s.listener.Addr().String()) 90 | s.node.waitGroup.Add(1) 91 | defer func() { 92 | logger.Debug("defaultServiceServer.start exit") 93 | s.node.waitGroup.Done() 94 | }() 95 | 96 | for { 97 | //logger.Debug("defaultServiceServer.start loop"); 98 | s.listener.SetDeadline(time.Now().Add(1 * time.Millisecond)) 99 | if conn, err := s.listener.Accept(); err != nil { 100 | opError, ok := err.(*net.OpError) 101 | if !ok || !opError.Timeout() { 102 | logger.Debugf("s.listner.Accept() failed") 103 | return 104 | } 105 | } else { 106 | logger.Debugf("Connected from %s", conn.RemoteAddr().String()) 107 | session := newRemoteClientSession(s, conn) 108 | s.sessions.PushBack(session) 109 | go session.start() 110 | } 111 | 112 | timeoutChan := time.After(1 * time.Millisecond) 113 | select { 114 | case ev := <-s.sessionCloseChan: 115 | if ev.err != nil { 116 | logger.Error("session error: %v", ev.err) 117 | } 118 | for e := s.sessions.Front(); e != nil; e = e.Next() { 119 | if e.Value == ev.session { 120 | logger.Debugf("service session %v removed", e.Value) 121 | s.sessions.Remove(e) 122 | break 123 | } 124 | } 125 | case <-s.shutdownChan: 126 | logger.Debug("defaultServiceServer.start Receive shutdownChan") 127 | s.listener.Close() 128 | logger.Debug("defaultServiceServer.start closed listener") 129 | _, err := callRosAPI(s.node.masterURI, "unregisterService", 130 | s.node.qualifiedName, s.service, s.rosrpcAddr) 131 | if err != nil { 132 | logger.Warn("Failed unregisterService(%s): %v", s.service, err) 133 | } 134 | logger.Debugf("Called unregisterService(%s)", s.service) 135 | for e := s.sessions.Front(); e != nil; e = e.Next() { 136 | session := e.Value.(*remoteClientSession) 137 | session.quitChan <- struct{}{} 138 | } 139 | s.sessions.Init() // Clear all sessions 140 | logger.Debug("defaultServiceServer.start session cleared") 141 | return 142 | case <-timeoutChan: 143 | break 144 | } 145 | } 146 | } 147 | 148 | type remoteClientSession struct { 149 | server *defaultServiceServer 150 | conn net.Conn 151 | quitChan chan struct{} 152 | responseChan chan []byte 153 | errorChan chan error 154 | tcpTimeout time.Duration 155 | } 156 | 157 | func newRemoteClientSession(s *defaultServiceServer, conn net.Conn) *remoteClientSession { 158 | session := new(remoteClientSession) 159 | session.server = s 160 | session.conn = conn 161 | session.quitChan = make(chan struct{}, 1) 162 | session.responseChan = make(chan []byte) 163 | session.errorChan = make(chan error) 164 | session.tcpTimeout = s.tcpTimeout 165 | return session 166 | } 167 | 168 | func (s *remoteClientSession) start() { 169 | logger := s.server.node.logger 170 | conn := s.conn 171 | nodeID := s.server.node.qualifiedName 172 | service := s.server.service 173 | md5sum := s.server.srvType.MD5Sum() 174 | srvType := s.server.srvType.Name() 175 | var err error 176 | logger.Debugf("remoteClientSession.start '%s'", s.server.service) 177 | defer func() { 178 | logger.Debug("remoteClientSession.start exit") 179 | }() 180 | defer func() { 181 | if err := recover(); err != nil { 182 | if e, ok := err.(error); ok { 183 | e = fmt.Errorf("remoteClientSession %v error: %v", s, e) 184 | s.server.sessionCloseChan <- &remoteClientSessionCloseEvent{s, e} 185 | } else { 186 | e = fmt.Errorf("remoteClientSession %v error: Unkonwn error value", s) 187 | s.server.sessionCloseChan <- &remoteClientSessionCloseEvent{s, e} 188 | } 189 | } else { 190 | s.server.sessionCloseChan <- &remoteClientSessionCloseEvent{s, nil} 191 | } 192 | }() 193 | 194 | // 1. Read request header 195 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 196 | reqHeader, err := readConnectionHeader(conn) 197 | if err != nil { 198 | panic(err) 199 | } 200 | reqHeaderMap := make(map[string]string) 201 | for _, h := range reqHeader { 202 | reqHeaderMap[h.key] = h.value 203 | logger.Debugf(" `%s` = `%s`", h.key, h.value) 204 | } 205 | 206 | // 2. Write response header 207 | var headers []header 208 | headers = append(headers, header{"service", service}) 209 | headers = append(headers, header{"md5sum", md5sum}) 210 | headers = append(headers, header{"type", srvType}) 211 | headers = append(headers, header{"callerid", nodeID}) 212 | logger.Debug("TCPROS Response Header") 213 | for _, h := range headers { 214 | logger.Debugf(" `%s` = `%s`", h.key, h.value) 215 | } 216 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 217 | if err := writeConnectionHeader(headers, conn); err != nil { 218 | panic(err) 219 | } 220 | 221 | if probe, ok := reqHeaderMap["probe"]; ok && probe == "1" { 222 | logger.Debug("TCPROS header 'probe' detected. Session closed") 223 | return 224 | } 225 | if reqHeaderMap["service"] != service || 226 | reqHeaderMap["md5sum"] != md5sum { 227 | logger.Fatalf("Incompatible message type!") 228 | } 229 | 230 | // 3. Read request 231 | logger.Debug("Reading message size...") 232 | var msgSize uint32 233 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 234 | if err := binary.Read(conn, binary.LittleEndian, &msgSize); err != nil { 235 | panic(err) 236 | } 237 | logger.Debugf(" %d", msgSize) 238 | resBuffer := make([]byte, int(msgSize)) 239 | logger.Debug("Reading message body...") 240 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 241 | if _, err = io.ReadFull(conn, resBuffer); err != nil { 242 | panic(err) 243 | } 244 | 245 | s.server.node.jobChan <- func() { 246 | srv := s.server.srvType.NewService() 247 | reader := NewReader(resBuffer) 248 | err := srv.ReqMessage().Deserialize(reader) 249 | if err != nil { 250 | s.errorChan <- err 251 | } 252 | args := []reflect.Value{reflect.ValueOf(srv)} 253 | fun := reflect.ValueOf(s.server.handler) 254 | results := fun.Call(args) 255 | 256 | if len(results) != 1 { 257 | logger.Debug("Service callback return type must be 'error'") 258 | s.errorChan <- err 259 | return 260 | } 261 | result := results[0] 262 | if result.IsNil() { 263 | logger.Debug("Service callback success") 264 | var buf bytes.Buffer 265 | _ = srv.ResMessage().Serialize(&buf) 266 | s.responseChan <- buf.Bytes() 267 | } else { 268 | logger.Debug("Service callback failure") 269 | if err, ok := result.Interface().(error); ok { 270 | s.errorChan <- err 271 | } else { 272 | s.errorChan <- fmt.Errorf("Service handler has invalid signature") 273 | } 274 | } 275 | } 276 | 277 | timeoutChan := time.After(1000 * time.Millisecond) 278 | select { 279 | case resMsg := <-s.responseChan: 280 | // 4. Write OK byte 281 | var ok byte = 1 282 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 283 | if err := binary.Write(conn, binary.LittleEndian, &ok); err != nil { 284 | panic(err) 285 | } 286 | // 5. Write response 287 | logger.Debug(len(resMsg)) 288 | size := uint32(len(resMsg)) 289 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 290 | if err := binary.Write(conn, binary.LittleEndian, size); err != nil { 291 | panic(err) 292 | } 293 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 294 | if _, err := conn.Write(resMsg); err != nil { 295 | panic(err) 296 | } 297 | case err := <-s.errorChan: 298 | logger.Error(err) 299 | // 4. Write OK byte 300 | var ok byte 301 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 302 | if err := binary.Write(conn, binary.LittleEndian, &ok); err != nil { 303 | panic(err) 304 | } 305 | errMsg := err.Error() 306 | size := uint32(len(errMsg)) 307 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 308 | if err := binary.Write(conn, binary.LittleEndian, size); err != nil { 309 | panic(err) 310 | } 311 | conn.SetDeadline(time.Now().Add(s.tcpTimeout)) 312 | if _, err := conn.Write([]byte(errMsg)); err != nil { 313 | panic(err) 314 | } 315 | case <-s.quitChan: 316 | s.errorChan <- fmt.Errorf("service shut down") 317 | case <-timeoutChan: 318 | panic(fmt.Errorf("service callback timeout")) 319 | } 320 | } 321 | -------------------------------------------------------------------------------- /ros/set.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | func contains(array []string, key string) bool { 4 | for _, item := range array { 5 | if item == key { 6 | return true 7 | } 8 | } 9 | return false 10 | } 11 | 12 | func unique(array []string) []string { 13 | set := map[string]bool{} 14 | for _, item := range array { 15 | set[item] = true 16 | } 17 | var result []string 18 | for k, _ := range set { 19 | result = append(result, k) 20 | } 21 | return result 22 | } 23 | 24 | func setUnion(lhs []string, rhs []string) []string { 25 | set := map[string]bool{} 26 | for _, item := range lhs { 27 | set[item] = true 28 | } 29 | for _, item := range rhs { 30 | set[item] = true 31 | } 32 | var result []string 33 | for k, _ := range set { 34 | result = append(result, k) 35 | } 36 | return result 37 | } 38 | 39 | func setDifference(lhs []string, rhs []string) []string { 40 | left := map[string]bool{} 41 | for _, item := range lhs { 42 | left[item] = true 43 | } 44 | right := map[string]bool{} 45 | for _, item := range rhs { 46 | right[item] = true 47 | } 48 | for k, _ := range right { 49 | delete(left, k) 50 | } 51 | var result []string 52 | for k, _ := range left { 53 | result = append(result, k) 54 | } 55 | return result 56 | } 57 | -------------------------------------------------------------------------------- /ros/set_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "testing" 5 | ) 6 | 7 | func TestUnique(t *testing.T) { 8 | var data = []string{ 9 | "a", "b", "c", "a", 10 | } 11 | result := unique(data) 12 | if len(result) != 3 { 13 | t.Fail() 14 | } 15 | } 16 | 17 | func TestUnion(t *testing.T) { 18 | var a = []string{ 19 | "a", "b", "c", "a", 20 | } 21 | 22 | var b = []string{ 23 | "a", "b", "d", 24 | } 25 | 26 | result := setUnion(a, b) 27 | if len(result) != 4 { 28 | t.Errorf("Expected 4 but %d", len(result)) 29 | } 30 | 31 | for _, k := range []string{"a", "b", "c", "d"} { 32 | if !contains(result, k) { 33 | t.Fail() 34 | } 35 | } 36 | } 37 | 38 | func TestDifference(t *testing.T) { 39 | var a = []string{ 40 | "a", "b", "c", "a", 41 | } 42 | 43 | var b = []string{ 44 | "a", "b", "d", "e", 45 | } 46 | 47 | result := setDifference(a, b) 48 | if len(result) != 1 { 49 | t.Errorf("Expected 1 but %d", len(result)) 50 | } 51 | for _, k := range []string{"c"} { 52 | if !contains(result, k) { 53 | t.Fail() 54 | } 55 | } 56 | 57 | result = setDifference(b, a) 58 | if len(result) != 2 { 59 | t.Errorf("Expected 2 but %d", len(result)) 60 | } 61 | for _, k := range []string{"d", "e"} { 62 | if !contains(result, k) { 63 | t.Fail() 64 | } 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /ros/subscriber.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "encoding/binary" 5 | "fmt" 6 | "io" 7 | "net" 8 | "reflect" 9 | "sync" 10 | "time" 11 | ) 12 | 13 | type messageEvent struct { 14 | bytes []byte 15 | event MessageEvent 16 | } 17 | 18 | // The subscription object runs in own goroutine (startSubscription). 19 | // Do not access any properties from other goroutine. 20 | type defaultSubscriber struct { 21 | topic string 22 | msgType MessageType 23 | pubList []string 24 | pubListChan chan []string 25 | msgChan chan messageEvent 26 | callbacks []interface{} 27 | addCallbackChan chan interface{} 28 | shutdownChan chan struct{} 29 | connections map[string]chan struct{} 30 | disconnectedChan chan string 31 | } 32 | 33 | func newDefaultSubscriber(topic string, msgType MessageType, callback interface{}) *defaultSubscriber { 34 | sub := new(defaultSubscriber) 35 | sub.topic = topic 36 | sub.msgType = msgType 37 | sub.msgChan = make(chan messageEvent, 10) 38 | sub.pubListChan = make(chan []string, 10) 39 | sub.addCallbackChan = make(chan interface{}, 10) 40 | sub.shutdownChan = make(chan struct{}, 10) 41 | sub.disconnectedChan = make(chan string, 10) 42 | sub.connections = make(map[string]chan struct{}) 43 | sub.callbacks = []interface{}{callback} 44 | return sub 45 | } 46 | 47 | func (sub *defaultSubscriber) start(wg *sync.WaitGroup, nodeID string, nodeURI string, masterURI string, jobChan chan func(), logger Logger, unregisterFromNode func()) { 48 | logger.Debugf("Subscriber goroutine for %s started.", sub.topic) 49 | wg.Add(1) 50 | defer wg.Done() 51 | defer func() { 52 | logger.Debug("defaultSubscriber.start exit") 53 | }() 54 | for { 55 | logger.Debug("Loop") 56 | select { 57 | case list := <-sub.pubListChan: 58 | logger.Debug("Receive pubListChan") 59 | deadPubs := setDifference(sub.pubList, list) 60 | newPubs := setDifference(list, sub.pubList) 61 | sub.pubList = list 62 | 63 | for _, pub := range deadPubs { 64 | quitChan := sub.connections[pub] 65 | quitChan <- struct{}{} 66 | delete(sub.connections, pub) 67 | } 68 | 69 | for _, pub := range newPubs { 70 | protocols := []interface{}{[]interface{}{"TCPROS"}} 71 | result, err := callRosAPI(pub, "requestTopic", nodeID, sub.topic, protocols) 72 | if err != nil { 73 | logger.Fatalf("[DefaultSubscriber] %v", err) 74 | continue 75 | } 76 | 77 | protocolParams := result.([]interface{}) 78 | for _, x := range protocolParams { 79 | logger.Debug(x) 80 | } 81 | 82 | name := protocolParams[0].(string) 83 | if name == "TCPROS" { 84 | addr := protocolParams[1].(string) 85 | port := protocolParams[2].(int32) 86 | uri := fmt.Sprintf("%s:%d", addr, port) 87 | quitChan := make(chan struct{}, 10) 88 | sub.connections[pub] = quitChan 89 | go startRemotePublisherConn(logger, 90 | uri, sub.topic, 91 | sub.msgType.MD5Sum(), 92 | sub.msgType.Name(), nodeID, 93 | sub.msgChan, 94 | quitChan, 95 | sub.disconnectedChan) 96 | } else { 97 | logger.Warnf("rosgo Not support protocol '%s'", name) 98 | } 99 | } 100 | 101 | case callback := <-sub.addCallbackChan: 102 | logger.Debug("Receive addCallbackChan") 103 | sub.callbacks = append(sub.callbacks, callback) 104 | 105 | case msgEvent := <-sub.msgChan: 106 | // Pop received message then bind callbacks and enqueue to the job channle. 107 | logger.Debug("Receive msgChan") 108 | callbacks := make([]interface{}, len(sub.callbacks)) 109 | copy(callbacks, sub.callbacks) 110 | jobChan <- func() { 111 | m := sub.msgType.NewMessage() 112 | reader := NewReader(msgEvent.bytes) 113 | if err := m.Deserialize(reader); err != nil { 114 | logger.Error(err) 115 | } 116 | args := []reflect.Value{reflect.ValueOf(m), reflect.ValueOf(msgEvent.event)} 117 | for _, callback := range callbacks { 118 | fun := reflect.ValueOf(callback) 119 | numArgsNeeded := fun.Type().NumIn() 120 | if numArgsNeeded <= 2 { 121 | fun.Call(args[0:numArgsNeeded]) 122 | } 123 | } 124 | } 125 | logger.Debug("Callback job enqueued.") 126 | 127 | case pubURI := <-sub.disconnectedChan: 128 | logger.Debugf("Connection to %s was disconnected.", pubURI) 129 | delete(sub.connections, pubURI) 130 | 131 | case <-sub.shutdownChan: 132 | // Shutdown subscription goroutine 133 | logger.Debug("Receive shutdownChan") 134 | for _, closeChan := range sub.connections { 135 | closeChan <- struct{}{} 136 | close(closeChan) 137 | } 138 | _, err := callRosAPI(masterURI, "unregisterSubscriber", nodeID, sub.topic, nodeURI) 139 | if err != nil { 140 | logger.Warn(err) 141 | } 142 | 143 | unregisterFromNode() 144 | return 145 | } 146 | } 147 | } 148 | 149 | func startRemotePublisherConn(logger Logger, 150 | pubURI string, topic string, md5sum string, 151 | msgType string, nodeID string, 152 | msgChan chan messageEvent, 153 | quitChan chan struct{}, 154 | disconnectedChan chan string) { 155 | logger.Debug("startRemotePublisherConn()") 156 | 157 | defer func() { 158 | logger.Debug("startRemotePublisherConn() exit") 159 | }() 160 | 161 | conn, err := net.Dial("tcp", pubURI) 162 | if err != nil { 163 | logger.Fatalf("Failed to connect %s!", pubURI) 164 | } 165 | 166 | // 1. Write connection header 167 | var headers []header 168 | headers = append(headers, header{"topic", topic}) 169 | headers = append(headers, header{"md5sum", md5sum}) 170 | headers = append(headers, header{"type", msgType}) 171 | headers = append(headers, header{"callerid", nodeID}) 172 | logger.Debug("TCPROS Connection Header") 173 | for _, h := range headers { 174 | logger.Debugf(" `%s` = `%s`", h.key, h.value) 175 | } 176 | err = writeConnectionHeader(headers, conn) 177 | if err != nil { 178 | logger.Fatal("Failed to write connection header.") 179 | } 180 | 181 | // 2. Read reponse header 182 | var resHeaders []header 183 | resHeaders, err = readConnectionHeader(conn) 184 | if err != nil { 185 | logger.Fatal("Failed to read reasponse header.") 186 | } 187 | logger.Debug("TCPROS Response Header:") 188 | resHeaderMap := make(map[string]string) 189 | for _, h := range resHeaders { 190 | resHeaderMap[h.key] = h.value 191 | logger.Debugf(" `%s` = `%s`", h.key, h.value) 192 | } 193 | 194 | if md5sum != resHeaderMap["md5sum"] && md5sum != "*" { 195 | panic("incompatible message type: md5sum mismatch") 196 | } 197 | 198 | logger.Debug("Start receiving messages...") 199 | event := MessageEvent{ // Event struct to be sent with each message. 200 | PublisherName: resHeaderMap["callerid"], 201 | ConnectionHeader: resHeaderMap, 202 | } 203 | 204 | // 3. Start reading messages 205 | readingSize := true 206 | var msgSize uint32 207 | var buffer []byte 208 | for { 209 | select { 210 | case <-quitChan: 211 | return 212 | default: 213 | conn.SetDeadline(time.Now().Add(1000 * time.Millisecond)) 214 | if readingSize { 215 | //logger.Debug("Reading message size...") 216 | err := binary.Read(conn, binary.LittleEndian, &msgSize) 217 | if err != nil { 218 | if err == io.EOF { 219 | logger.Infof("Publisher %s on topic %s disconnected", pubURI, topic) 220 | disconnectedChan <- pubURI 221 | return 222 | } 223 | if neterr, ok := err.(net.Error); ok && neterr.Timeout() { 224 | // Timed out 225 | //logger.Debug(neterr) 226 | continue 227 | } else { 228 | logger.Error("Failed to read a message size", err) 229 | disconnectedChan <- pubURI 230 | return 231 | } 232 | } 233 | logger.Debugf(" %d", msgSize) 234 | buffer = make([]byte, int(msgSize)) 235 | readingSize = false 236 | } else { 237 | //logger.Debug("Reading message body...") 238 | _, err = io.ReadFull(conn, buffer) 239 | if err != nil { 240 | if err == io.EOF { 241 | logger.Info("Publisher disconnected") 242 | disconnectedChan <- pubURI 243 | return 244 | } 245 | if neterr, ok := err.(net.Error); ok && neterr.Timeout() { 246 | // Timed out 247 | //logger.Debug(neterr) 248 | continue 249 | } else { 250 | logger.Error("Failed to read a message body", err) 251 | disconnectedChan <- pubURI 252 | return 253 | } 254 | } 255 | event.ReceiptTime = time.Now() 256 | msgChan <- messageEvent{bytes: buffer, event: event} 257 | readingSize = true 258 | } 259 | } 260 | } 261 | } 262 | 263 | func (sub *defaultSubscriber) Shutdown() { 264 | sub.shutdownChan <- struct{}{} 265 | } 266 | 267 | func (sub *defaultSubscriber) GetNumPublishers() int { 268 | return len(sub.pubList) 269 | } 270 | -------------------------------------------------------------------------------- /ros/temporal.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | const maxUint32 = int64(^uint32(0)) 4 | 5 | func normalizeTemporal(sec int64, nsec int64) (uint32, uint32) { 6 | const SecondInNanosecond = 1000000000 7 | if nsec > SecondInNanosecond { 8 | sec += nsec / SecondInNanosecond 9 | nsec = nsec % SecondInNanosecond 10 | } else if nsec < 0 { 11 | sec += nsec/SecondInNanosecond - 1 12 | nsec = nsec%SecondInNanosecond + SecondInNanosecond 13 | } 14 | 15 | if sec < 0 || sec > maxUint32 { 16 | panic("Time is out of range") 17 | } 18 | 19 | return uint32(sec), uint32(nsec) 20 | } 21 | 22 | func cmpUint64(lhs, rhs uint64) int { 23 | var result int 24 | if lhs > rhs { 25 | result = 1 26 | } else if lhs < rhs { 27 | result = -1 28 | } else { 29 | result = 0 30 | } 31 | return result 32 | } 33 | 34 | type temporal struct { 35 | Sec uint32 36 | NSec uint32 37 | } 38 | 39 | func (t *temporal) IsZero() bool { 40 | return t.Sec == 0 && t.NSec == 0 41 | } 42 | 43 | func (t *temporal) ToSec() float64 { 44 | return float64(t.Sec) + float64(t.NSec)*1e-9 45 | } 46 | 47 | func (t *temporal) ToNSec() uint64 { 48 | return uint64(t.Sec)*1000000000 + uint64(t.NSec) 49 | } 50 | 51 | func (t *temporal) FromSec(sec float64) { 52 | nsec := uint64(sec * 1e9) 53 | t.FromNSec(nsec) 54 | } 55 | 56 | func (t *temporal) FromNSec(nsec uint64) { 57 | t.Sec, t.NSec = normalizeTemporal(0, int64(nsec)) 58 | } 59 | 60 | func (t *temporal) Normalize() { 61 | t.Sec, t.NSec = normalizeTemporal(int64(t.Sec), int64(t.NSec)) 62 | } 63 | -------------------------------------------------------------------------------- /ros/temporal_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "testing" 5 | ) 6 | 7 | func TestNormalizeTemporal(t *testing.T) { 8 | var sec, nsec uint32 9 | sec, nsec = normalizeTemporal(1, 2) 10 | if sec != 1 || nsec != 2 { 11 | t.Error(sec, nsec) 12 | } 13 | 14 | sec, nsec = normalizeTemporal(1, 2000000001) 15 | if sec != 3 || nsec != 1 { 16 | t.Error(sec, nsec) 17 | } 18 | 19 | sec, nsec = normalizeTemporal(3, -2000000001) 20 | if sec != 0 || nsec != 999999999 { 21 | t.Error(sec, nsec) 22 | } 23 | } 24 | 25 | func TestTemporalIsZero(t *testing.T) { 26 | var t1 temporal 27 | if !t1.IsZero() { 28 | t.Fail() 29 | } 30 | 31 | t1.Sec = 1 32 | t1.NSec = 0 33 | if t1.IsZero() { 34 | t.Fail() 35 | } 36 | 37 | t1.Sec = 0 38 | t1.NSec = 1 39 | if t1.IsZero() { 40 | t.Fail() 41 | } 42 | 43 | t1.Sec = 1 44 | t1.NSec = 1 45 | if t1.IsZero() { 46 | t.Fail() 47 | } 48 | } 49 | 50 | func TestTemporalSet(t *testing.T) { 51 | t1 := temporal{1, 2} 52 | if t1.Sec != 1 { 53 | t.Fail() 54 | } 55 | if t1.NSec != 2 { 56 | t.Fail() 57 | } 58 | } 59 | 60 | func TestTemporalToSec(t *testing.T) { 61 | t1 := temporal{1, 500000000} 62 | if t1.ToSec() != 1.5 { 63 | t.Error(t1.ToSec()) 64 | } 65 | t1.Sec, t1.NSec = 0, 1500000000 66 | if t1.ToSec() != 1.5 { 67 | t.Error(t1.ToSec()) 68 | } 69 | } 70 | 71 | func TestTemporalToNSec(t *testing.T) { 72 | t1 := temporal{1, 500000000} 73 | if t1.ToNSec() != 1500000000 { 74 | t.Fail() 75 | } 76 | t1.Sec, t1.NSec = 0, 1500000000 77 | if t1.ToNSec() != 1500000000 { 78 | t.Fail() 79 | } 80 | } 81 | 82 | func TestTemporalFromSec(t *testing.T) { 83 | var t1 temporal 84 | t1.FromSec(1.5) 85 | if t1.Sec != 1 || t1.NSec != 500000000 { 86 | t.Fail() 87 | } 88 | } 89 | 90 | func TestTemporalFromNSec(t *testing.T) { 91 | var t1 temporal 92 | t1.FromNSec(1500000000) 93 | if t1.Sec != 1 || t1.NSec != 500000000 { 94 | t.Fail() 95 | } 96 | } 97 | -------------------------------------------------------------------------------- /ros/time.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | gotime "time" 5 | ) 6 | 7 | type Time struct { 8 | temporal 9 | } 10 | 11 | func NewTime(sec uint32, nsec uint32) Time { 12 | sec, nsec = normalizeTemporal(int64(sec), int64(nsec)) 13 | return Time{temporal{sec, nsec}} 14 | } 15 | 16 | func Now() Time { 17 | var t Time 18 | t.FromNSec(uint64(gotime.Now().UnixNano())) 19 | return t 20 | } 21 | 22 | func (t *Time) Diff(from Time) Duration { 23 | sec, nsec := normalizeTemporal(int64(t.Sec)-int64(from.Sec), 24 | int64(t.NSec)-int64(from.NSec)) 25 | return Duration{temporal{sec, nsec}} 26 | } 27 | 28 | func (t *Time) Add(d Duration) Time { 29 | sec, nsec := normalizeTemporal(int64(t.Sec)+int64(d.Sec), 30 | int64(t.NSec)+int64(d.NSec)) 31 | return Time{temporal{sec, nsec}} 32 | } 33 | 34 | func (t *Time) Sub(d Duration) Time { 35 | sec, nsec := normalizeTemporal(int64(t.Sec)-int64(d.Sec), 36 | int64(t.NSec)-int64(d.NSec)) 37 | return Time{temporal{sec, nsec}} 38 | } 39 | 40 | func (t *Time) Cmp(other Time) int { 41 | return cmpUint64(t.ToNSec(), other.ToNSec()) 42 | } 43 | -------------------------------------------------------------------------------- /ros/time_test.go: -------------------------------------------------------------------------------- 1 | package ros 2 | 3 | import ( 4 | "testing" 5 | ) 6 | 7 | func TestNewTime(t *testing.T) { 8 | t1 := NewTime(1, 2) 9 | if t1.Sec != 1 { 10 | t.Fail() 11 | } 12 | if t1.NSec != 2 { 13 | t.Fail() 14 | } 15 | } 16 | 17 | func TestTimeAdd(t *testing.T) { 18 | var t1 Time 19 | t1.FromNSec(500000000) 20 | 21 | var d Duration 22 | d.FromNSec(800000000) 23 | 24 | t2 := t1.Add(d) 25 | if t2.Sec != 1 { 26 | t.Error(t2.Sec) 27 | } 28 | if t2.NSec != 300000000 { 29 | t.Error(t2.NSec) 30 | } 31 | } 32 | 33 | func TestTimeSub(t *testing.T) { 34 | var t1 Time 35 | t1.FromNSec(1300000000) 36 | 37 | var d Duration 38 | d.FromNSec(500000000) 39 | 40 | t2 := t1.Sub(d) 41 | if t2.Sec != 0 { 42 | t.Error(t2.Sec) 43 | } 44 | if t2.NSec != 800000000 { 45 | t.Error(t2.NSec) 46 | } 47 | } 48 | 49 | func TestTimeDiff(t *testing.T) { 50 | var t1, t2 Time 51 | t1.FromNSec(1300000000) 52 | t2.FromNSec(500000000) 53 | 54 | d := t1.Diff(t2) 55 | if d.Sec != 0 { 56 | t.Error(d.Sec) 57 | } 58 | if d.NSec != 800000000 { 59 | t.Error(d.NSec) 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /test/client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from rospy_tutorials.srv import AddTwoInts 4 | import rospy 5 | import time 6 | 7 | 8 | def main(): 9 | rospy.init_node('add_two_ints_server') 10 | s = rospy.ServiceProxy('add_two_ints', AddTwoInts) 11 | print "Ready to add two ints." 12 | 13 | args = rospy.myargv() 14 | 15 | a = int(args[1]) 16 | b = int(args[2]) 17 | 18 | result = s(a, b) 19 | rospy.loginfo("{} + {} = {}".format(a, b, result.sum)) 20 | time.sleep(1.0) 21 | 22 | if __name__ == "__main__": 23 | main() 24 | 25 | -------------------------------------------------------------------------------- /test/listener.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from std_msgs.msg import String 4 | 5 | 6 | def callback(data): 7 | rospy.loginfo(rospy.get_name() + ": I heard %s" % data.data) 8 | 9 | 10 | def listener(): 11 | rospy.init_node('listener', anonymous=True, log_level=rospy.DEBUG) 12 | rospy.Subscriber("chatter", String, callback) 13 | rospy.spin() 14 | 15 | 16 | if __name__ == '__main__': 17 | listener() 18 | -------------------------------------------------------------------------------- /test/server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from rospy_tutorials.srv import AddTwoInts, AddTwoIntsResponse 4 | import rospy 5 | 6 | def handle_add_two_ints(req): 7 | print "Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)) 8 | return AddTwoIntsResponse(req.a + req.b) 9 | 10 | def add_two_ints_server(): 11 | rospy.init_node('add_two_ints_server') 12 | s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints) 13 | print "Ready to add two ints." 14 | rospy.spin() 15 | 16 | if __name__ == "__main__": 17 | add_two_ints_server() 18 | -------------------------------------------------------------------------------- /test/talker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | from std_msgs.msg import String 4 | 5 | 6 | def talker(): 7 | pub = rospy.Publisher('chatter', String) 8 | rospy.init_node('talker', anonymous=True) 9 | while not rospy.is_shutdown(): 10 | str = "%s: hello world %s" % (rospy.get_name(), rospy.get_time()) 11 | rospy.loginfo(str) 12 | pub.publish(String(str)) 13 | rospy.sleep(1.0) 14 | 15 | 16 | if __name__ == '__main__': 17 | try: 18 | talker() 19 | except rospy.ROSInterruptException: 20 | pass 21 | -------------------------------------------------------------------------------- /test/test_client/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | //go:generate gengo srv rospy_tutorials/AddTwoInts 4 | import ( 5 | "fmt" 6 | "os" 7 | "rospy_tutorials" 8 | "strconv" 9 | 10 | "github.com/fetchrobotics/rosgo/ros" 11 | ) 12 | 13 | func main() { 14 | if len(os.Args) != 3 { 15 | fmt.Print("USAGE: test_client ") 16 | os.Exit(-1) 17 | } 18 | 19 | node, err := ros.NewNode("client", os.Args) 20 | if err != nil { 21 | fmt.Println(err) 22 | os.Exit(-1) 23 | } 24 | defer node.Shutdown() 25 | logger := node.Logger() 26 | logger.SetSeverity(ros.LogLevelDebug) 27 | cli := node.NewServiceClient("/add_two_ints", rospy_tutorials.SrvAddTwoInts) 28 | defer cli.Shutdown() 29 | var a, b int64 30 | a, err = strconv.ParseInt(os.Args[1], 10, 32) 31 | if err != nil { 32 | fmt.Print(err) 33 | fmt.Println() 34 | os.Exit(1) 35 | } 36 | b, err = strconv.ParseInt(os.Args[2], 10, 32) 37 | if err != nil { 38 | fmt.Print(err) 39 | fmt.Println() 40 | os.Exit(1) 41 | } 42 | var srv rospy_tutorials.AddTwoInts 43 | srv.Request.A = a 44 | srv.Request.B = b 45 | if err = cli.Call(&srv); err != nil { 46 | fmt.Print(err) 47 | fmt.Println() 48 | } else { 49 | fmt.Printf("%d + %d = %d", 50 | srv.Request.A, srv.Request.B, srv.Response.Sum) 51 | fmt.Println() 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /test/test_listener/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | //go:generate gengo msg std_msgs/String 4 | import ( 5 | "fmt" 6 | "os" 7 | "std_msgs" 8 | 9 | "github.com/fetchrobotics/rosgo/ros" 10 | ) 11 | 12 | func callback(msg *std_msgs.String) { 13 | fmt.Printf("Received: %s\n", msg.Data) 14 | } 15 | 16 | func main() { 17 | node, err := ros.NewNode("/listener", os.Args) 18 | if err != nil { 19 | fmt.Println(err) 20 | os.Exit(-1) 21 | } 22 | defer node.Shutdown() 23 | node.Logger().SetSeverity(ros.LogLevelDebug) 24 | node.NewSubscriber("/chatter", std_msgs.MsgString, callback) 25 | node.Spin() 26 | } 27 | -------------------------------------------------------------------------------- /test/test_listener_with_event/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | //go:generate gengo msg std_msgs/String 4 | import ( 5 | "fmt" 6 | "os" 7 | "std_msgs" 8 | 9 | "github.com/fetchrobotics/rosgo/ros" 10 | ) 11 | 12 | func callback(msg *std_msgs.String, event ros.MessageEvent) { 13 | fmt.Printf("Received: %s from %s, header = %v, time = %v\n", 14 | msg.Data, event.PublisherName, event.ConnectionHeader, event.ReceiptTime) 15 | } 16 | 17 | func main() { 18 | node, err := ros.NewNode("/listener", os.Args) 19 | if err != nil { 20 | fmt.Println(err) 21 | os.Exit(-1) 22 | } 23 | defer node.Shutdown() 24 | node.Logger().SetSeverity(ros.LogLevelDebug) 25 | node.NewSubscriber("/chatter", std_msgs.MsgString, callback) 26 | node.Spin() 27 | } 28 | -------------------------------------------------------------------------------- /test/test_message/AllFieldTypes.msg: -------------------------------------------------------------------------------- 1 | byte FOO=1 2 | byte BAR=2 3 | string HOGE=hoge 4 | 5 | Header h 6 | byte b 7 | int8 i8 8 | int16 i16 9 | int32 i32 10 | int64 i64 11 | uint8 u8 12 | uint16 u16 13 | uint32 u32 14 | uint64 u64 15 | float32 f32 16 | float64 f64 17 | time t 18 | duration d 19 | string s 20 | std_msgs/ColorRGBA c 21 | uint32[] dyn_ary 22 | uint32[2] fix_ary 23 | #std_msgs/ColorRGBA[] msg_ary 24 | -------------------------------------------------------------------------------- /test/test_message/message_test.go: -------------------------------------------------------------------------------- 1 | package test_message 2 | 3 | //go:generate gengo msg rosgo_tests/AllFieldTypes AllFieldTypes.msg 4 | //go:generate gengo msg std_msgs/Header 5 | //go:generate gengo msg std_msgs/Int16 6 | //go:generate gengo msg std_msgs/Int32 7 | //go:generate gengo msg std_msgs/ColorRGBA 8 | import ( 9 | "bytes" 10 | "fmt" 11 | "rosgo_tests" 12 | "std_msgs" 13 | "testing" 14 | 15 | "github.com/fetchrobotics/rosgo/ros" 16 | ) 17 | 18 | func TestInitialize(t *testing.T) { 19 | msg := rosgo_tests.AllFieldTypes{} 20 | fmt.Println(msg) 21 | fmt.Println(msg.H) 22 | 23 | if msg.B != 0 { 24 | t.Error(msg.B) 25 | } 26 | 27 | if msg.I8 != 0 { 28 | t.Error(msg.I8) 29 | } 30 | 31 | if msg.I16 != 0 { 32 | t.Error(msg.I16) 33 | } 34 | 35 | if msg.I32 != 0 { 36 | t.Error(msg.I32) 37 | } 38 | 39 | if msg.I64 != 0 { 40 | t.Error(msg.I64) 41 | } 42 | 43 | if msg.U8 != 0 { 44 | t.Error(msg.U8) 45 | } 46 | 47 | if msg.U16 != 0 { 48 | t.Error(msg.U16) 49 | } 50 | 51 | if msg.U32 != 0 { 52 | t.Error(msg.U32) 53 | } 54 | 55 | if msg.U64 != 0 { 56 | t.Error(msg.U64) 57 | } 58 | 59 | if msg.F32 != 0.0 { 60 | t.Error(msg.F32) 61 | } 62 | 63 | if msg.F64 != 0.0 { 64 | t.Error(msg.F64) 65 | } 66 | 67 | if msg.T.Sec != 0 || msg.T.NSec != 0 { 68 | t.Error(msg.T) 69 | } 70 | 71 | if msg.D.Sec != 0 || msg.D.NSec != 0 { 72 | t.Error(msg.D) 73 | } 74 | 75 | if msg.S != "" { 76 | t.Error(msg.S) 77 | } 78 | 79 | if msg.C.R != 0.0 || msg.C.G != 0.0 || msg.C.B != 0.0 || msg.C.A != 0 { 80 | t.Error(msg.C) 81 | } 82 | 83 | if len(msg.DynAry) != 0 { 84 | t.Error(msg.DynAry) 85 | } 86 | 87 | if len(msg.FixAry) != 2 || msg.FixAry[0] != 0 || msg.FixAry[1] != 0 { 88 | t.Error(msg.FixAry) 89 | } 90 | 91 | } 92 | 93 | func CheckBytes(t *testing.T, a, b []byte) { 94 | if !bytes.Equal(a, b) { 95 | if len(a) != len(b) { 96 | t.Errorf("expected length is %d but %d", len(a), len(b)) 97 | } else { 98 | for i := 0; i < len(a); i++ { 99 | if a[i] != b[i] { 100 | t.Errorf("result[%3d] is expected to be %02X but %02X", i, a[i], b[i]) 101 | } else { 102 | t.Errorf("%02X", a[i]) 103 | } 104 | } 105 | } 106 | } 107 | } 108 | 109 | func TestSerializeHeader(t *testing.T) { 110 | var msg std_msgs.Header 111 | msg.Seq = 0x89ABCDEF 112 | msg.Stamp = ros.NewTime(0x89ABCDEF, 0x01234567) 113 | msg.FrameId = "frame_id" 114 | var buf bytes.Buffer 115 | err := msg.Serialize(&buf) 116 | if err != nil { 117 | t.Error(err) 118 | } 119 | result := buf.Bytes() 120 | expected := []byte{ 121 | // Header.Seq 122 | 0xEF, 0xCD, 0xAB, 0x89, 123 | // Header.Stamp 124 | 0xEF, 0xCD, 0xAB, 0x89, 125 | 0x67, 0x45, 0x23, 0x01, 126 | // Header.FrameId 127 | 0x08, 0x00, 0x00, 0x00, 128 | 0x66, 0x72, 0x61, 0x6D, 0x65, 0x5F, 0x69, 0x64, 129 | } 130 | CheckBytes(t, expected, result) 131 | } 132 | 133 | func TestSerializeInt16(t *testing.T) { 134 | var msg std_msgs.Int16 135 | msg.Data = 0x0123 136 | var buf bytes.Buffer 137 | err := msg.Serialize(&buf) 138 | if err != nil { 139 | t.Error(err) 140 | } 141 | result := buf.Bytes() 142 | expected := []byte{ 143 | 0x23, 0x01, 144 | } 145 | CheckBytes(t, expected, result) 146 | } 147 | 148 | func TestSerializeInt32(t *testing.T) { 149 | var msg std_msgs.Int32 150 | msg.Data = 0x01234567 151 | var buf bytes.Buffer 152 | err := msg.Serialize(&buf) 153 | if err != nil { 154 | t.Error(err) 155 | } 156 | result := buf.Bytes() 157 | expected := []byte{ 158 | 0x67, 0x45, 0x23, 0x01, 159 | } 160 | CheckBytes(t, expected, result) 161 | } 162 | 163 | func getTestData() []byte { 164 | return []byte{ 165 | // Header.Seq 166 | 0xEF, 0xCD, 0xAB, 0x89, 167 | // Header.Stamp 168 | 0xEF, 0xCD, 0xAB, 0x89, 169 | 0x67, 0x45, 0x23, 0x01, 170 | // Header.FrameId 171 | 0x08, 0x00, 0x00, 0x00, 172 | 0x66, 0x72, 0x61, 0x6D, 0x65, 0x5F, 0x69, 0x64, 173 | // B 174 | 0x01, 175 | // I8 176 | 0x01, 177 | // I16 178 | 0x23, 0x01, 179 | // I32 180 | 0x67, 0x45, 0x23, 0x01, 181 | // I64 182 | 0xEF, 0xCD, 0xAB, 0x89, 0x67, 0x45, 0x23, 0x01, 183 | // U8 184 | 0x01, 185 | // U16 186 | 0x23, 0x01, 187 | // U32 188 | 0x67, 0x45, 0x23, 0x01, 189 | // U64 190 | 0xEF, 0xCD, 0xAB, 0x89, 0x67, 0x45, 0x23, 0x01, 191 | // F32 192 | 0xDB, 0x0F, 0x49, 0x40, 193 | // F64 194 | 0x18, 0x2D, 0x44, 0x54, 0xFB, 0x21, 0x09, 0x40, 195 | // T 196 | 0xEF, 0xCD, 0xAB, 0x89, 197 | 0x67, 0x45, 0x23, 0x01, 198 | // D 199 | 0xEF, 0xCD, 0xAB, 0x89, 200 | 0x67, 0x45, 0x23, 0x01, 201 | // S 202 | 0x0D, 0x00, 0x00, 0x00, 203 | 0x48, 0x65, 0x6C, 0x6C, 0x6F, 0x2C, 0x20, 0x77, 0x6F, 0x72, 0x6C, 0x64, 0x21, 204 | // C 205 | 0x00, 0x00, 0x80, 0x3F, 206 | 0x00, 0x00, 0x00, 0x3F, 207 | 0x00, 0x00, 0x80, 0x3E, 208 | 0x00, 0x00, 0x00, 0x3E, 209 | // DynAry 210 | 0x02, 0x00, 0x00, 0x00, 211 | 0x67, 0x45, 0x23, 0x01, 212 | 0xEF, 0xCD, 0xAB, 0x89, 213 | // FixAry 214 | 0x67, 0x45, 0x23, 0x01, 215 | 0xEF, 0xCD, 0xAB, 0x89, 216 | } 217 | } 218 | 219 | func TestSerialize(t *testing.T) { 220 | var msg rosgo_tests.AllFieldTypes 221 | 222 | msg.H.Seq = 0x89ABCDEF 223 | msg.H.Stamp = ros.NewTime(0x89ABCDEF, 0x01234567) 224 | msg.H.FrameId = "frame_id" 225 | msg.B = 0x01 226 | msg.I8 = 0x01 227 | msg.I16 = 0x0123 228 | msg.I32 = 0x01234567 229 | msg.I64 = 0x0123456789ABCDEF 230 | msg.U8 = 0x01 231 | msg.U16 = 0x0123 232 | msg.U32 = 0x01234567 233 | msg.U64 = 0x0123456789ABCDEF 234 | msg.F32 = 3.141592653589793238462643383 235 | msg.F64 = 3.1415926535897932384626433832795028842 236 | msg.T = ros.NewTime(0x89ABCDEF, 0x01234567) 237 | msg.D = ros.NewDuration(0x89ABCDEF, 0x01234567) 238 | msg.S = "Hello, world!" 239 | msg.C = std_msgs.ColorRGBA{1.0, 0.5, 0.25, 0.125} 240 | 241 | msg.DynAry = append(msg.DynAry, 0x01234567) 242 | msg.DynAry = append(msg.DynAry, 0x89ABCDEF) 243 | msg.FixAry[0] = 0x01234567 244 | msg.FixAry[1] = 0x89ABCDEF 245 | 246 | var buf bytes.Buffer 247 | err := msg.Serialize(&buf) 248 | if err != nil { 249 | t.Error(err) 250 | } 251 | 252 | result := buf.Bytes() 253 | expected := getTestData() 254 | CheckBytes(t, expected, result) 255 | } 256 | 257 | func TestDeserialize(t *testing.T) { 258 | source := getTestData() 259 | reader := ros.NewReader(source) 260 | var msg rosgo_tests.AllFieldTypes 261 | err := msg.Deserialize(reader) 262 | if err != nil { 263 | t.Error(err) 264 | } 265 | 266 | if msg.H.Seq != 0x89ABCDEF { 267 | t.Error(msg.H.Seq) 268 | } 269 | if msg.H.Stamp.Sec != 0x89ABCDEF || msg.H.Stamp.NSec != 0x01234567 { 270 | t.Error(msg.H.Stamp) 271 | } 272 | if msg.H.FrameId != "frame_id" { 273 | t.Error(msg.H.FrameId) 274 | } 275 | if msg.B != 0x01 { 276 | t.Error(msg.B) 277 | } 278 | if msg.I8 != 0x01 { 279 | t.Error(msg.I8) 280 | } 281 | if msg.I16 != 0x0123 { 282 | t.Error(msg.I16) 283 | } 284 | if msg.I32 != 0x01234567 { 285 | t.Error(msg.I32) 286 | } 287 | if msg.I64 != 0x0123456789ABCDEF { 288 | t.Error(msg.I64) 289 | } 290 | if msg.U8 != 0x01 { 291 | t.Error(msg.U8) 292 | } 293 | if msg.U16 != 0x0123 { 294 | t.Error(msg.U16) 295 | } 296 | if msg.U32 != 0x01234567 { 297 | t.Error(msg.U32) 298 | } 299 | if msg.U64 != 0x0123456789ABCDEF { 300 | t.Error(msg.U64) 301 | } 302 | if msg.F32 != 3.141592653589793238462643383 { 303 | t.Error(msg.F32) 304 | } 305 | if msg.F64 != 3.1415926535897932384626433832795028842 { 306 | t.Error(msg.F64) 307 | } 308 | if msg.T.Sec != 0x89ABCDEF || msg.T.NSec != 0x01234567 { 309 | t.Error(msg.T) 310 | } 311 | if msg.D.Sec != 0x89ABCDEF || msg.D.NSec != 0x01234567 { 312 | t.Error(msg.D) 313 | } 314 | if msg.S != "Hello, world!" { 315 | t.Error(msg.S) 316 | } 317 | if msg.C.R != 1.0 || msg.C.G != 0.5 || msg.C.B != 0.25 || msg.C.A != 0.125 { 318 | t.Error(msg.C) 319 | } 320 | if msg.DynAry[0] != 0x01234567 || msg.DynAry[1] != 0x89ABCDEF { 321 | t.Error(msg.DynAry) 322 | } 323 | if msg.FixAry[0] != 0x01234567 || msg.FixAry[1] != 0x89ABCDEF { 324 | t.Error(msg.DynAry) 325 | } 326 | if reader.Len() != 0 { 327 | t.Fail() 328 | } 329 | } 330 | -------------------------------------------------------------------------------- /test/test_param/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "fmt" 5 | "log" 6 | "os" 7 | 8 | "github.com/fetchrobotics/rosgo/ros" 9 | ) 10 | 11 | func main() { 12 | node, err := ros.NewNode("/test_param", os.Args) 13 | if err != nil { 14 | fmt.Println(err) 15 | os.Exit(-1) 16 | } 17 | defer node.Shutdown() 18 | 19 | if hasParam, err := node.HasParam("/rosdistro"); err != nil { 20 | log.Fatalf("HasParam failed: %v", err) 21 | } else { 22 | if !hasParam { 23 | log.Fatal("HasParam() failed.") 24 | } 25 | } 26 | 27 | if foundKey, err := node.SearchParam("rosdistro"); err != nil { 28 | log.Fatalf("SearchParam failed: %v", err) 29 | } else { 30 | if foundKey != "/rosdistro" { 31 | log.Fatal("SearchParam() failed.") 32 | } 33 | } 34 | 35 | if param, err := node.GetParam("/rosdistro"); err != nil { 36 | log.Fatalf("GetParam: %v", err) 37 | } else { 38 | if value, ok := param.(string); !ok { 39 | log.Fatal("GetParam() failed.") 40 | } else { 41 | if value != "kinetic\n" { 42 | log.Fatalf("Expected 'kinetic\\n' but '%s'", value) 43 | } 44 | } 45 | } 46 | 47 | if err := node.SetParam("/test_param", 42); err != nil { 48 | log.Fatalf("SetParam failed: %v", err) 49 | } 50 | 51 | if param, err := node.GetParam("/test_param"); err != nil { 52 | log.Fatalf("GetParam failed: %v", err) 53 | } else { 54 | if value, ok := param.(int32); ok { 55 | if value != 42 { 56 | log.Fatalf("Expected 42 but %d", value) 57 | } 58 | } else { 59 | log.Fatal("GetParam('/test_param') failed.") 60 | } 61 | } 62 | 63 | if err := node.DeleteParam("/test_param"); err != nil { 64 | log.Fatalf("DeleteParam failed: %v", err) 65 | } 66 | 67 | log.Print("Success") 68 | } 69 | -------------------------------------------------------------------------------- /test/test_server/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | //go:generate gengo srv rospy_tutorials/AddTwoInts 4 | import ( 5 | "fmt" 6 | "os" 7 | "rospy_tutorials" 8 | 9 | "github.com/fetchrobotics/rosgo/ros" 10 | ) 11 | 12 | func callback(srv *rospy_tutorials.AddTwoInts) error { 13 | srv.Response.Sum = srv.Request.A + srv.Request.B 14 | fmt.Printf("%d + %d = %d\n", srv.Request.A, srv.Request.B, srv.Response.Sum) 15 | return nil 16 | } 17 | 18 | func main() { 19 | node, err := ros.NewNode("server", os.Args) 20 | if err != nil { 21 | fmt.Println(err) 22 | os.Exit(-1) 23 | } 24 | defer node.Shutdown() 25 | logger := node.Logger() 26 | logger.SetSeverity(ros.LogLevelDebug) 27 | server := node.NewServiceServer("/add_two_ints", rospy_tutorials.SrvAddTwoInts, callback) 28 | if server == nil { 29 | fmt.Println("Failed to initialize '/add_two_ints' service server") 30 | os.Exit(-1) 31 | } 32 | defer server.Shutdown() 33 | node.Spin() 34 | } 35 | -------------------------------------------------------------------------------- /test/test_simple_action_client/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | import ( 4 | "actionlib_tutorials" 5 | "fmt" 6 | "os" 7 | 8 | "github.com/fetchrobotics/rosgo/actionlib" 9 | "github.com/fetchrobotics/rosgo/ros" 10 | ) 11 | 12 | func main() { 13 | node, err := ros.NewNode("test_fibonacci_client", os.Args) 14 | if err != nil { 15 | fmt.Println(err) 16 | os.Exit(-1) 17 | } 18 | 19 | logger := node.Logger() 20 | defer node.Shutdown() 21 | go node.Spin() 22 | 23 | ac := actionlib.NewSimpleActionClient(node, "fibonacci", actionlib_tutorials.ActionFibonacci) 24 | logger.Info("Waiting for server to start") 25 | 26 | started := ac.WaitForServer(ros.NewDuration(0, 0)) 27 | if !started { 28 | logger.Info("Action server failed to start within timeout period.") 29 | return 30 | } 31 | 32 | logger.Info("Action server started, sending goal.") 33 | goal := &actionlib_tutorials.FibonacciGoal{Order: 20} 34 | ac.SendGoal(goal, nil, nil, nil) 35 | 36 | finished := ac.WaitForResult(ros.NewDuration(60, 0)) 37 | if finished { 38 | state, err := ac.GetState() 39 | if err != nil { 40 | logger.Errorf("Error getting state: %v", err) 41 | return 42 | } 43 | logger.Infof("Action finished: %v", state) 44 | } else { 45 | logger.Errorf("Action did not finish before the timeout") 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /test/test_simple_action_client_with_callbacks/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | //go:generate gengo action actionlib_tutorials/Fibonacci 4 | 5 | import ( 6 | "actionlib_tutorials" 7 | "fmt" 8 | "os" 9 | 10 | "github.com/fetchrobotics/rosgo/actionlib" 11 | 12 | "github.com/fetchrobotics/rosgo/ros" 13 | ) 14 | 15 | type fibonacciClient struct { 16 | node ros.Node 17 | name string 18 | ac actionlib.SimpleActionClient 19 | logger ros.Logger 20 | } 21 | 22 | func newfibonacciClient(node ros.Node, name string) *fibonacciClient { 23 | fc := &fibonacciClient{ 24 | node: node, 25 | ac: actionlib.NewSimpleActionClient(node, name, actionlib_tutorials.ActionFibonacci), 26 | logger: node.Logger(), 27 | } 28 | 29 | fc.logger.Info("Waiting for server to start") 30 | fc.ac.WaitForServer(ros.NewDuration(0, 0)) 31 | fc.logger.Info("Server started") 32 | return fc 33 | } 34 | 35 | func (fc *fibonacciClient) activeCb() { 36 | fc.logger.Info("Goal just went active") 37 | } 38 | 39 | func (fc *fibonacciClient) feedbackCb(fb *actionlib_tutorials.FibonacciFeedback) { 40 | fc.logger.Infof("Got feedback of from server: %v", fb.Sequence) 41 | } 42 | 43 | func (fc *fibonacciClient) doneCb(state uint8, result *actionlib_tutorials.FibonacciResult) { 44 | fc.logger.Infof("Finished in state %v", state) 45 | fc.logger.Infof("Sequence: %v", result.Sequence) 46 | fc.node.Shutdown() 47 | } 48 | 49 | func (fc *fibonacciClient) sendGoal(order int32) { 50 | goal := &actionlib_tutorials.FibonacciGoal{Order: order} 51 | fc.ac.SendGoal(goal, fc.doneCb, fc.activeCb, fc.feedbackCb) 52 | } 53 | 54 | func main() { 55 | node, err := ros.NewNode("test_fibonacci_client", os.Args) 56 | if err != nil { 57 | fmt.Println(err) 58 | os.Exit(-1) 59 | } 60 | 61 | fc := newfibonacciClient(node, "fibonacci") 62 | fc.sendGoal(10) 63 | 64 | node.Spin() 65 | } 66 | -------------------------------------------------------------------------------- /test/test_simple_action_server/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | //go:generate gengo action actionlib_tutorials/Averaging 4 | import ( 5 | "actionlib_tutorials" 6 | "fmt" 7 | "math" 8 | "os" 9 | "std_msgs" 10 | 11 | "github.com/fetchrobotics/rosgo/actionlib" 12 | "github.com/fetchrobotics/rosgo/ros" 13 | ) 14 | 15 | type averagingServer struct { 16 | node ros.Node 17 | as actionlib.SimpleActionServer 18 | dataCount int32 19 | goal int32 20 | sum float32 21 | sumSq float64 22 | feedback *actionlib_tutorials.AveragingFeedback 23 | result *actionlib_tutorials.AveragingResult 24 | sub ros.Subscriber 25 | logger ros.Logger 26 | } 27 | 28 | func newAveragingServer(node ros.Node, name string) { 29 | avg := new(averagingServer) 30 | avg.node = node 31 | avg.logger = node.Logger() 32 | avg.as = actionlib.NewSimpleActionServer(node, name, actionlib_tutorials.ActionAveraging, nil, false) 33 | avg.sub = node.NewSubscriber("/random_number", std_msgs.MsgFloat32, avg.analysisCallback) 34 | 35 | avg.as.RegisterGoalCallback(avg.goalCallback) 36 | avg.as.RegisterPreemptCallback(avg.preemptCallback) 37 | avg.as.Start() 38 | } 39 | 40 | func (avg *averagingServer) goalCallback() { 41 | avg.dataCount = 0 42 | avg.sum = 0 43 | avg.sumSq = 0 44 | 45 | goal, err := avg.as.AcceptNewGoal() 46 | if err != nil { 47 | avg.logger.Errorf("Error accepting new goal: %v", err) 48 | return 49 | } 50 | 51 | avgGoal, ok := goal.(*actionlib_tutorials.AveragingGoal) 52 | if !ok { 53 | avg.logger.Errorf("Error accepting new goal: expected averaging action goal") 54 | return 55 | } 56 | 57 | avg.goal = avgGoal.Samples 58 | } 59 | 60 | func (avg *averagingServer) preemptCallback() { 61 | avg.as.SetPreempted(nil, "") 62 | } 63 | 64 | func (avg *averagingServer) analysisCallback(msg *std_msgs.Float32) { 65 | if !avg.as.IsActive() { 66 | return 67 | } 68 | 69 | avg.dataCount++ 70 | avg.sum += msg.Data 71 | avg.feedback.Sample = avg.dataCount 72 | avg.feedback.Data = msg.Data 73 | avg.feedback.Mean = avg.sum / float32(avg.dataCount) 74 | avg.sumSq = math.Pow(float64(msg.Data), 2) 75 | avg.feedback.StdDev = float32(math.Sqrt(math.Abs((avg.sumSq/float64(msg.Data) - math.Pow(float64(avg.feedback.Mean), 2))))) 76 | 77 | if avg.dataCount > avg.goal { 78 | avg.result.Mean = avg.feedback.Mean 79 | avg.result.StdDev = avg.feedback.StdDev 80 | 81 | if avg.result.Mean < 5.0 { 82 | avg.logger.Info("Averaging action aborted") 83 | avg.as.SetAborted(avg.result, "") 84 | } else { 85 | avg.logger.Info("Averaging action succeeded") 86 | avg.as.SetSucceeded(avg.result, "") 87 | } 88 | } 89 | } 90 | 91 | func main() { 92 | node, err := ros.NewNode("test_averaging_server", os.Args) 93 | if err != nil { 94 | fmt.Println(err) 95 | os.Exit(-1) 96 | } 97 | defer node.Shutdown() 98 | 99 | newAveragingServer(node, "averaging") 100 | node.Spin() 101 | } 102 | -------------------------------------------------------------------------------- /test/test_simple_action_server_with_callbacks/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | //go:generate gengo action actionlib_tutorials/Fibonacci 4 | import ( 5 | "actionlib_tutorials" 6 | "fmt" 7 | "os" 8 | "time" 9 | 10 | "github.com/fetchrobotics/rosgo/actionlib" 11 | "github.com/fetchrobotics/rosgo/ros" 12 | ) 13 | 14 | // fibonacciServer implements a fibonacci simple action server 15 | // using the execute callback 16 | type fibonacciServer struct { 17 | node ros.Node 18 | logger ros.Logger 19 | as actionlib.SimpleActionServer 20 | name string 21 | fb ros.Message 22 | result ros.Message 23 | } 24 | 25 | // newfibonacciServer creates a new fibonacci action server and starts it 26 | func newFibonacciServer(node ros.Node, name string) { 27 | s := &fibonacciServer{ 28 | node: node, 29 | name: name, 30 | logger: node.Logger(), 31 | } 32 | 33 | s.as = actionlib.NewSimpleActionServer(node, name, 34 | actionlib_tutorials.ActionFibonacci, s.executeCallback, false) 35 | s.as.Start() 36 | } 37 | 38 | func (s *fibonacciServer) executeCallback(goal *actionlib_tutorials.FibonacciGoal) { 39 | feed := &actionlib_tutorials.FibonacciFeedback{} 40 | feed.Sequence = append(feed.Sequence, 0) 41 | feed.Sequence = append(feed.Sequence, 1) 42 | success := true 43 | 44 | for i := 1; i < int(goal.Order); i++ { 45 | if s.as.IsPreemptRequested() { 46 | success = false 47 | if err := s.as.SetPreempted(nil, ""); err != nil { 48 | s.logger.Error(err) 49 | } 50 | break 51 | } 52 | 53 | val := feed.Sequence[i] + feed.Sequence[i-1] 54 | feed.Sequence = append(feed.Sequence, val) 55 | 56 | s.as.PublishFeedback(feed) 57 | time.Sleep(1000 * time.Millisecond) 58 | } 59 | 60 | if success { 61 | result := &actionlib_tutorials.FibonacciResult{Sequence: feed.Sequence} 62 | if err := s.as.SetSucceeded(result, "goal"); err != nil { 63 | s.logger.Error(err) 64 | } 65 | } 66 | } 67 | 68 | func main() { 69 | node, err := ros.NewNode("test_fibonacci_server", os.Args) 70 | if err != nil { 71 | fmt.Println(err) 72 | os.Exit(-1) 73 | } 74 | defer node.Shutdown() 75 | 76 | newFibonacciServer(node, "fibonacci") 77 | node.Spin() 78 | } 79 | -------------------------------------------------------------------------------- /test/test_talker/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | //go:generate gengo msg std_msgs/String 4 | import ( 5 | "actionlib_tutorials" 6 | "fmt" 7 | "os" 8 | "std_msgs" 9 | "time" 10 | 11 | "github.com/fetchrobotics/rosgo/ros" 12 | ) 13 | 14 | "github.com/fetchrobotics/rosgo/ros" 15 | ) 16 | 17 | func main() { 18 | node, err := ros.NewNode("/talker", os.Args) 19 | if err != nil { 20 | fmt.Println(err) 21 | os.Exit(-1) 22 | } 23 | defer node.Shutdown() 24 | node.Logger().SetSeverity(ros.LogLevelDebug) 25 | pub := node.NewPublisher("/chatter", std_msgs.MsgString) 26 | 27 | for node.OK() { 28 | node.SpinOnce() 29 | var msg std_msgs.String 30 | msg.Data = fmt.Sprintf("hello %s", time.Now().String()) 31 | fmt.Println(msg.Data) 32 | pub.Publish(&msg) 33 | time.Sleep(time.Second) 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /test/test_talker_with_callbacks/main.go: -------------------------------------------------------------------------------- 1 | package main 2 | 3 | //go:generate gengo msg std_msgs/String 4 | import ( 5 | "fmt" 6 | "os" 7 | "std_msgs" 8 | "time" 9 | 10 | "github.com/fetchrobotics/rosgo/ros" 11 | ) 12 | 13 | func main() { 14 | node, err := ros.NewNode("/talker", os.Args) 15 | if err != nil { 16 | fmt.Println(err) 17 | os.Exit(-1) 18 | } 19 | defer node.Shutdown() 20 | node.Logger().SetSeverity(ros.LogLevelDebug) 21 | pub := node.NewPublisherWithCallbacks("/chatter", std_msgs.MsgString, onConnect, onDisconnect) 22 | 23 | for node.OK() { 24 | node.SpinOnce() 25 | var msg std_msgs.String 26 | msg.Data = fmt.Sprintf("hello %s", time.Now().String()) 27 | fmt.Println(msg.Data) 28 | pub.Publish(&msg) 29 | time.Sleep(time.Second) 30 | } 31 | } 32 | 33 | func onConnect(pub ros.SingleSubscriberPublisher) { 34 | fmt.Printf("-------Connect callback: node %s topic %s\n", pub.GetSubscriberName(), pub.GetTopic()) 35 | var msg std_msgs.String 36 | msg.Data = fmt.Sprintf("hello %s", pub.GetSubscriberName()) 37 | pub.Publish(&msg) 38 | } 39 | 40 | func onDisconnect(pub ros.SingleSubscriberPublisher) { 41 | fmt.Printf("-------Disconnect callback: node %s topic %s\n", pub.GetSubscriberName(), pub.GetTopic()) 42 | } 43 | --------------------------------------------------------------------------------