Six leg try

executor.go

@@ -0,0 +1,126 @@
+package gait
+
+import (
+	"context"
+	"fmt"
+	"log"
+	"sync"
+
+	"github.com/alexmorten/mhist/models"
+	mhist "github.com/alexmorten/mhist/proto"
+)
+
+// Executor handles movements
+type Executor struct {
+	mhistClient  mhist.MhistClient
+	actionStream mhist.Mhist_StoreStreamClient
+
+	currentMovement Movement
+	movementQueue   []Movement
+	events          chan struct{}
+	*sync.Mutex
+}
+
+// NewExecutor ...
+func NewExecutor(client mhist.MhistClient) *Executor {
+	stream, err := client.StoreStream(context.Background())
+	if err != nil {
+		panic(err)
+	}
+
+	return &Executor{
+		mhistClient:  client,
+		actionStream: stream,
+		Mutex:        &sync.Mutex{},
+		events:       make(chan struct{}, 1),
+	}
+}
+
+// AddNextMovement ...
+func (e *Executor) AddNextMovement(movement Movement) {
+	e.Lock()
+	defer e.Unlock()
+	log.Println("adding new movement")
+	e.movementQueue = append(e.movementQueue, movement)
+	e.events <- struct{}{}
+}
+
+// MarkActionDoneFor leg
+func (e *Executor) MarkActionDoneFor(legName string, doneCommand string) {
+	e.Lock()
+	defer e.Unlock()
+	for _, leg := range e.currentMovement.legs {
+		if leg.name == legName && e.currentMovement.command == doneCommand {
+			leg.done = true
+			log.Println("marked action done for leg", legName, "and for command", doneCommand)
+
+			e.events <- struct{}{}
+			return
+		}
+	}
+
+	log.Println("couldn't mark action for leg", legName, "and for command", doneCommand, "as done")
+}
+
+// Run should be called in a new goroutine
+func (e *Executor) Run() {
+	for {
+		<-e.events
+
+		e.checkState()
+	}
+}
+
+func (e *Executor) checkState() {
+	e.Lock()
+	defer e.Mutex.Unlock()
+
+	if e.currentMovement == nil {
+		if len(e.movementQueue) > 0 {
+			e.currentMovement = e.movementQueue[0]
+
+			e.movementQueue = e.movementQueue[1:len(e.movementQueue)]
+		} else {
+			return
+		}
+	}
+
+	e.checkCurrentMovementState()
+}
+
+func (e *Executor) checkCurrentMovementState() {
+	if e.currentMovement == nil {
+		return
+	}
+
+	var currentAction *Action = e.currentMovement
+
+	if !currentAction.started {
+		log.Println("current Action not started, executing it")
+		e.executeAction(currentAction)
+		return
+	}
+
+	if currentAction.Done() {
+		log.Println("current Action done, taking next one")
+		e.currentMovement = currentAction.next
+		e.checkCurrentMovementState()
+	}
+}
+
+func (e *Executor) executeAction(action *Action) {
+	action.started = true
+
+	for _, leg := range action.legs {
+		write(e.actionStream, leg.name, action.command)
+	}
+}
+
+func write(c mhist.Mhist_StoreStreamClient, legName, message string) {
+	log.Println("writing action", message, "to", legName)
+	err := c.Send(&mhist.MeasurementMessage{Name: "gait_actions", Measurement: mhist.MeasurementFromModel(&models.Raw{Value: []byte(fmt.Sprintf("%s %s", legName, message))})})
+
+	if err != nil {
+		panic(err)
+	}
+}

feedback_subscriber.go

@@ -0,0 +1,66 @@
+package gait
+
+import (
+	"context"
+	"strings"
+
+	mhist "github.com/alexmorten/mhist/proto"
+)
+
+// SubscribeToFeedback ...
+func SubscribeToFeedback(c mhist.MhistClient, handler func(legName string, doneCommand string)) {
+	stream, err := c.Subscribe(context.Background(), &mhist.Filter{Names: []string{"gait_feedback"}})
+	if err != nil {
+		panic(err)
+	}
+
+	for {
+		m, err := stream.Recv()
+		if err != nil {
+			panic(err)
+		}
+
+		if legName, command, ok := parseFeedbackMessage(m); ok {
+			handler(legName, command)
+		}
+	}
+
+}
+
+func parseFeedbackMessage(m *mhist.MeasurementMessage) (legName, command string, ok bool) {
+	if r := m.GetMeasurement().GetRaw(); r != nil {
+		message := string(r.Value)
+		return parseFeedback(message)
+	}
+
+	return "", "", false
+}
+
+func parseFeedback(message string) (legName, command string, ok bool) {
+	splitLine := strings.SplitN(message, " ", 3)
+	if len(splitLine) != 3 {
+		return "", "", false
+	}
+
+	for i, part := range splitLine {
+		splitLine[i] = strings.ToLower(removeNewLineChars(part))
+	}
+
+	legName = splitLine[0]
+	verb := splitLine[1]
+	command = splitLine[2]
+
+	if verb != "did" {
+		return "", "", false
+	}
+
+	return legName, command, true
+}
+
+func removeNewLineChars(s string) string {
+	return strings.ReplaceAll(
+		strings.ReplaceAll(s, "\r\n", ""),
+		"\n",
+		"",
+	)
+}

