Commit c50ce8f3 authored by roangel's avatar roangel
Browse files

weird conflict?

parents f1f50837 ff054b34
......@@ -2,5 +2,7 @@ pps_ws/build/
pps_ws/devel/
*.pyc
*.orig
*.bag
pps_ws/src/d_fall_pps/GUI_Qt/build*
......@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
genmsg
rosbag
roslib
)
......@@ -177,7 +178,7 @@ generate_messages(
catkin_package(
INCLUDE_DIRS include ${MY_LIB_PATH_INC} # GUI -- include headers from GUI in package
LIBRARIES
CATKIN_DEPENDS roscpp rospy std_msgs rosbag
CATKIN_DEPENDS roscpp rospy std_msgs rosbag roslib
DEPENDS
)
......@@ -195,9 +196,9 @@ include_directories(
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/d_fall_pps.cpp
# )
##add_library(${PROJECT_NAME}
## src/blablalba.cpp
##)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
......
<launch>
<!-- <node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService"> -->
<!-- </node> -->
<node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
</node>
<!-- <node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py"> -->
<!-- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" /> -->
<!-- </node> -->
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
<rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
</node>
<!-- <node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient"> -->
<!-- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" /> -->
<!-- </node> -->
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<!-- <node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService"> -->
<!-- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" /> -->
<!-- </node> -->
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<!-- <node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService"> -->
<!-- </node> -->
<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
</node>
<!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
<node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" />
<!-- <node pkg="rosbag" type="record" name="testbag" /> -->
<node pkg="rosbag" type="record" name="testbag" />
<!-- When we have a GUI, this has to be adapted and included -->
......
......@@ -45,10 +45,12 @@
<build_depend>std_msgs</build_depend>
<build_depend>genmsg</build_depend>
<build_depend>rosbag</build_depend>
<build_depend>roslib</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>rosbag</run_depend>
<run_depend>roslib</run_depend>
<!-- The export tag contains other, unspecified, tags -->
......
......@@ -25,38 +25,91 @@
#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
#include "CentralManagerService.h"
using namespace d_fall_pps;
using namespace std;
//reads crazyflie db from the file specified in params
/*CrazyflieDB readCrazyflieDB() {
return 0;
}*/
//writes crazyflie db to the file specified in params
void writeCrazyflieDB(CrazyflieDB& cfDB) {
}
CrazyflieDB crazyflieDB;
bool cmRead(CMRead::Request &request, CMRead::Response &response) {
response.crazyflieDB = crazyflieDB;
return true;
}
int findEntryByStudID(unsigned int studID) {
for(int i = 0; i < crazyflieDB.crazyflieEntries.size(); i++) {
CrazyflieEntry entry = crazyflieDB.crazyflieEntries[i];
if(entry.studentID == studID) {
return i;
}
}
return -1;
}
bool cmQuery(CMQuery::Request &request, CMQuery::Response &response) {
int cfIndex = findEntryByStudID(request.studentID);
if(cfIndex != -1) {
response.crazyflieContext = crazyflieDB.crazyflieEntries[cfIndex].crazyflieContext;
return true;
} else {
return false;
}
}
return true;
int findEntryByCF(string name) {
for(int i = 0; i < crazyflieDB.crazyflieEntries.size(); i++) {
CrazyflieEntry entry = crazyflieDB.crazyflieEntries[i];
string cfName = entry.crazyflieContext.crazyflieName;
if(cfName == name) {
return i;
}
}
return -1;
}
bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response) {
return true;
switch(request.mode) {
case ENTRY_INSERT_OR_UPDATE: {
string cfName = request.crazyflieEntry.crazyflieContext.crazyflieName;
int cfIndex = findEntryByCF(cfName);
if(cfIndex == -1) {
crazyflieDB.crazyflieEntries.push_back(request.crazyflieEntry);
} else {
crazyflieDB.crazyflieEntries[cfIndex] = request.crazyflieEntry;
}
return true;
}
case ENTRY_REMOVE: {
string cfName = request.crazyflieEntry.crazyflieContext.crazyflieName;
int cfIndex = findEntryByCF(cfName);
if(cfIndex == -1) {
return false;
} else {
crazyflieDB.crazyflieEntries.erase(crazyflieDB.crazyflieEntries.begin() +cfIndex);
return true;
}
}
default: return false;
}
}
bool cmCommand(CMCommand::Request &request, CMCommand::Response &response) {
return true;
switch(request.command) {
case CMD_SAVE: {
//writeCrazyflieDB(crazyflieDB);
return true;
}
case CMD_RELOAD: {
//crazyflieDB = readCrazyflieDB();
return true;
}
default: return false;
}
}
int main(int argc, char* argv[]) {
......
......@@ -18,6 +18,7 @@
#include <stdlib.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>
#include <ros/package.h>
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/CentralManager.h"
......@@ -27,6 +28,7 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Int32.h"
#include "std_msgs/UInt32.h"
#include "d_fall_pps/ControlCommand.h"
......@@ -110,31 +112,6 @@ void viconCallback(const ViconData& viconData) {
ROS_ERROR("Failed to call safe controller");
}
}
/*
if(!safetyCheck(data, controllerCall.response.controlOutput)){
ROS_INFO_STREAM("AutocontrolOn >>>>>> SWITCHED OFF");
if(safetyDelay == 0){
ROS_INFO_STREAM("ROS Shutdown");
//bag.close();
ros::shutdown();
}
ControlCommand switchOffControls;
switchOffControls.roll = 0;
switchOffControls.pitch = 0;
switchOffControls.yaw = 0;
switchOffControls.motorCmd1 = 0;
switchOffControls.motorCmd2 = 0;
switchOffControls.motorCmd3 = 0;
switchOffControls.motorCmd4 = 0;
switchOffControls.onboardControllerType = 0;
controllerCall.response.controlOutput = switchOffControls;
}
else{
safetyDelay=20;
}
controlCommandPublisher.publish(controllerCall.response.controlOutput);
......@@ -145,16 +122,29 @@ void viconCallback(const ViconData& viconData) {
std_msgs::Int32 i;
i.data = 42;
bag.write("testfoo: ", ros::Time::now(), str);
bag.write("test42: ", ros::Time::now(), i);
*/
controlCommandPublisher.publish(controllerCall.response.controlOutput);
} else{ //crazyflie disabled
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = 2; //set to motor_mode
controlCommandPublisher.publish(zeroOutput);
}
/*
float32 roll
float32 pitch
float32 yaw
uint16 motorCmd1
uint16 motorCmd2
uint16 motorCmd3
uint16 motorCmd4
*/
std_msgs::UInt32 i;
i.data = controllerCall.response.controlOutput.roll;
bag.write("test: ", ros::Time::now(), controllerCall.response.controlOutput);
controlCommandPublisher.publish(controllerCall.response.controlOutput);
} else { //crazyflie disabled
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = 2; //set to motor_mode
controlCommandPublisher.publish(zeroOutput);
}
}
}
......@@ -259,8 +249,12 @@ int main(int argc, char* argv[]){
usingSafeController = true;
loadSafeController();
bag.open("testbag.bag", rosbag::bagmode::Write);
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO_STREAM(package_path);
std::string testbag_file = package_path + "datarecord.bag";
bag.open(testbag_file, rosbag::bagmode::Write);
//bag.close();
ros::spin();
return 0;
......
uint32 studentID
---
CrazyflieContext context
CrazyflieContext crazyflieContext
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