Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

reinstate getparam for a single crazyflie #497

Closed
wants to merge 5 commits into from
Closed

reinstate getparam for a single crazyflie #497

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f446b6e
reinstate getparam for a single crazyflie
knmcguire May 7, 2024
6b2b9bb
Update crazyflie.py
knmcguire May 27, 2024
94d81ab
Update crazyflie.py
knmcguire May 28, 2024
5d3537f
add right docstrings
knmcguire May 28, 2024
800ceac
Merge branch 'main' into get-param-restore
knmcguire May 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
111 changes: 78 additions & 33 deletions crazyflie_py/crazyflie_py/crazyflie.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,9 @@
#!/usr/bin/env python
"""
This module does contains the Crazyflie and Crazyflieserver classes.

These classes are used for the Crazyflie_example scripts
"""

from collections import defaultdict

Expand Down Expand Up @@ -29,6 +34,15 @@


def arrayToGeometryPoint(a):
"""
Convert an array to a Geometry Point.

Args:
a (list): A list containing the x, y, and z coordinates.

Returns:
Point: A Geometry Point with the coordinates from the input array.
"""
result = Point()
result.x = a[0]
result.y = a[1]
Expand All @@ -53,6 +67,12 @@ class TimeHelper:
"""

def __init__(self, node):
"""
Construct TimeHelper.

Args:
node: ROS node reference.
"""
self.node = node
# self.rosRate = None
self.rateHz = None
Expand Down Expand Up @@ -114,7 +134,8 @@ def __init__(self, node, cfname, paramTypeDict):

# self.tf = tf

self.emergencyService = node.create_client(Empty, prefix + '/emergency')
self.emergencyService = node.create_client(
Empty, prefix + '/emergency')
self.emergencyService.wait_for_service()
self.takeoffService = node.create_client(Takeoff, prefix + '/takeoff')
self.takeoffService.wait_for_service()
Expand Down Expand Up @@ -144,22 +165,26 @@ def __init__(self, node, cfname, paramTypeDict):
self.status = {}

# Query some settings
getParamsService = node.create_client(GetParameters, '/crazyflie_server/get_parameters')
getParamsService.wait_for_service()
self.getParamsService = node.create_client(
GetParameters, '/crazyflie_server/get_parameters')
self.getParamsService.wait_for_service()
req = GetParameters.Request()
req.names = ['robots.{}.initial_position'.format(cfname), 'robots.{}.uri'.format(cfname)]
future = getParamsService.call_async(req)
req.names = ['robots.{}.initial_position'.format(
cfname), 'robots.{}.uri'.format(cfname)]
future = self.getParamsService.call_async(req)
while rclpy.ok():
rclpy.spin_once(node)
if future.done():
response = future.result()
# extract initial position
if response.values[0].type == ParameterType.PARAMETER_INTEGER_ARRAY:
self.initialPosition = np.array(response.values[0].integer_array_value)
self.initialPosition = np.array(
response.values[0].integer_array_value)
elif response.values[0].type == ParameterType.PARAMETER_DOUBLE_ARRAY:
self.initialPosition = np.array(response.values[0].double_array_value)
self.initialPosition = np.array(
response.values[0].double_array_value)
else:
assert(False)
assert (False)

# extract uri
self.uri = response.values[1].string_value
Expand Down Expand Up @@ -391,7 +416,8 @@ def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
pieces = []
for poly in trajectory.polynomials:
piece = TrajectoryPolynomialPiece()
piece.duration = rclpy.duration.Duration(seconds=poly.duration).to_msg()
piece.duration = rclpy.duration.Duration(
seconds=poly.duration).to_msg()
piece.poly_x = poly.px.p.tolist()
piece.poly_y = poly.py.p.tolist()
piece.poly_z = poly.pz.p.tolist()
Expand Down Expand Up @@ -502,26 +528,37 @@ def arm(self, arm=True):
# '/world', '/cf' + str(self.id), rospy.Time(0))
# return np.array(position)

# def getParam(self, name):
# """Returns the current value of the onboard named parameter.
def getParam(self, name):
"""
Get the current value of the onboard named parameter.

# Parameters are named values of various primitive C types that control
# the firmware's behavior. For more information, see
# https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/logparam/.
Parameters are named values of various primitive C types that control
the firmware's behavior. For more information, see
https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/logparam/.

# Parameters are read at system startup over the radio and cached.
# The ROS launch file can also be used to set parameter values at startup.
# Subsequent calls to :meth:`setParam()` will update the cached value.
# However, if the parameter changes for any other reason, the cached value
# might become stale. This situation is not common.
Parameters are read at system startup over the radio and cached.
The ROS launch file can also be used to set parameter values at startup.
Subsequent calls to :meth:`setParam()` will update the cached value.
However, if the parameter changes for any other reason, the cached value
might become stale. This situation is not common.

# Args:
# name (str): The parameter's name.
Args:
name (str): The parameter's name.

# Returns:
# value (Any): The parameter's value.
# """
# return rospy.get_param(self.prefix + '/' + name)
Returns:
value (Any): The parameter's value.
"""
param_name = self.prefix[1:] + '.params.' + name
req = GetParameters.Request()
req.names = [param_name]
future = self.getParamsService.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
param_type = self.paramTypeDict[name]
if param_type == ParameterType.PARAMETER_INTEGER:
param_value = future.result().values[0].integer_value
elif param_type == ParameterType.PARAMETER_DOUBLE:
param_value = future.result().values[0].double_value
return param_value

def setParam(self, name, value):
"""
Expand All @@ -537,9 +574,11 @@ def setParam(self, name, value):
param_name = self.prefix[1:] + '.params.' + name
param_type = self.paramTypeDict[name]
if param_type == ParameterType.PARAMETER_INTEGER:
param_value = ParameterValue(type=param_type, integer_value=int(value))
param_value = ParameterValue(
type=param_type, integer_value=int(value))
elif param_type == ParameterType.PARAMETER_DOUBLE:
param_value = ParameterValue(type=param_type, double_value=float(value))
param_value = ParameterValue(
type=param_type, double_value=float(value))
req = SetParameters.Request()
req.parameters = [Parameter(name=param_name, value=param_value)]
self.setParamsService.call_async(req)
Expand Down Expand Up @@ -779,7 +818,8 @@ def __init__(self):
self.landService.wait_for_service()
self.goToService = self.create_client(GoTo, 'all/go_to')
self.goToService.wait_for_service()
self.startTrajectoryService = self.create_client(StartTrajectory, 'all/start_trajectory')
self.startTrajectoryService = self.create_client(
StartTrajectory, 'all/start_trajectory')
self.startTrajectoryService.wait_for_service()
self.armService = self.create_client(Arm, 'all/arm')
# self.armService.wait_for_service()
Expand All @@ -801,7 +841,8 @@ def __init__(self):
cfnames.append(cfname)

