From 820cce9471e3f56c543409b73c45d13f37a47eda Mon Sep 17 00:00:00 2001
From: bucyril <bucyril@ethz.ch>
Date: Mon, 22 May 2017 11:35:15 +0200
Subject: [PATCH] client not uses most of the CrazyflieContext, except for area
---
.../src/d_fall_pps/crazyradio/CrazyRadio.py | 35 +++++++++----------
pps_ws/src/d_fall_pps/param/ClientConfig.yaml | 3 --
pps_ws/src/d_fall_pps/param/StudentID | 2 +-
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 5 ++-
4 files changed, 22 insertions(+), 23 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 29437afb..0dd17c8c 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -37,8 +37,6 @@ CONTROLLER_ANGLE = 1
CONTROLLER_RATE = 0
RAD_TO_DEG = 57.296
-bag = rosbag.Bag('/home/d-fall/D-FaLL-System/pps_ws/src/d_fall_pps/test.bag', 'w')
-
class PPSRadioClient:
"""
CrazyRadio client that recieves the commands from the controller and
@@ -147,24 +145,25 @@ if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
- if rospy.has_param("~crazyFlieAddress"):
- radio_address = rospy.get_param("~crazyFlieAddress")
- rospy.loginfo("Crazyradio connecting to %s" % radio_address)
- global cf_client
+ while not rospy.has_param("~crazyFlieAddress"):
+ time.sleep(0.05)
+ #wait until address parameter is set by PPSClient
+
+ radio_address = "radio://" + rospy.get_param("~crazyFlieAddress")
+ rospy.loginfo("Crazyradio connecting to %s" % radio_address)
+ global cf_client
- cf_client = PPSRadioClient(radio_address)
- time.sleep(1.0)
+ cf_client = PPSRadioClient(radio_address)
+ time.sleep(1.0)
- rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
+ rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
- rospy.spin()
- rospy.loginfo("Turning off crazyflie")
+ rospy.spin()
+ rospy.loginfo("Turning off crazyflie")
- cf_client._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, CONTROLLER_MOTOR)
- #wait for client to send its commands
- time.sleep(1.0)
+ cf_client._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, CONTROLLER_MOTOR)
+ #wait for client to send its commands
+ time.sleep(1.0)
- cf_client._cf.close_link()
- rospy.loginfo("Link closed")
- else:
- rospy.logerr("No radio address provided")
+ cf_client._cf.close_link()
+ rospy.loginfo("Link closed")
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index 91fbaf88..91885f97 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,6 +1,3 @@
-teamName: "Two"
-crazyFlieName: "cfTwo"
-crazyFlieAddress: "radio://0/69/2M"
safeController: "SafeControllerService/RateController"
customController: ""
diff --git a/pps_ws/src/d_fall_pps/param/StudentID b/pps_ws/src/d_fall_pps/param/StudentID
index 7ed6ff82..00750edc 100644
--- a/pps_ws/src/d_fall_pps/param/StudentID
+++ b/pps_ws/src/d_fall_pps/param/StudentID
@@ -1 +1 @@
-5
+3
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 7797b13d..ecd8c00d 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -150,10 +150,13 @@ void loadCrazyflieContext() {
if(centralManager.call(contextCall)) {
context = contextCall.response.crazyflieContext;
- ROS_INFO_STREAM("CrazyflieContext:" << context);
+ ROS_INFO_STREAM("CrazyflieContext:\n" << context);
} else {
ROS_ERROR("Failed to load context");
}
+
+ ros::NodeHandle nh("CrazyRadio");
+ nh.setParam("crazyFlieAddress", context.crazyflieAddress);
}
void loadSafeController() {
--
GitLab