Commit 24f75cf7 authored by bucyril's avatar bucyril
Browse files

CrazyRadio now loads address from param file

parent 3d8daf1a
......@@ -94,7 +94,7 @@ class PPSRadioClient:
def motorCommandCallback(data):
"""Callback for motor controller actions"""
cf_client._send_to_commander(0, 0, 0, 0, data.cmd1 * 1000, data.cmd2 * 1000, data.cmd3 * 1000, data.cmd4 * 1000, CONTROLLER_MOTOR)
cf_client._send_to_commander(0, 0, 0, 0, data.cmd1, data.cmd2 * 1000, data.cmd3 * 1000, data.cmd4 * 1000, CONTROLLER_MOTOR)
rospy.loginfo("motor controller callback: %s, %s, %s, %s", data.cmd1, data.cmd2, data.cmd3, data.cmd4)
def angleCommandCallback(data):
......@@ -114,33 +114,23 @@ if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
while not rospy.is_shutdown():
# Scan for Crazyflies and use the first one found
rospy.loginfo("Scanning interfaces for Crazyflies...")
available=[]
available = cflib.crtp.scan_interfaces()
rospy.loginfo("Crazyflies found:")
for i in available:
rospy.loginfo("----------------------------------------------------------------")
rospy.loginfo(i[0])
for i in available:
print i[0]
if len(available) > 0:
global cf_client
#TODO: load address from parameters
cf_client = PPSRadioClient("radio://0/72/2M")
#cf_client = PPSRadioClient(available[0][0])
time.sleep(2.0)
#TODO: change publisher name if not correct
rospy.Subscriber("/PPSClient/MotorCommand", MotorCommand, motorCommandCallback)
rospy.Subscriber("/PPSClient/AngleCommand", AngleCommand, angleCommandCallback)
rospy.Subscriber("/PPSClient/RateCommand", RateCommand, rateCommandCallback)
rospy.spin()
rospy.loginfo("Turning off crazyflie")
cf_client._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, CONTROLLER_MOTOR)
cf_client._cf.close_link()
else:
rospy.logerr("No Crazyflies found")
if rospy.has_param("/CrazyRadio/CrazyFlieAddress"):
radio_address = rospy.get_param("/CrazyRadio/CrazyFlieAddress")
rospy.loginfo("Connecting to %s" % radio_address)
global cf_client
#TODO: load address from parameters
cf_client = PPSRadioClient(radio_address)
time.sleep(1.0)
#TODO: change publisher name if not correct
rospy.Subscriber("/PPSClient/MotorCommand", MotorCommand, motorCommandCallback)
rospy.Subscriber("/PPSClient/AngleCommand", AngleCommand, angleCommandCallback)
rospy.Subscriber("/PPSClient/RateCommand", RateCommand, rateCommandCallback)
rospy.spin()
rospy.loginfo("Turning off crazyflie")
cf_client._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, CONTROLLER_MOTOR)
cf_client._cf.close_link()
else:
rospy.logerr("No radio address provided")
<launch>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py"/>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/launch/studentParams.yaml" />
</node>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
</node>
......@@ -19,4 +21,4 @@
</node>
-->
</launch>
\ No newline at end of file
</launch>
TeamName: 'Two'
CrazyFlieName: "cfTwo"
CrazyFlieAddress: "radio://0/99/2M"
#controllertypes to add with adjustable
#motor, angle and rate
\ No newline at end of file
#motor, angle and rate
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment