Commit a1870da5 authored by roangel's avatar roangel
Browse files

Merge branch 'pps_project' of https://gitlab.ethz.ch/D-FaLL/D-FaLL-System into pps_project

parents 04fc630a 3ee57de6
......@@ -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
MotorCommand.msg
AngleCommand.msg
RateCommand.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
......
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib
roslib.load_manifest('d_fall_pps')
import roslib; roslib.load_manifest('crazypkg')
import rospy
from d_fall_pps.msg import ControlParameters
from crazypkg.msg import ControllerOutputPackage
# General import
......@@ -29,12 +28,15 @@ from cflib.crazyflie.log import LogConfig
# Logging settings
logging.basicConfig(level=logging.ERROR)
class SimpleClient:
CONTROLLER_MOTOR = 2
CONTROLLER_ANGLE = 1
CONTROLLER_RATE = 0
class PPSRadioClient:
"""
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.
CrazyRadio client that recieves the commands from the controller and
sends them in a CRTP package to the crazyflie with the specified
address.
"""
def __init__(self, link_uri):
......@@ -66,64 +68,50 @@ class SimpleClient:
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_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 "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)
self._cf.send_packet(pk)
# 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: ")
def motorCommandCallback(data):
"""Callback for motor controller actions"""
cf_client._send_to_commander(0, 0, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, CONTROLLER_MOTOR)
rospy.loginfo("motor controller callback: %s, %s, %s, %s", data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)
def angleCommandCallback(data):
"""Callback for angle controller actions"""
cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_ANGLE)
rospy.loginfo("angle controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
def rateCommandCallback(data):
"""Callback for rate controller actions"""
cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_RATE)
rospy.loginfo("rate controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
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=[]
......@@ -132,20 +120,20 @@ if __name__ == '__main__':
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/80/250K')
#cf_client = SimpleClient(available[0][0])
time.sleep(5.0)
rospy.Subscriber("FlightControl/topicControllerOutput", ControllerOutputPackage, subscriberControllerOutputCallback)
#TODO: load address from parameters
cf_client = PPSRadioClient(available[0][0])
time.sleep(3.0)
#TODO: change publisher name if not correct
rospy.Subscriber("PPSClient/MotorCommands", MotorCommand, motorCommandCallback)
rospy.Subscriber("PPSClient/AngleCommands", AngleCommand, angleCommandCallback)
rospy.Subscriber("PPSClient/RateCommands", RateCommand, rateCommandCallback)
rospy.spin()
else:
rospy.logerr("No Crazyflies found, cannot run example")
#inp=raw_input('press any key');
time.sleep(0.5)
......
<launch>
<!-- Might be needed when our CrazyRadio works. There is no CrazyRadio.launch file yet!
<include file="$(find d_fall_pps)/launch/CrazyRadio.launch" />
-->
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
</node>
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/launch/studentParams.yaml" />
</node>
<!-- We will need something similar for our Control node(s)
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="FlightControl">
</node>
-->
<!-- When we have a GUI, this has to be adapted and included
<node pkg="d_fall_pps" name="GUI" output="screen" type="GUI">
<rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" />
</node>
-->
</launch>
\ No newline at end of file
TeamName: 'Two'
CrazyFlieName: "cfTwo"
#controllertypes to add with adjustable
#motor, angle and rate
\ No newline at end of file
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>
......
......@@ -17,23 +17,141 @@
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
//the teamname and the assigned crazyflie, will be extracted from studentParams.yaml
std::string team; //is this needed?
std::string cflie;
//debugging
int callbackCalls = 0;
//is called upon every new arrival of data in main
void viconCallback(const d_fall_pps::ViconData& data){
ROS_INFO("Callback called");
ROS_INFO_STREAM(data);
//debugging
++callbackCalls;
//ROS_INFO("Callback called #%d",callbackCalls);
//ROS_INFO("Recived Pitch in this callback: %f", data.pitch);
//ROS_INFO("received data:"); ROS_INFO_STREAM(data);
//ROS_INFO("My teamname is:"); ROS_INFO_STREAM(team);
//ROS_INFO("My crazyflie is:"); ROS_INFO_STREAM(cflie);
//extract data from "data" and publish/add to service for controller
if(data.crazyflieName == cflie){
d_fall_pps::ViconData myDataToPublish;
myDataToPublish.crazyflieName = data.crazyflieName;
myDataToPublish.x = data.x;
myDataToPublish.y = data.y;
myDataToPublish.z = data.z;
myDataToPublish.roll = data.roll;
myDataToPublish.pitch = data.pitch;
myDataToPublish.yaw = data.yaw;
myDataToPublish.acquiringTime = data.acquiringTime;
//ROS_INFO("data to share with right controller:");
//ROS_INFO_STREAM(myDataToPublish);
//TODO:
//Some way of choosing the correct controller: Safe or Custom
}
else {
ROS_INFO("ViconData from other crazyflie received");
}
}
//Send to Crayzradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
void callbackRunRateController(const ros::TimerEvent&)
{
DistributePowerAndSendToCrazyflie(m_pRateController->computeOutput());
}
void DistributePowerAndSendToCrazyflie(ControllerOutput rateControllerOutput)
{
assert(rateControllerOutput.onboardControllerType=eOnboardMotorCmdController);
assert(m_isRateOffboard==true);
rateControllerOutput.motorCmd1=m_CrazyControllerOutput.motorCmd1-rateControllerOutput.roll/2.0-rateControllerOutput.pitch/2.0-rateControllerOutput.yaw;
rateControllerOutput.motorCmd2=m_CrazyControllerOutput.motorCmd2-rateControllerOutput.roll/2.0+rateControllerOutput.pitch/2.0+rateControllerOutput.yaw;
rateControllerOutput.motorCmd3=m_CrazyControllerOutput.motorCmd3+rateControllerOutput.roll/2.0+rateControllerOutput.pitch/2.0-rateControllerOutput.yaw;
rateControllerOutput.motorCmd4=m_CrazyControllerOutput.motorCmd4+rateControllerOutput.roll/2.0-rateControllerOutput.pitch/2.0+rateControllerOutput.yaw;
SendToCrazyflie(rateControllerOutput);
}
void SendToCrazyflie(ControllerOutput package)
{
if(m_isStopped)
{
m_packageToSend.motorCmd1=0;
m_packageToSend.motorCmd2=0;
m_packageToSend.motorCmd3=0;
m_packageToSend.motorCmd4=0;
m_packageToSend.onboardControllerType=eOnboardMotorCmdController;
m_pPublisherControllerOutput->publish(m_packageToSend);
return;
}
else
{
m_packageToSend.roll=package.roll*RAD2DEG;
m_packageToSend.pitch=package.pitch*RAD2DEG;
m_packageToSend.yaw=package.yaw*RAD2DEG;
m_packageToSend.thrust=SaturateToUINT16(package.thrust);
m_packageToSend.motorCmd1=SaturateToUINT16(package.motorCmd1);
m_packageToSend.motorCmd2=SaturateToUINT16(package.motorCmd2);
m_packageToSend.motorCmd3=SaturateToUINT16(package.motorCmd3);
m_packageToSend.motorCmd4=SaturateToUINT16(package.motorCmd4);
m_packageToSend.onboardControllerType=package.onboardControllerType;
m_pPublisherControllerOutput->publish(m_packageToSend);
}
}
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char* argv[]){
ROS_INFO_STREAM("PPSClient started");
ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~");
//get the params defined in studentParams.yaml
if(!nodeHandle.getParam("TeamName",team)){
ROS_ERROR("Failed to get TeamName");
}
if(!nodeHandle.getParam("CrazyFlieName",cflie)){
ROS_ERROR("Failed to get CrazyFlieName");
}
ROS_INFO_STREAM("about to subscribe");
//maybe set second argument to 1 (as we only want the most recent data)
ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
ROS_INFO_STREAM("subscribed");
//publish package_for_crazyradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//m_pNodeHandle=nodeHandle;
//m_pCallbackQueueControlMgr=new ros::CallbackQueue();
//m_pNodeHandle->setCallbackQueue(m_pCallbackQueueControlMgr);
ROS_INFO_STREAM("creating publishers for package_for_crazyradio");
ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::AngleCommandsPackage>("AngleCommands", 10));
ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::RateCommandsPackage>("RateCommands", 10));
ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::MotorCommandsPackage>("MotorCommands", 10));
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
ros::spin();
return 0;
......
......@@ -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
AngleCommand controlOutput
ViconData crazyflieLocation
Setpoint setpoint
---
MotorCommand controlOutput
ViconData crazyflieLocation
Setpoint setpoint
---
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