# Query all parameters
listParamsService = self.create_client(ListParameters, '/crazyflie_server/list_parameters')
listParamsService = self.create_client(
ListParameters, '/crazyflie_server/list_parameters')
listParamsService.wait_for_service()
req = ListParameters.Request()
req.depth = ListParameters.Request.DEPTH_RECURSIVE
Expand Down Expand Up @@ -994,16 +1035,20 @@ def setParam(self, name, value):
param_name = 'all.params.' + name
param_type = self.paramTypeDict[name]
if param_type == ParameterType.PARAMETER_INTEGER:
param_value = ParameterValue(type=param_type, integer_value=int(value))
param_value = ParameterValue(
type=param_type, integer_value=int(value))
elif param_type == ParameterType.PARAMETER_DOUBLE:
param_value = ParameterValue(type=param_type, double_value=float(value))
param_value = ParameterValue(
type=param_type, double_value=float(value))
req = SetParameters.Request()
req.parameters = [Parameter(name=param_name, value=param_value)]
self.setParamsService.call_async(req)
except KeyError as e:
self.get_logger().warn(f'(crazyflie.py)setParam : keyError raised {e}')
self.get_logger().warn(
f'(crazyflie.py)setParam : keyError raised {e}')
except Exception as e:
self.get_logger().warn(f'(crazyflie.py)setParam : exception raised {e}')
self.get_logger().warn(
f'(crazyflie.py)setParam : exception raised {e}')

def cmdFullState(self, pos, vel, acc, yaw, omega):
"""
Expand Down
Loading