started adding a rosbag reader

Author: brychanrobot

.gitignore

@@ -22,3 +22,5 @@ _testmain.go
 *.exe
 *.test
 *.prof
+
+*.bag

ros.go

@@ -5,8 +5,6 @@ import (
 	"fmt"
 	"io"
 	"log"
-	"regexp"
-	"strings"
 	"sync"
 
 	"golang.org/x/net/websocket"
@@ -16,6 +14,11 @@ var (
 	messageCount = 0
 )
 
+type Base struct {
+	Op string `json:"op"`
+	Id string `json:"id"`
+}
+
 type Ros struct {
 	origin           string
 	url              string
@@ -55,6 +58,21 @@ func (ros *Ros) getServiceResponse(service *ServiceCall) *ServiceResponse {
 	return serviceResponse.(*ServiceResponse)
 }
 
+func (ros *Ros) getTopicResponse(topic *Topic) *interface{} {
+	response := make(chan interface{})
+	ros.receivedMapMutex.Lock()
+	ros.receivedMap[topic.Id] = response
+	ros.receivedMapMutex.Unlock()
+	err := websocket.JSON.Send(ros.ws, topic)
+	if err != nil {
+		fmt.Println("Couldn't send msg")
+	}
+	log.Println(ros.receivedMap)
+
+	topicResponse := <-response
+	return &topicResponse
+}
+
 func (ros *Ros) returnToAppropriateChannel(id string, data interface{}) {
 	ros.receivedMapMutex.Lock()
 	ros.receivedMap[id] <- data
@@ -73,34 +91,34 @@ func (ros *Ros) handleIncoming() {
 			break
 		}
 
-		opRegex, err := regexp.Compile(`"op"\s*:\s*"[[:alpha:],_]*`)
-		if err != nil {
-			log.Println(err)
-		}
-		opString := opRegex.FindString(string(msg))
-		splitOpString := strings.Split(opString, "\"")
-		operation := splitOpString[len(splitOpString)-1]
-
-		//log.Println(operation)
-
 		/*
-			var data map[string]interface{}
-			jsonErr := json.Unmarshal(msg, &data)
-			//fmt.Printf("Received from server: %s\n", data)
-			if jsonErr != nil {
-				panic(jsonErr)
+			opRegex, err := regexp.Compile(`"op"\s*:\s*"[[:alpha:],_]*`)
+			if err != nil {
+				log.Println(err)
 			}
-
-			ros.receivedMapMutex.Lock()
-			ros.receivedMap[data["id"].(string)] <- data
-			ros.receivedMapMutex.Unlock()
+			opString := opRegex.FindString(string(msg))
+			splitOpString := strings.Split(opString, "\"")
+			operation := splitOpString[len(splitOpString)-1]
 		*/
-		if operation == "service_response" {
+
+		var base Base
+		json.Unmarshal(msg, &base)
+
+		log.Println(base)
+
+		if base.Op == "service_response" {
 			var serviceResponse ServiceResponse
 			json.Unmarshal(msg, &serviceResponse)
 			ros.receivedMapMutex.Lock()
 			ros.receivedMap[serviceResponse.Id] <- &serviceResponse
 			ros.receivedMapMutex.Unlock()
+		} else if base.Op == "publish" {
+			log.Println(base)
+			var topic Topic
+			json.Unmarshal(msg, &topic)
+			ros.receivedMapMutex.Lock()
+			ros.receivedMap[topic.Topic] <- &topic
+			ros.receivedMapMutex.Unlock()
 		}
 	}
 }
@@ -112,6 +130,22 @@ func (ros *Ros) GetTopics() []string {
 	return topics
 }
 
-func (ros *Ros) Subscribe(topic *Topic) {
+func (ros *Ros) Subscribe(topicName string, callback TopicCallback) {
+	//topicResponse := ros.getTopicResponse(topic)
+	topic := NewTopic(topicName)
 
+	response := make(chan interface{})
+	ros.receivedMapMutex.Lock()
+	ros.receivedMap[topic.Topic] = response
+	ros.receivedMapMutex.Unlock()
+	err := websocket.JSON.Send(ros.ws, topic)
+	if err != nil {
+		fmt.Println("Couldn't send msg")
+	}
+
+	go func() {
+		for {
+			callback(&(<-response).(*Topic).Msg)
+		}
+	}()
 }

rosbag.go

@@ -0,0 +1,52 @@
+package main
+
+import (
+	"bufio"
+	"encoding/binary"
+	"flag"
+	"log"
+	"os"
+)
+
+func parseRecordHeader(reader *bufio.Reader) {
+	var headerLength int32
+	binary.Read(reader, binary.LittleEndian, &headerLength)
+	//oneByte, err := reader.ReadByte()
+	log.Println(headerLength)
+
+	var fieldLength int32
+
+	binary.Read(reader, binary.LittleEndian, &fieldLength)
+	fieldName, _ := reader.ReadString('=')
+	log.Println(fieldName)
+
+}
+
+func parseRosbag(path string) {
+	log.Println(path)
+	file, err := os.Open(path) // For read access.
+	if err != nil {
+		log.Fatal(err)
+	}
+	defer file.Close()
+
+	reader := bufio.NewReader(file)
+
+	line, _ := reader.ReadString('\n')
+	log.Print(line)
+
+	file.Seek(4096, 0) //offsets by the size of the chunk header
+
+	reader = bufio.NewReader(file)
+
+	parseRecordHeader(reader)
+	/*chunkHeader := make([]byte, 4096)
+	_, _ = file.Read(chunkHeader)
+	log.Println(string(bytes))
+	*/
+}
+
+func main() {
+	flag.Parse()
+	parseRosbag(flag.Arg(0))
+}

topic.go

@@ -1,6 +1,23 @@
 package goros
 
+import (
+	"encoding/json"
+	"fmt"
+)
+
 type Topic struct {
-	Op string
-	Id string
+	Op    string          `json:"op"`
+	Id    string          `json:"id"`
+	Topic string          `json:"topic"`
+	Msg   json.RawMessage `json:"msg,omitempty"`
+}
+
+type TopicCallback func(*json.RawMessage)
+
+func NewTopic(topicName string) *Topic {
+	topic := &Topic{Op: "subscribe", Topic: topicName}
+	topic.Id = fmt.Sprintf("%s:%s:%d", topic.Op, topic.Topic, messageCount)
+	messageCount++
+
+	return topic
 }