Commit f4f59b73 authored by muelmarc's avatar muelmarc
Browse files

rosbag included. not working and creating error at startup. rest should...

rosbag included. not working and creating error at startup. rest should however still run. safetycheck added again ... switches off and terminates ros when area boundaries are left
parent 1ead1f62
......@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
genmsg
rosbag
)
## System dependencies are found with CMake's conventions
......@@ -134,7 +135,7 @@ generate_messages(
catkin_package(
INCLUDE_DIRS include
LIBRARIES
CATKIN_DEPENDS roscpp rospy std_msgs
CATKIN_DEPENDS roscpp rospy std_msgs rosbag
DEPENDS
)
......
......@@ -27,6 +27,8 @@
<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" />
<!-- When we have a GUI, this has to be adapted and included
<node pkg="d_fall_pps" name="GUI" output="screen" type="GUI">
......
......@@ -44,9 +44,11 @@
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>genmsg</build_depend>
<build_depend>rosbag</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>rosbag</run_depend>
<!-- The export tag contains other, unspecified, tags -->
......
......@@ -16,6 +16,8 @@
#include "ros/ros.h"
#include <stdlib.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/CentralManager.h"
......@@ -45,27 +47,34 @@ ros::ServiceClient customController;
ros::ServiceClient centralManager;
ros::Publisher controlCommandPublisher;
rosbag::Bag bag;
//describes the area of the crazyflie and other parameters
CrazyflieContext context;
//wheter to use safe of custom controller
bool usingSafeController;
int safetyDelay;
//checks if crazyflie is within allowed area and if custom controller returns no data
bool safetyCheck(ViconData data, ControlCommand controlCommand) {
CrazyflieContext CrazyflieContext;
//position check
if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
return true;
safetyDelay--;
return false;
}
if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
return true;
safetyDelay--;
return false;
}
if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
return true;
safetyDelay--;
return false;
}
return false;
return true;
}
//is called when new data from Vicon arrives
......@@ -73,6 +82,8 @@ void viconCallback(const ViconData& data) {
if(data.crazyflieName == crazyflieName) {
Controller controllerCall;
controllerCall.request.crazyflieLocation = data;
if(!usingSafeController) {
bool success = customController.call(controllerCall);
......@@ -81,7 +92,6 @@ void viconCallback(const ViconData& data) {
usingSafeController = true;
}
//usingSafeController = !safetyCheck(data, controllerCall.response.controlOutput);
usingSafeController = true; //debug
}
......@@ -91,8 +101,40 @@ void viconCallback(const ViconData& data) {
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);
std_msgs::String str;
str.data = std::string("foo");
std_msgs::Int32 i;
i.data = 42;
bag.write("testfoo: ", ros::Time::now(), str);
bag.write("test42: ", ros::Time::now(), i);
}
}
......@@ -167,6 +209,11 @@ int main(int argc, char* argv[]){
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
//ros::service::waitForService("/CentralManagerService/CentralManager");
centralManager = nodeHandle.serviceClient<CentralManager>("/CentralManagerService/CentralManager");
loadCrazyflieContext();
//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
ROS_INFO_STREAM("successfully subscribed to ViconData");
......@@ -182,9 +229,8 @@ int main(int argc, char* argv[]){
usingSafeController = true;
loadSafeController();
//ros::service::waitForService("/CentralManagerService/CentralManager");
centralManager = nodeHandle.serviceClient<CentralManager>("/CentralManagerService/CentralManager");
loadCrazyflieContext();
bag.open("testbag.bag", rosbag::bagmode::Write);
ros::spin();
return 0;
......
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