Commit 7961567d authored by bucyril's avatar bucyril
Browse files

renamed messages from xyzControl to xyzCommand

parent f91f93a6
......@@ -58,9 +58,9 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files(
FILES
ViconData.msg
MotorControl.msg
AngleControl.msg
RateControl.msg
MotorCommand.msg
AngleCommand.msg
RateCommand.msg
Setpoint.msg
)
......
......@@ -64,24 +64,20 @@ class PPSRadioClient:
This callback is executed as soon as the connection to the
quadrotor is established.
"""
#print "Connection to %s successful: " % link_uri
rospy.loginfo("Connection to %s successful: " % link_uri)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
#print "Connection to %s failed: %s" % (link_uri, msg)
rospy.logerr("Connection to %s failed: %s" % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
#print "Connection to %s lost: %s" % (link_uri, msg)
rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
#print "Disconnected from %s" % link_uri
rospy.logwarn("Disconnected from %s" % link_uri)
def _send_to_commander(self,roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode):
......@@ -108,12 +104,11 @@ def rateControlCallback(data):
if __name__ == '__main__':
rospy.init_node('CrazyRadio', anonymous=True)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
while not rospy.is_shutdown():
# Initialize the low-level drivers (don't list the debug drivers)
# Scan for Crazyflies and use the first one found
rospy.loginfo("Scanning interfaces for Crazyflies...")
available=[]
......@@ -127,7 +122,10 @@ if __name__ == '__main__':
#TODO: load address from parameters
cf_client = PPSRadioClient(available[0][0])
time.sleep(3.0)
#TODO: register action/subscriber/.. callback
#TODO: change publisher name if not correct
rospy.Subscriber("PPSClient/MotorCommand", MotorCommand, motorControlCallback)
rospy.Subscriber("PPSClient/AngleCommand", AngleCommand, angleControlCallback)
rospy.Subscriber("PPSClient/RateCommand", RateCommand, rateControlCallback)
rospy.spin()
else:
......
ViconData crazyflieLocation
Setpoint setpoint
---
AngleControl controlOutput
AngleCommand controlOutput
ViconData crazyflieLocation
Setpoint setpoint
---
MotorControl controlOutput
MotorCommand controlOutput
ViconData crazyflieLocation
Setpoint setpoint
---
RateControl controlOutput
RateCommand controlOutput
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