go.mod

@@ -1,9 +1,10 @@
-module github.com/codeuniversity/meb-gait
+module github.com/codeuniversity/control-gait
 
 go 1.13
 
 require (
-	github.com/codeuniversity/nervo v0.0.0-20191009231846-4359b872b69e
+	github.com/alexmorten/mhist v0.2.0
+	github.com/codeuniversity/nervo v0.0.0-20191118103657-0741bd3c3331
 	github.com/cosiner/argv v0.0.1 // indirect
 	github.com/go-delve/delve v1.3.2 // indirect
 	github.com/konsorten/go-windows-terminal-sequences v1.0.2 // indirect
@@ -18,6 +19,6 @@ require (
 	golang.org/x/arch v0.0.0-20190927153633-4e8777c89be4 // indirect
 	golang.org/x/crypto v0.0.0-20191029031824-8986dd9e96cf // indirect
 	golang.org/x/sys v0.0.0-20191029155521-f43be2a4598c // indirect
-	google.golang.org/grpc v1.23.1
+	google.golang.org/grpc v1.24.0
 	gopkg.in/yaml.v2 v2.2.4 // indirect
 )

go.sum

@@ -1,13 +1,18 @@
 cloud.google.com/go v0.26.0/go.mod h1:aQUYkXzVsufM+DwF1aE+0xfcU+56JwCaLick0ClmMTw=
 github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU=
 github.com/alecthomas/gometalinter v2.0.11+incompatible/go.mod h1:qfIpQGGz3d+NmgyPBqv+LSh50emm1pt72EtcX2vKYQk=
+github.com/alexmorten/mhist v0.2.0 h1:RaT6aSFA82a5qtMRoUYftg+57GfUhOaQJt1TIPr9WAU=
+github.com/alexmorten/mhist v0.2.0/go.mod h1:EN9fSAbjHRIy7wLr91Qsr6auKLfUBGVZVdNdob+i0VA=
 github.com/armon/consul-api v0.0.0-20180202201655-eb2c6b5be1b6/go.mod h1:grANhF5doyWs3UAsr3K4I6qtAmlQcZDesFNEHPZAzj8=
 github.com/chzyer/logex v1.1.10/go.mod h1:+Ywpsq7O8HXn0nuIou7OrIPyXbp3wmkHB+jjWRnGsAI=
 github.com/chzyer/readline v0.0.0-20180603132655-2972be24d48e/go.mod h1:nSuG5e5PlCu98SY8svDHJxuZscDgtXS6KTTbou5AhLI=
 github.com/chzyer/test v0.0.0-20180213035817-a1ea475d72b1/go.mod h1:Q3SI9o4m/ZMnBNeIyt5eFwwo7qiLfzFZmjNmxjkiQlU=
 github.com/client9/misspell v0.3.4/go.mod h1:qj6jICC3Q7zFZvVWo7KLAzC3yx5G7kyvSDkc90ppPyw=
+github.com/codeuniversity/control-gait v0.0.0-20191103100242-248066494bbd h1:8D0Bv69zOn97uNdEXkgj07UEGD0qPJ9d9gYihgLC9Us=
 github.com/codeuniversity/nervo v0.0.0-20191009231846-4359b872b69e h1:qmwnFVwY4pwP+Nsllr1xylRLGVBG1MeJQpIhJ2J7ynA=
 github.com/codeuniversity/nervo v0.0.0-20191009231846-4359b872b69e/go.mod h1:zWDIq4OORk1mJaQltniS7Agkm1HNkAauFe04s3Q6YNU=
+github.com/codeuniversity/nervo v0.0.0-20191118103657-0741bd3c3331 h1:vKB4wV8TaEdvWFfZa3zDXJARd9PQ847BCqyCpkNvqHc=
+github.com/codeuniversity/nervo v0.0.0-20191118103657-0741bd3c3331/go.mod h1:KKHdR6+p4Ady036jPJXZ6n0ao5wZS7bql/0jE/f9th0=
 github.com/coreos/etcd v3.3.10+incompatible/go.mod h1:uF7uidLiAD3TWHmW31ZFd/JWoc32PjwdhPthX9715RE=
 github.com/coreos/go-etcd v2.0.0+incompatible/go.mod h1:Jez6KQU2B/sWsbdaef3ED8NzMklzPG4d5KIOhIy30Tk=
 github.com/coreos/go-semver v0.2.0/go.mod h1:nnelYz7RCh+5ahJtPPxZlU+153eP4D4r3EedlOD2RNk=
@@ -22,6 +27,7 @@ github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSs
 github.com/fsnotify/fsnotify v1.4.7/go.mod h1:jwhsz4b93w/PPRr/qN1Yymfu8t87LnFCMoQvtojpjFo=
 github.com/go-delve/delve v1.3.2 h1:K8VjV+Q2YnBYlPq0ctjrvc9h7h03wXszlszzfGW5Tog=
 github.com/go-delve/delve v1.3.2/go.mod h1:LLw6qJfIsRK9WcwV2IRRqsdlgrqzOeuGrQOCOIhDpt8=
+github.com/golang/glog v0.0.0-20160126235308-23def4e6c14b h1:VKtxabqXZkF25pY9ekfRL6a582T4P37/31XEstQ5p58=
 github.com/golang/glog v0.0.0-20160126235308-23def4e6c14b/go.mod h1:SBH7ygxi8pfUlaOkMMuAQtPIUF8ecWP5IEl/CR7VP2Q=
 github.com/golang/lint v0.0.0-20181026193005-c67002cb31c3/go.mod h1:tluoj9z5200jBnyusfRPU2LqT6J+DAorxEvtC7LHB+E=
 github.com/golang/mock v1.1.1/go.mod h1:oTYuIxOrZwtPieC+H1uAHpcLFnEyAGVDL/k47Jfbm0A=
@@ -73,6 +79,7 @@ github.com/peterh/liner v1.1.0 h1:f+aAedNJA6uk7+6rXsYBnhdo4Xux7ESLe+kcuVUF5os=
 github.com/peterh/liner v1.1.0/go.mod h1:CRroGNssyjTd/qIG2FyxByd2S8JEAZXBl4qUrZf8GS0=
 github.com/pkg/profile v0.0.0-20170413231811-06b906832ed0/go.mod h1:hJw3o1OdXxsrSjjVksARp5W95eeEaEfptyVZyv6JUPA=
 github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4=
+github.com/rs/cors v1.6.0/go.mod h1:gFx+x8UowdsKA9AchylcLynDq+nNFfI8FkUZdN/jGCU=
 github.com/russross/blackfriday v0.0.0-20180428102519-11635eb403ff/go.mod h1:JO/DiYxRf+HjHt06OyowR9PTA263kcR/rfWxYHBV53g=
 github.com/russross/blackfriday v1.5.2/go.mod h1:JO/DiYxRf+HjHt06OyowR9PTA263kcR/rfWxYHBV53g=
 github.com/sirupsen/logrus v0.0.0-20180523074243-ea8897e79973 h1:3AJZYTzw3gm3TNTt30x0CCKD7GOn2sdd50Hn35fQkGY=
@@ -122,6 +129,8 @@ golang.org/x/net v0.0.0-20190311183353-d8887717615a/go.mod h1:t9HGtf8HONx5eT2rtn
 golang.org/x/net v0.0.0-20190404232315-eb5bcb51f2a3/go.mod h1:t9HGtf8HONx5eT2rtn7q6eTqICYqUVnKs3thJo3Qplg=
 golang.org/x/net v0.0.0-20190912160710-24e19bdeb0f2 h1:4dVFTC832rPn4pomLSz1vA+are2+dU19w1H8OngV7nc=
 golang.org/x/net v0.0.0-20190912160710-24e19bdeb0f2/go.mod h1:z5CRVTTTmAJ677TzLLGU+0bjPO0LkuOLi4/5GtJWs/s=
+golang.org/x/net v0.0.0-20191028085509-fe3aa8a45271 h1:N66aaryRB3Ax92gH0v3hp1QYZ3zWWCCUR/j8Ifh45Ss=
+golang.org/x/net v0.0.0-20191028085509-fe3aa8a45271/go.mod h1:z5CRVTTTmAJ677TzLLGU+0bjPO0LkuOLi4/5GtJWs/s=
 golang.org/x/oauth2 v0.0.0-20180821212333-d2e6202438be/go.mod h1:N/0e6XlmueqKjAGxoOufVs8QHGRruUQn6yWY3a++T0U=
 golang.org/x/sync v0.0.0-20180314180146-1d60e4601c6f/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM=
 golang.org/x/sync v0.0.0-20190423024810-112230192c58/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM=
@@ -149,6 +158,8 @@ google.golang.org/genproto v0.0.0-20180817151627-c66870c02cf8 h1:Nw54tB0rB7hY/N0
 google.golang.org/genproto v0.0.0-20180817151627-c66870c02cf8/go.mod h1:JiN7NxoALGmiZfu7CAH4rXhgtRTLTxftemlI0sWmxmc=
 google.golang.org/grpc v1.23.1 h1:q4XQuHFC6I28BKZpo6IYyb3mNO+l7lSOxRuYTCiDfXk=
 google.golang.org/grpc v1.23.1/go.mod h1:Y5yQAOtifL1yxbo5wqy6BxZv8vAUGQwXBOALyacEbxg=
+google.golang.org/grpc v1.24.0 h1:vb/1TCsVn3DcJlQ0Gs1yB1pKI6Do2/QNwxdKqmc/b0s=
+google.golang.org/grpc v1.24.0/go.mod h1:XDChyiUovWa60DnaeDeZmSW86xtLtjtZbwvSiRnRtcA=
 gopkg.in/airbrake/gobrake.v2 v2.0.9/go.mod h1:/h5ZAUhDkGaJfjzjKLSjv6zCL6O0LLBxU4K+aSYdM/U=
 gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0=
 gopkg.in/check.v1 v1.0.0-20180628173108-788fd7840127/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0=

high_level_subscriber.go

@@ -0,0 +1,29 @@
+package gait
+
+import (
+	"context"
+	"log"
+
+	mhist "github.com/alexmorten/mhist/proto"
+)
+
+// SubscribeToHighLevelAction ...
+func SubscribeToHighLevelAction(c mhist.MhistClient, handler func(movement Movement)) {
+	stream, err := c.Subscribe(context.Background(), &mhist.Filter{Names: []string{"hlc_actions"}})
+	if err != nil {
+		panic(err)
+	}
+
+	for {
+		_, err := stream.Recv()
+		log.Println("bla")
+		if err != nil {
+			panic(err)
+		}
+
+		// handler(NewMovement([]string{"leg1", "leg3", "leg5"}))
+		// handler(NewMovement([]string{"leg2", "leg4", "leg6"}))
+
+		handler(NewMovement([]string{"leg1"}))
+	}
+}

main/main.go

@@ -1,66 +1,26 @@
 package main
 
 import (
-	"context"
-	"fmt"
-	"os"
-	"time"
+	"flag"
 
-	"github.com/codeuniversity/nervo/proto"
+	mhist "github.com/alexmorten/mhist/proto"
+	gait "github.com/codeuniversity/control-gait"
 	"google.golang.org/grpc"
 )
 
 func main() {
-	conn, err := grpc.Dial(os.Args[1], grpc.WithInsecure())
+	var mhistAddress string
+	flag.StringVar(&mhistAddress, "mhist_address", "localhost:6666", "address to mhist including port")
+	flag.Parse()
+	mhistConn, err := grpc.Dial(mhistAddress, grpc.WithInsecure())
 	if err != nil {
 		panic(err)
 	}
-	c := proto.NewNervoServiceClient(conn)
+	mhistC := mhist.NewMhistClient(mhistConn)
 
-	response, err := c.ListControllers(
-		context.Background(),
-		&proto.ControllerListRequest{},
-	)
+	executor := gait.NewExecutor(mhistC)
+	go gait.SubscribeToFeedback(mhistC, executor.MarkActionDoneFor)
+	go gait.SubscribeToHighLevelAction(mhistC, executor.AddNextMovement)
 
-	if err != nil {
-		panic(err)
-	}
-
-	for index, info := range response.ControllerInfos {
-		fmt.Println(index, info.Name, info.PortName)
-	}
-
-	portName := findLeg1PortName(response.ControllerInfos)
-	if portName == "" {
-		panic("Oh fuck, no port name")
-	}
-	moveLeg(c, portName)
-}
-
-func findLeg1PortName(infos []*proto.ControllerInfo) string {
-	for _, info := range infos {
-		if info.Name == "leg1" {
-			return info.PortName
-		}
-	}
-
-	return ""
-}
-
-func moveLeg(c proto.NervoServiceClient, portName string) {
-	write(c, portName, "lift up \n")
-	time.Sleep(1 * time.Second)
-	write(c, portName, "move forward \n")
-	time.Sleep(1 * time.Second)
-	write(c, portName, "lift down \n")
-	time.Sleep(1 * time.Second)
-	write(c, portName, "move back")
-}
-func write(c proto.NervoServiceClient, portName, message string) {
-	_, err := c.WriteToController(context.Background(), &proto.WriteToControllerRequest{
-		ControllerPortName: portName, Message: []byte(message),
-	})
-	if err != nil {
-		panic(err)
-	}
+	executor.Run()
 }