plan_go_to Requires an Extra t Parameter in cffirmware, Causing Compatibility Issues

[Vincent1029384756]

Hello! I’m running into an issue using the goTo function in Crazyswarm2, specifically when calling plan_go_to in the Crazyflie simulation firmware.

Whenever I ran a script that uses the GoTo service, for example,

ros2 launch crazyflie_examples launch.py script:=figure8 backend:=sim

I get this error message:

[crazyflie_server-1] TypeError: plan_go_to() missing 1 required positional argument: 't'

I noticed that in the definition of GoTo.srv, the parameters required does not include 't':

uint8 group_mask
bool relative
geometry_msgs/Point goal
float32 yaw # deg
builtin_interfaces/Duration duration
---

however, in the definition of plan_to_go from cffirmware, the plan_go_to function require this parameter 't':

int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool relative, bool linear, struct vec hover_pos, float hover_yaw, float duration, float t)
{
	if (relative) {
		hover_pos = vadd(hover_pos, curr_eval->pos);
		hover_yaw += curr_eval->yaw;
	}

	// compute the shortest possible rotation towards 0
	float curr_yaw = normalize_radians(curr_eval->yaw);
	hover_yaw = normalize_radians(hover_yaw);
	float delta_yaw = shortest_signed_angle_radians(curr_yaw, hover_yaw);
	float goal_yaw = curr_yaw + delta_yaw;

	if (linear) {
		struct vec vel = vdiv(vsub(hover_pos,curr_eval->pos), duration);
		float omz = delta_yaw/duration;
		
		piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
		curr_eval->pos, curr_yaw, vel, omz, vzero(),
		hover_pos,      goal_yaw, vel, omz, vzero());
	}
	else {
		piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
		curr_eval->pos, curr_yaw, curr_eval->vel, curr_eval->omega.z, curr_eval->acc,
		hover_pos,      goal_yaw,      vzero(),        0,                  vzero());
	}

	p->reversed = false;
	p->state = TRAJECTORY_STATE_FLYING;
	p->type = TRAJECTORY_TYPE_PIECEWISE;
	p->planned_trajectory.t_begin = t;
	p->trajectory = &p->planned_trajectory;
	return 0;
}

int plan_go_to(struct planner *p, bool relative, bool linear, struct vec hover_pos, float hover_yaw, float duration, float t)
{
	struct traj_eval setpoint = plan_current_goal(p, t);
	return plan_go_to_from(p, &setpoint, relative, linear, hover_pos, hover_yaw, duration, t);
}

Questions:

Is this a compatibility issue between the Crazyflie firmware and the Crazyswarm2 Python API?
Was the t parameter introduced in a recent firmware update, and if so, how should it be handled in the current Crazyswarm2 implementation?
Are there recommended workarounds or updates to resolve this issue?

Any help would be greatly appreciated!

[whoenig]

This commit in the firmware broke API compatibility: bitcraze/crazyflie-firmware@bac4b10.

For now the workaround is to use a firmware version before that commit. We'll have to adjust Software-in-the-loop interface for the new API.

[Vincent1029384756]

Hi, I did some investigation the other day, and discovered a quick fix.

So the goTo function in crazyflie_sil.py originally looks like this:

def goTo(self, goal, yaw, duration, relative=False, groupMask=0):
        if self._isGroup(groupMask):
            if self.mode != CrazyflieSIL.MODE_HIGH_POLY:
                raise ValueError('goTo from low-level modes not yet supported.')
            self.mode = CrazyflieSIL.MODE_HIGH_POLY
            current_time = self.time_func()  # Ensure `t` is computed
            print(f"time_func: {self.time_func}")
            print(f"t from time_func: {self.time_func()}")

            firm.plan_go_to(
                self.planner,
                relative,
                firm.mkvec(*goal),
                yaw, duration, current_time)

In which 6 parameters were passed to the function 'firm.plan_to_go'.
However, in the new firmware, the plan_to_go function looks like this:

def plan_go_to(p, relative, linear, hover_pos, hover_yaw, duration, t):
    return _cffirmware.plan_go_to(p, relative, linear, hover_pos, hover_yaw, duration, t)

In which there are 7 parameter, the extra one being the boolean parameter "linear".

So I tired passing one additional boolean value to the function 'goTo' after 'relative':

def goTo(self, goal, yaw, duration, relative=False, groupMask=0):
        if self._isGroup(groupMask):
            if self.mode != CrazyflieSIL.MODE_HIGH_POLY:
                raise ValueError('goTo from low-level modes not yet supported.')
            self.mode = CrazyflieSIL.MODE_HIGH_POLY
            current_time = self.time_func()  # Ensure `t` is computed
            print(f"time_func: {self.time_func}")
            print(f"t from time_func: {self.time_func()}")

            firm.plan_go_to(
                self.planner,
                relative,
                False,
                firm.mkvec(*goal),
                yaw, duration, current_time)

And the simulation worked.

Lemme know if this is a proper way to quick fix the issue.

Also, what does the new 'linear' parameter do

[whoenig]

Thanks - that's a proper quick fix. We plan to actually add support for the new feature that caused the issue (spiraling) in the simulation, which requires some additional firmware changes for the bindings.

[knmcguire]

Since this is a bug, perhaps better to continue to communicate in the ticket: #587

I've also made a ticket on the Crazyflie-firmware side of things: bitcraze/crazyflie-firmware#1435. There is a PR coming up that would perhaps break it even further so we should fix both.

The python bindings are not part of the Bitcraze nightly builds yet or the crazyswarm2 testing at the office...