diff --git a/README.md b/README.md index 0592b26..da48281 100644 --- a/README.md +++ b/README.md @@ -11,9 +11,9 @@ In root directory, call `make` to generate binary file. To clean up, call `make ```bash ./motorsim -c ./cfg/motors.yaml motor --id [ID] serve ``` -2. Start Motor controller server (Beware, controllers commands except Port 8080): +2. Start Motor controller server (Beware, if modifying port controllers commands except Port 8080): ```bash - ./motorsim -c ./cfg/motors.yaml controller serve -p 8080 + ./motorsim -c ./cfg/motors.yaml controller serve ``` ### Client usage #### Motor controller commands @@ -21,9 +21,9 @@ In root directory, call `make` to generate binary file. To clean up, call `make ```bash ./motorsim -c ./cfg/motors.yaml controller getJoints ``` -- Set joints values (Beware, currently set the same values for all joints): +- Set joints values: ```bash - ./motorsim -c ./cfg/motors.yaml controller setJoints --j [Joint goals] + ./motorsim -c ./cfg/motors.yaml controller setJoints [Joint goals] ``` #### Motor commands Every command should be provided with valid ID from [config file](./cfg/motors.yaml): @@ -33,7 +33,7 @@ Every command should be provided with valid ID from [config file](./cfg/motors.y ``` - Set motor velocity: ```bash - ./motorsim -c ./cfg/motors.yaml motor --id [ID] moveVel --vel [Velocity] + ./motorsim -c ./cfg/motors.yaml motor --id [ID] moveVel -- [Velocity] ``` ## TODO @@ -42,4 +42,4 @@ Every command should be provided with valid ID from [config file](./cfg/motors.y - [ ] Use coroutines for gRPC services callbacks. - [ ] Fix get Motor State service (Behavior not robust). - [ ] Fix Controller SetJoint command CLI omitting multiples input values. -- [ ] Complete CLI help messages. \ No newline at end of file +- [ ] Send error responses to clients of gRPC services. \ No newline at end of file diff --git a/src/cmd/controller/controller.go b/src/cmd/controller/controller.go index fe6ce19..cb75064 100644 --- a/src/cmd/controller/controller.go +++ b/src/cmd/controller/controller.go @@ -23,10 +23,10 @@ var( // controllerCmd represents the controller command var controllerCmd = &cobra.Command{ Use: "controller", - Short: "controller command description", - Long: `controller command description`, + Short: "Motor controller command", + Long: `Motor controller command`, Run: func(cmd *cobra.Command, args []string) { - fmt.Println("controller called") + cmd.Help() //Nothing to do, show help }, } diff --git a/src/cmd/controller/getJoints.go b/src/cmd/controller/getJoints.go index aae6e04..e1be81d 100644 --- a/src/cmd/controller/getJoints.go +++ b/src/cmd/controller/getJoints.go @@ -22,10 +22,9 @@ import ( // getJointsCmd represents the getJoints command var getJointsCmd = &cobra.Command{ Use: "getJoints", - Short: "getJoints command descritpion", - Long: `getJoints command descritpion`, + Short: "Retreive motor positions", + Long: `Retreive the positions of the motor controlled by Motor Controller server`, Run: func(cmd *cobra.Command, args []string) { - // fmt.Println("getJoints called") GetJoints() }, } diff --git a/src/cmd/controller/serve.go b/src/cmd/controller/serve.go index a392445..aea2a13 100644 --- a/src/cmd/controller/serve.go +++ b/src/cmd/controller/serve.go @@ -30,8 +30,8 @@ var ( // serveCmd represents the serve command var serveCmd = &cobra.Command{ Use: "serve", - Short: "serve command descritpion", - Long: `serve command descritpion`, + Short: "Start Motor Controller server", + Long: `Start Motor Controller server`, Run: func(cmd *cobra.Command, args []string) { fmt.Println("serve called with: Port ", port) fmt.Println(cmd.CommandPath()) @@ -66,7 +66,7 @@ type server struct { func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, error) { //TODO: use as a coroutine to prevent blocking the main thread var( - tgt_angles = []float64{in.GetAngles()[0], in.GetAngles()[0], in.GetAngles()[0]} //TODO: Replace by in.GetAngles() + tgt_angles = in.GetAngles() //TODO: Replace by in.GetAngles() cmd_vel = make([]float64, len(tgt_angles)) //Commanded velocities motor_pos = []float64{} //Current motor positions max_vels = []float64{} //Maximum velocities @@ -78,7 +78,7 @@ func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, erro ) //Get motor states - for i := range tgt_angles { + for i := range motor_configs { //TODO: Use a coroutine to avoid blocking the main thread motor_state := getMotorState(i) //Get current motor state motor_pos = append(motor_pos, motor_state.Angle) @@ -92,10 +92,14 @@ func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, erro // log.Printf("t: %v - %v", slices.Min(traj_times), traj_times) if controller_error ==""{ //If no error, check for valid target angles - for i, angle := range tgt_angles { - if angle > motor_configs[i].Max_pos || angle < motor_configs[i].Min_pos{ - controller_error = "InvalidTargetAngles" - break + if len(tgt_angles)> len(motor_configs){ //Check size + controller_error = "InvalidRequestSize" + } else { //Check angles limit + for i, angle := range tgt_angles { + if angle > motor_configs[i].Max_pos || angle < motor_configs[i].Min_pos{ + controller_error = "InvalidTargetAngles" + break + } } } } @@ -136,6 +140,9 @@ func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, erro case "MotorFault": log.Printf("ERROR - Motor error preventing command: %v", motor_errors) return &pb.Angles{Angles: motor_pos}, nil //Return motor positions TODO: Return error message + case "InvalidRequestSize": + log.Printf("ERROR - Request size [%d] is invalid. Expected size [%d]", len(tgt_angles), len(motor_configs)) + return &pb.Angles{Angles: motor_pos}, nil case "InvalidTargetAngles": log.Print("ERROR - out of limits:") for i, tgt := range tgt_angles{ @@ -191,7 +198,7 @@ func setMotorVel(idx int, vel float64){ //TODO: Fix compiling issue with google.protobuf.Empty message func (s *server) GetJoints(ctx context.Context, in *pb.Empty) (*pb.Angles, error) { - //TODO: use a coroutines to prevent blocking the main thread + //TODO: use a coroutines to prevent blocking the main thread and server crash in case of errors var angles = []float64{} for i, _ := range motor_configs { angles = append(angles,getMotorState(i).Angle) diff --git a/src/cmd/controller/setJoints.go b/src/cmd/controller/setJoints.go index 87d873f..4586698 100644 --- a/src/cmd/controller/setJoints.go +++ b/src/cmd/controller/setJoints.go @@ -6,7 +6,7 @@ package controller import ( "fmt" - + "strconv" "github.com/spf13/cobra" "context" @@ -22,16 +22,21 @@ import ( // setJointsCmd represents the setJoints command var setJointsCmd = &cobra.Command{ Use: "setJoints", - Short: "setJoints command descritpion", - Long: `setJoints command descritpion`, + Short: "Set target joint angles for the motors", + Long: `Request the Motor Controller to command the motors to reach the target joint angles`, Run: func(cmd *cobra.Command, args []string) { - fmt.Println("setJoints called with target :", cmd.Flag("j").Value) - tgt_angles, err:=cmd.Flags().GetFloat64Slice("j") - if err!= nil { - log.Fatalf("Failed read requested velocities %v", err) - }else{ - SetJoints(tgt_angles) + //Parse target angles + var tgt_angles []float64 + for _, arg := range args { + angle, err := strconv.ParseFloat(arg, 64) + if err!= nil{ + log.Fatalf("Failed to parse joint angle %v", err) + }else{ + tgt_angles = append(tgt_angles, angle) + } } + //Set joint angles + SetJoints(tgt_angles) }, } @@ -46,10 +51,8 @@ func init() { // Cobra supports local flags which will only run when this command // is called directly, e.g.: - //TODO: Anonymous flag (with anonymous flag group ?) - //TODO: Fix input of multiple values as slice - setJointsCmd.Flags().Float64Slice("j", nil, "Target joint values") //or IntSlice ? - setJointsCmd.MarkFlagRequired("j") + setJointsCmd.Flags().Float64Slice("", nil, "Target joint values") //Only used for help message + // setJointsCmd.MarkFlagRequired("") } //gRPC Client @@ -70,5 +73,5 @@ func SetJoints(tgt_angles []float64){ if err != nil { log.Fatalf("could not send: %v", err) } - log.Printf("Received: %v", r.GetAngles()) + log.Printf("Moving to (°): %v", r.GetAngles()) } \ No newline at end of file diff --git a/src/cmd/motor/listen.go b/src/cmd/motor/listen.go index ae14f57..aa9f264 100644 --- a/src/cmd/motor/listen.go +++ b/src/cmd/motor/listen.go @@ -22,10 +22,10 @@ import ( // listenCmd represents the listen command var listenCmd = &cobra.Command{ Use: "listen", - Short: "listen command descritpion", - Long: `listen command descritpion`, + Short: "Listen motor state", + Long: `Continuous retrieval of complete motor state from motor server`, Run: func(cmd *cobra.Command, args []string) { - fmt.Println("listen called with ID: ", cmd.Flag("id").Value) + // fmt.Println("listen called with ID: ", cmd.Flag("id").Value) updateConfig() listen() //Blocking call }, diff --git a/src/cmd/motor/motor.go b/src/cmd/motor/motor.go index ee5a8eb..cd234d2 100644 --- a/src/cmd/motor/motor.go +++ b/src/cmd/motor/motor.go @@ -39,10 +39,10 @@ var( // motorCmd represents the motor command var motorCmd = &cobra.Command{ Use: "motor", - Short: "motor command descritpion", - Long: `motor command descritpion`, + Short: "Motor command", + Long: `Motor command`, Run: func(cmd *cobra.Command, args []string) { - fmt.Println("motor called with ID: ", cmd.Flag("id").Value) + cmd.Help() //Nothing to do, show help }, } @@ -54,7 +54,7 @@ func init() { // Cobra supports Persistent Flags which will work for this command // and all subcommands, e.g.: motorCmd.PersistentFlags().Uint16Var(&motorID, "id", 0, "Identifier number") - motorCmd.MarkFlagRequired("id") + motorCmd.MarkPersistentFlagRequired("id") // Cobra supports local flags which will only run when this command // is called directly, e.g.: diff --git a/src/cmd/motor/moveVel.go b/src/cmd/motor/moveVel.go index 40ec378..9e5078e 100644 --- a/src/cmd/motor/moveVel.go +++ b/src/cmd/motor/moveVel.go @@ -6,6 +6,7 @@ package motor import ( "fmt" + "strconv" "github.com/spf13/cobra" @@ -22,16 +23,20 @@ import ( // moveVelCmd represents the moveVel command var moveVelCmd = &cobra.Command{ Use: "moveVel", - Short: "moveVel command descritpion", - Long: `moveVel command descritpion`, + Short: "Set velocity of motor", + Long: `Request motor server to move at given velocity`, Run: func(cmd *cobra.Command, args []string) { - fmt.Println("moveVel called with: ID ", cmd.Flag("id").Value, " Vel ", cmd.Flag("vel").Value) - updateConfig() - var vel, err = cmd.Flags().GetFloat64("vel") - if err!= nil { - log.Fatalf("Failed read requested velocity %v", err) + //Parse target velocity + if len(args)!=1 { + log.Fatalf("Wrong number of arguments") }else{ - moveVel(vel) + vel, err := strconv.ParseFloat(args[0],64) + if err!= nil{ + log.Fatalf("Failed to parse joint velocity %v", err) + } else { + updateConfig() + moveVel(vel) //Set motor velocity + } } }, } @@ -47,8 +52,8 @@ func init() { // Cobra supports local flags which will only run when this command // is called directly, e.g.: - //TODO: Anonymous flag (with anonymous flag group ?) - moveVelCmd.Flags().Float64("vel", 0, "Velocity (degrees/s)") + moveVelCmd.Flags().Float64("", 0, "Velocity (degrees/s)") //Only used for help message + // moveVelCmd.MarkFlagRequired("vel") } //gRPC Client @@ -68,5 +73,7 @@ func moveVel(cmd_vel float64){ _, err = c.SetVelocity(ctx, &pb.Velocity{Velocity: cmd_vel}) if err != nil { log.Fatalf("could not send: %v", err) + } else { + log.Printf("Requested velocity [%f]°/s to motor %d", cmd_vel, curr_config.Id) } } \ No newline at end of file diff --git a/src/cmd/motor/serve.go b/src/cmd/motor/serve.go index ac07f33..9da4c6d 100644 --- a/src/cmd/motor/serve.go +++ b/src/cmd/motor/serve.go @@ -27,11 +27,9 @@ var( // serveCmd represents the serve command var serveCmd = &cobra.Command{ Use: "serve", - Short: "serve command descritpion", - Long: `serve command descritpion`, + Short: "Start motor server", + Long: `Start motor server`, Run: func(cmd *cobra.Command, args []string) { - fmt.Println("serve called with ID ", cmd.Flag("id").Value) - // fmt.Println(cmd.CommandPath()) updateConfig() init_sim() serve() diff --git a/src/cmd/root.go b/src/cmd/root.go index 2f0dcd2..ec42eb0 100644 --- a/src/cmd/root.go +++ b/src/cmd/root.go @@ -42,12 +42,12 @@ func init() { // will be global for your application. RootCmd.PersistentFlags().StringVarP(&cfgFile, "config", "c", "", "config file (default is $HOME/.cobra.yaml)") - //RootCmd.MarkFlagRequired("config") + RootCmd.MarkPersistentFlagRequired("config") // viper.BindPFlag("config", RootCmd.PersistentFlags().Lookup("config")) // Cobra also supports local flags, which will only run // when this action is called directly. - RootCmd.Flags().BoolP("toggle", "t", false, "Help message for toggle") + // RootCmd.Flags().BoolP("toggle", "t", false, "Help message for toggle") } // initConfig reads in config file and ENV variables if set.