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

Added messages and services for the controller types

parent af27decc
......@@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
genmsg
)
## System dependencies are found with CMake's conventions
......@@ -57,7 +58,9 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files(
FILES
ViconData.msg
MotorCommands.msg
MotorControl.msg
AngleControl.msg
RateControl.msg
Setpoint.msg
)
......@@ -69,7 +72,9 @@ add_message_files(
# )
add_service_files(
FILES
Controller.srv
MotorController.srv
AngleController.srv
RateController.srv
)
## Generate actions in the 'action' folder
......
......@@ -68,6 +68,7 @@ class SimpleClient:
"""
#print "Connection to %s successful: " % link_uri
rospy.loginfo("Connection to %s successful: " % link_uri)
cf_client._send_to_commander(1, 1, 1, 100, 100, 100, 100, 100, 1)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
......@@ -100,7 +101,7 @@ class SimpleClient:
def _send_to_commander(self,roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffHHHHHH', roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
pk.data = struct.pack('<fffH', roll, pitch, yaw, thrust)
self._cf.send_packet(pk)
# self._cf.commander.send_setpoint (roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
......@@ -135,10 +136,10 @@ if __name__ == '__main__':
# uri would can be specified directly, as for example: radio://0/70/250K
# instead of available[0][0]
global cf_client
cf_client = SimpleClient('radio://0/80/250K')
#cf_client = SimpleClient(available[0][0])
#cf_client = SimpleClient('radio://0/80/250K')
cf_client = SimpleClient(available[0][0])
time.sleep(5.0)
rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
#rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
rospy.spin()
else:
......
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib
roslib.load_manifest('d_fall_pps')
import rospy
from d_fall_pps.msg import ControlParameters
# General import
import time, sys
import struct
import logging
# Add library
#sys.path.append("lib")
# CrazyFlie client imports
import cflib
from cflib.crazyflie import Crazyflie
from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
import cflib.drivers.crazyradio
# Logging import
from cflib.crazyflie.log import LogConfig
# Logging settings
logging.basicConfig(level=logging.ERROR)
class SimpleClient:
"""
Example script that runs several threads that read Vicon measurements
from a file and send it together with the setpoints to the Crazyflie.
It also employs a keyboard event detector that allows the user to
manipulate the setpoints with keys.
"""
def __init__(self, link_uri):
# Setpoints to be sent to the CrazyFlie
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
self.motor1cmd = 0.0
self.motor2cmd = 0.0
self.motor3cmd = 0.0
self.motor4cmd = 0.0
# Initialize the CrazyFlie and add callbacks
self._cf = Crazyflie()
# Add callbacks that get executed depending on the connection status.
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
# Connect to the Crazyflie
print "Connecting to %s" % link_uri
self._cf.open_link(link_uri)
def _connected(self, link_uri):
"""
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)
cf_client._send_to_commander(0, 0, 0, 1000, 0, 0, 0, 0, 0)
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_commands(self,cmd1,cmd2,cmd3,cmd4):
# Send setpoints at the given frequency.
# Fill the CRTP packet with the setpoints and send it to the stabilizer
pk = CRTPPacket()
pk.port = CRTPPort.STABILIZER
pk.data = struct.pack('<ffff', cmd1,cmd2,cmd3,cmd4)
self._cf.send_packet(pk)
print('command')
#print "Motor commands: %f, %f, %f, %f" % (cmd1,cmd2,cmd3,cmd4)
def _send_to_commander(self,roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
#pk.data = struct.pack('<fffHHHHHH', roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
pk.data = struct.pack('<fffH', roll, pitch, yaw, thrust)
self._cf.send_packet(pk)
print(thrust)
# self._cf.commander.send_setpoint (roll, pitch, yaw, thrust,cmd1,cmd2,cmd3,cmd4,mode)
# print "Motor commands: %f, %f, %f, %f" % (cmd1,cmd2,cmd3,cmd4)
def subscriberControllerOutputCallback(data):
#cf_client._send_commands(data.cmd1,data.cmd2,data.cmd3,data.cmd4)
cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust,data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
#rospy.loginfo(data.onboardControllerType);
#rospy.loginfo(data.motorCmd1);
#rospy.logdebug_throttle(2,"sending motor commands to crazyflie: ")
if __name__ == '__main__':
rospy.init_node('CrazyRadio', anonymous=True)
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=[]
available = cflib.crtp.scan_interfaces()
rospy.loginfo("Crazyflies found:")
for i in available:
print i[0]
if len(available) > 0:
# uri would can be specified directly, as for example: radio://0/70/250K
# instead of available[0][0]
global cf_client
#cf_client = SimpleClient('radio://0/111/250K')
cf_client = SimpleClient(available[0][0])
time.sleep(5.0)
#rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
while True:
cf_client._send_to_commander(1, 1, 1, 1000, 100, 100, 100, 100, 100)
rospy.spin()
else:
rospy.logerr("No Crazyflies found, cannot run example")
#inp=raw_input('press any key');
time.sleep(0.5)
cf_client._cf.close_link()
float32 rollAngle
float32 pitchAngle
float32 yawAngle
float32 rollRate
float32 pitchRate
float32 yawRate
......@@ -43,6 +43,7 @@
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>genmsg</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
......
......@@ -15,12 +15,12 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "ros/ros.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/RateController.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/MotorCommands.h"
#include "d_fall_pps/MotorControl.h"
bool calculateControlOutput(d_fall_pps::Controller::Request &request, d_fall_pps::Controller::Response &response) {
bool calculateControlOutput(d_fall_pps::RateController::Request &request, d_fall_pps::RateController::Response &response) {
ROS_INFO("calculate control output");
}
......@@ -29,7 +29,7 @@ int main(int argc, char* argv[]) {
ros::NodeHandle nodeHandle("~");
ros::ServiceServer service = nodeHandle.advertiseService("Controller", calculateControlOutput);
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
ROS_INFO("SafeControllerService ready");
ros::spin();
......
ViconData crazyflieLocation
Setpoint setpoint
---
MotorCommands controlOutput
AngleControl controlOutput
ViconData crazyflieLocation
Setpoint setpoint
---
MotorControl controlOutput
ViconData crazyflieLocation
Setpoint setpoint
---
RateControl 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