From 88eaa668a5b8425f4361be382520ed8c04ac7d1d Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Mon, 13 Nov 2023 13:49:58 +0100 Subject: [PATCH 1/5] Update crazyflie_server.py --- crazyflie/scripts/crazyflie_server.py | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index ad8e299b4..a95c06e0a 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -24,8 +24,7 @@ from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType -from crazyflie_interfaces.msg import Hover -from crazyflie_interfaces.msg import LogDataGeneric +from crazyflie_interfaces.msg import Hover, LogDataGeneric, FullState from motion_capture_tracking_interfaces.msg import NamedPoseArray from std_srvs.srv import Empty @@ -246,6 +245,10 @@ def __init__(self): Hover, name + "/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10 ) + self.create_subscription( + Hover, name + + "/cmd_full_state", partial(self._cmd_full_state_changed, uri=uri), 10 + ) qos_profile = QoSProfile(reliability =QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=1, @@ -846,6 +849,21 @@ def _cmd_hover_changed(self, msg, uri=""): self.swarm._cfs[uri].cf.commander.send_hover_setpoint(vx, vy, yawrate, z) self.get_logger().info(f"{uri}: Received hover topic {vx} {vy} {yawrate} {z}") + def _cmd_full_state_changed(self, msg, uri=""): + """ + Topic update callback to full state cmd topic + """ + pos = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z] + vel = [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z] + acc = [msg.acc.x, msg.acc.y, msg.acc.z] + q = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w] + roll_rate = msg.twist.angular.x + pitch_rate = msg.twist.angular.y + yaw_rate = msg.twist.angular.z + + self.swarm._cfs[uri].cf.commander.send_hover_setpoint(pos, vel, acc, q, roll_rate, pitch_rate, yaw_rate) + self.get_logger().info(f"{uri}: Received full state topic {pose} {vel} {acc} {q}, {roll_rate}, {pitch_rate}, {yaw_rate}) + def _remove_logging(self, request, response, uri="all"): """ Service callback to remove logging blocks of the crazyflie From c849435d307649813e44f6ca2ef0fcf395290612 Mon Sep 17 00:00:00 2001 From: torobo Date: Tue, 14 Nov 2023 14:48:15 +0900 Subject: [PATCH 2/5] debug cmd_full_state cb --- crazyflie/scripts/crazyflie_server.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index a95c06e0a..1b6018d72 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -246,7 +246,7 @@ def __init__(self): "/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10 ) self.create_subscription( - Hover, name + + FullState, name + "/cmd_full_state", partial(self._cmd_full_state_changed, uri=uri), 10 ) qos_profile = QoSProfile(reliability =QoSReliabilityPolicy.BEST_EFFORT, @@ -861,8 +861,8 @@ def _cmd_full_state_changed(self, msg, uri=""): pitch_rate = msg.twist.angular.y yaw_rate = msg.twist.angular.z - self.swarm._cfs[uri].cf.commander.send_hover_setpoint(pos, vel, acc, q, roll_rate, pitch_rate, yaw_rate) - self.get_logger().info(f"{uri}: Received full state topic {pose} {vel} {acc} {q}, {roll_rate}, {pitch_rate}, {yaw_rate}) + self.swarm._cfs[uri].cf.commander.send_full_state_setpoint(pos, vel, acc, q, roll_rate, pitch_rate, yaw_rate) + self.get_logger().info(f"{uri}: Received full state topic {pos} {vel} {acc} {q}, {roll_rate}, {pitch_rate}, {yaw_rate}") def _remove_logging(self, request, response, uri="all"): """ From 455eceb5cba48333782a816e784458c72cf2aabf Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Wed, 13 Dec 2023 09:38:44 +0100 Subject: [PATCH 3/5] Update overview.rst --- docs2/overview.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs2/overview.rst b/docs2/overview.rst index 7cb09506e..c824081d2 100644 --- a/docs2/overview.rst +++ b/docs2/overview.rst @@ -75,7 +75,7 @@ Support functionality with backends +---------------------+---------+-----------+---------+ | Manual control | +---------------------+---------+-----------+---------+ -| - cmd_full_state | Yes | No | Yes | +| - cmd_full_state | Yes | Yes | Yes | +---------------------+---------+-----------+---------+ | - cmd_position | Yes | No | No | +---------------------+---------+-----------+---------+ From 253a7aa54d9711a8605b672432f417d2eddb61ac Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Thu, 14 Dec 2023 09:49:22 +0100 Subject: [PATCH 4/5] remove logging --- crazyflie/scripts/crazyflie_server.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index ea75853c0..ca44a9320 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -946,8 +946,6 @@ def _cmd_hover_changed(self, msg, uri=""): yawrate = -1.0*degrees(msg.yaw_rate) self.swarm._cfs[uri].cf.commander.send_hover_setpoint( vx, vy, yawrate, z) - self.get_logger().info( - f"{uri}: Received hover topic {vx} {vy} {yawrate} {z}") def _cmd_full_state_changed(self, msg, uri=""): """ @@ -960,9 +958,7 @@ def _cmd_full_state_changed(self, msg, uri=""): roll_rate = msg.twist.angular.x pitch_rate = msg.twist.angular.y yaw_rate = msg.twist.angular.z - self.swarm._cfs[uri].cf.commander.send_full_state_setpoint(pos, vel, acc, q, roll_rate, pitch_rate, yaw_rate) - self.get_logger().info(f"{uri}: Received full state topic {pos} {vel} {acc} {q}, {roll_rate}, {pitch_rate}, {yaw_rate}") def _remove_logging(self, request, response, uri="all"): """ From ba56210da49a9a1029baf2714c03ec17dc2677fe Mon Sep 17 00:00:00 2001 From: Kimberly McGuire Date: Thu, 14 Dec 2023 09:50:19 +0100 Subject: [PATCH 5/5] Update overview.rst --- docs2/overview.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs2/overview.rst b/docs2/overview.rst index c824081d2..014cfc93d 100644 --- a/docs2/overview.rst +++ b/docs2/overview.rst @@ -75,7 +75,7 @@ Support functionality with backends +---------------------+---------+-----------+---------+ | Manual control | +---------------------+---------+-----------+---------+ -| - cmd_full_state | Yes | Yes | Yes | +| - cmd_full_state | Yes | Yes | Yes | +---------------------+---------+-----------+---------+ | - cmd_position | Yes | No | No | +---------------------+---------+-----------+---------+