v0.77.0-plus-profiling

components/arm/client.go

@@ -110,6 +110,8 @@ func (c *client) MoveThroughJointPositions(
 	options *MoveOptions,
 	extra map[string]interface{},
 ) error {
+	c.logger.Warn(c.name)
+	c.logger.Warn("moving to these positions", positions)
 	ext, err := protoutils.StructToStructPb(extra)
 	if err != nil {
 		return err

motionplan/ik/combinedInverseKinematics_cgo.go

@@ -6,6 +6,7 @@ import (
 	"context"
 	"math/rand"
 	"sync"
+	"time"
 
 	"go.uber.org/multierr"
 	"go.viam.com/utils"
@@ -35,6 +36,7 @@ func CreateCombinedIKSolver(
 	if nCPU == 0 {
 		nCPU = 1
 	}
+	logger.Info("CreateCombinedIKSolver ncpu: %d", nCPU)
 	for i := 1; i <= nCPU; i++ {
 		solver, err := CreateNloptSolver(ik.limits, logger, -1, true, true)
 		nlopt := solver.(*nloptIK)
@@ -56,13 +58,16 @@ func (ik *combinedIK) Solve(ctx context.Context,
 	m func([]float64) float64,
 	rseed int,
 ) error {
+	ik.logger.Info("solve start")
+	defer ik.logger.Info("solve end")
 	var err error
-	ctxWithCancel, cancel := context.WithCancel(ctx)
+	ctxWithTimeout, cancel := context.WithTimeout(ctx, time.Second*3)
 	defer cancel()
 
 	//nolint: gosec
 	randSeed := rand.New(rand.NewSource(int64(rseed)))
 
+	ik.logger.Info("len(ik.solvers) %d", len(ik.solvers))
 	errChan := make(chan error, len(ik.solvers))
 	var activeSolvers sync.WaitGroup
 	defer activeSolvers.Wait()
@@ -79,10 +84,13 @@ func (ik *combinedIK) Solve(ctx context.Context,
 			seedFloats = generateRandomPositions(randSeed, lowerBound, upperBound)
 		}
 
+		tmpi := i
 		utils.PanicCapturingGo(func() {
 			defer activeSolvers.Done()
 
-			errChan <- thisSolver.Solve(ctxWithCancel, c, seedFloats, m, parseed)
+			ik.logger.Infof("solve %d start", tmpi)
+			errChan <- thisSolver.Solve(ctxWithTimeout, c, seedFloats, m, parseed)
+			ik.logger.Infof("solve %d end", tmpi)
 		})
 	}
 
@@ -91,29 +99,36 @@ func (ik *combinedIK) Solve(ctx context.Context,
 
 	var collectedErrs error
 
-	// Wait until either 1) we have a success or 2) all solvers have returned false
-	// Multiple selects are necessary in the case where we get a ctx.Done() while there is also an error waiting
+	// Wait until either 1) all solvers have returned success or error or 2) all solvers have returned false
+	// Multiple selects are necessary in the case where we get a ctxWithTimeout.Done() while there is also an error waiting
 	for !done {
 		select {
 		case <-ctx.Done():
+			ik.logger.Info("ctx done, waiting for activeSolvers")
 			activeSolvers.Wait()
 			return ctx.Err()
 		default:
 		}
 
 		select {
+		case <-ctxWithTimeout.Done():
+			activeSolvers.Wait()
+			done = true
 		case err = <-errChan:
+			ik.logger.Info("got error from errChan, returned: %d", returned)
 			returned++
 			if err != nil {
 				collectedErrs = multierr.Combine(collectedErrs, err)
 			}
 		default:
 			if returned == len(ik.solvers) {
+				ik.logger.Info("done!")
 				done = true
 			}
 		}
 	}
 	cancel()
+	ik.logger.Info("past solutions, collecting errors from all solvers")
 	for returned < len(ik.solvers) {
 		// Collect return errors from all solvers
 		select {
@@ -129,8 +144,14 @@ func (ik *combinedIK) Solve(ctx context.Context,
 			collectedErrs = multierr.Combine(collectedErrs, err)
 		}
 	}
+
+	ik.logger.Info("past second loop, waiting for active solvers")
 	activeSolvers.Wait()
-	return collectedErrs
+	ik.logger.Info("active solvers done")
+	if !done {
+		return collectedErrs
+	}
+	return nil
 }
 
 // DoF returns the DoF of the solver.

motionplan/motionPlanner.go

@@ -66,6 +66,8 @@ type PlanRequest struct {
 
 // validatePlanRequest ensures PlanRequests are not malformed.
 func (req *PlanRequest) validatePlanRequest() error {
+	req.Logger.Info("validatePlanRequest start")
+	defer req.Logger.Info("validatePlanRequest end")
 	if req == nil {
 		return errors.New("PlanRequest cannot be nil")
 	}
@@ -201,6 +203,8 @@ func PlanFrameMotion(ctx context.Context,
 // Replan plans a motion from a provided plan request, and then will return that plan only if its cost is better than the cost of the
 // passed-in plan multiplied by `replanCostFactor`.
 func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanCostFactor float64) (Plan, error) {
+	request.Logger.Info("replan start")
+	defer request.Logger.Info("replan end")
 	// Make sure request is well formed and not missing vital information
 	if err := request.validatePlanRequest(); err != nil {
 		return nil, err
@@ -212,19 +216,25 @@ func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanC
 	if seed, ok := request.Options["rseed"].(int); ok {
 		rseed = seed
 	}
+	request.Logger.Info("before newPlanManager")
 	sfPlanner, err := newPlanManager(request.FrameSystem, request.Logger, rseed)
 	if err != nil {
 		return nil, err
 	}
+	request.Logger.Info("after newPlanManager")
 
 	newPlan, err := sfPlanner.planMultiWaypoint(ctx, request, currentPlan)
 	if err != nil {
 		return nil, err
 	}
+	request.Logger.Info("after planMultiWaypoint")
 
 	if replanCostFactor > 0 && currentPlan != nil {
+		request.Logger.Infof("replanning, replanCostFactor: %d, currentPlan: %p", replanCostFactor, currentPlan)
 		initialPlanCost := currentPlan.Trajectory().EvaluateCost(sfPlanner.opt().scoreFunc)
+		request.Logger.Infof("after first evaluate cost")
 		finalPlanCost := newPlan.Trajectory().EvaluateCost(sfPlanner.opt().scoreFunc)
+		request.Logger.Infof("after second evaluate cost")
 		request.Logger.CDebugf(ctx,
 			"initialPlanCost %f adjusted with cost factor to %f, replan cost %f",
 			initialPlanCost, initialPlanCost*replanCostFactor, finalPlanCost,

services/motion/builtin/builtin.go

@@ -424,12 +424,15 @@ func (ms *builtIn) DoCommand(ctx context.Context, cmd map[string]interface{}) (m
 }
 
 func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging.Logger) (motionplan.Plan, error) {
+	ms.logger.Info("plan start")
+	defer ms.logger.Info("plan end")
 	frameSys, err := ms.fsService.FrameSystem(ctx, req.WorldState.Transforms())
 	if err != nil {
 		return nil, err
 	}
 
 	// build maps of relevant components and inputs from initial inputs
+	ms.logger.Info("before fsService.CurrentInputs")
 	fsInputs, _, err := ms.fsService.CurrentInputs(ctx)
 	if err != nil {
 		return nil, err
@@ -441,6 +444,7 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging.
 		return nil, fmt.Errorf("component named %s not found in robot frame system", req.ComponentName.ShortName())
 	}
 
+	ms.logger.Info("before waypointsFromRequest")
 	startState, waypoints, err := waypointsFromRequest(req, fsInputs)
 	if err != nil {
 		return nil, err
@@ -455,13 +459,17 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging.
 		req.Extra["waypoints"] = nil
 	}
 
+	ms.logger.Info("after waypointsFromRequest")
+
 	// re-evaluate goal poses to be in the frame of World
 	// TODO (RSDK-8847) : this is a workaround to help account for us not yet being able to properly synchronize simultaneous motion across
 	// multiple components. If we are moving component1, mounted on arm2, to a goal in frame of component2, which is mounted on arm2, then
 	// passing that raw poseInFrame will certainly result in a plan which moves arm1 and arm2. We cannot guarantee that this plan is
 	// collision-free until RSDK-8847 is complete. By transforming goals to world, only one arm should move for such a plan.
 	worldWaypoints := []*motionplan.PlanState{}
 	solvingFrame := referenceframe.World
+
+	ms.logger.Infof("waypoints len: %d", len(waypoints))
 	for _, wp := range waypoints {
 		if wp.Poses() != nil {
 			step := referenceframe.FrameSystemPoses{}
@@ -478,9 +486,10 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging.
 			worldWaypoints = append(worldWaypoints, wp)
 		}
 	}
+	ms.logger.Infof("after waypoints")
 
 	// the goal is to move the component to goalPose which is specified in coordinates of goalFrameName
-	return motionplan.PlanMotion(ctx, &motionplan.PlanRequest{
+	plan, err := motionplan.PlanMotion(ctx, &motionplan.PlanRequest{
 		Logger:      logger,
 		Goals:       worldWaypoints,
 		StartState:  startState,
@@ -489,15 +498,24 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq, logger logging.
 		Constraints: req.Constraints,
 		Options:     req.Extra,
 	})
+	if err != nil {
+		return nil, err
+	}
+	ms.logger.Warn("the plan we got is:", plan.Trajectory())
+	return plan, nil
 }
 
 func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory) error {
+	ms.logger.Infof("before execute")
+	defer ms.logger.Infof("after execute")
+
 	// build maps of relevant components from initial inputs
 	_, resources, err := ms.fsService.CurrentInputs(ctx)
 	if err != nil {
 		return err
 	}
 
+	ms.logger.Infof("trajectory len: %d", len(trajectory))
 	// Batch GoToInputs calls if possible; components may want to blend between inputs
 	combinedSteps := []map[string][][]referenceframe.Input{}
 	currStep := map[string][][]referenceframe.Input{}
@@ -554,8 +572,10 @@ func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory
 		}
 	}
 	combinedSteps = append(combinedSteps, currStep)
+	ms.logger.Infof("combinedSteps len: %d", len(combinedSteps))
 
 	for _, step := range combinedSteps {
+		ms.logger.Infof("step len: %d", len(step))
 		for name, inputs := range step {
 			if len(inputs) == 0 {
 				continue
@@ -564,6 +584,7 @@ func (ms *builtIn) execute(ctx context.Context, trajectory motionplan.Trajectory
 			if !ok {
 				return fmt.Errorf("plan had step for resource %s but no resource with that name found in framesystem", name)
 			}
+			ms.logger.Infof("GoToInputs len: %d", len(inputs))
 			if err := r.GoToInputs(ctx, inputs...); err != nil {
 				// If there is an error on GoToInputs, stop the component if possible before returning the error
 				if actuator, ok := r.(inputEnabledActuator); ok {

web/cmd/server/main.go

@@ -3,11 +3,17 @@
 package main
 
 import (
+	"log"
+	"net/http"
+
 	"go.viam.com/utils"
 
 	// registers all components.
 	_ "go.viam.com/rdk/components/register"
 	"go.viam.com/rdk/logging"
+
+	// registers all services.
+	_ "net/http/pprof"
 	// registers all services.
 	_ "go.viam.com/rdk/services/register"
 	"go.viam.com/rdk/web/server"
@@ -16,5 +22,8 @@ import (
 var logger = logging.NewDebugLogger("entrypoint")
 
 func main() {
+	go func() {
+		log.Println(http.ListenAndServe("0.0.0.0:6060", nil))
+	}()
 	utils.ContextualMain(server.RunServer, logger)
 }