Skip to content

GitLab

Commit 1366152e authored by muelmarc's avatar muelmarc
Browse files

solved issued on data record. extended Student.launch with rqt dashboard... 

solved issued on data record. extended Student.launch with rqt dashboard plugin to send manually configured topics.
parent 42698ba2
Hide whitespace changes
Inline Side-by-side
pps_ws/src/d_fall_pps/PPS.perspective 0 → 100644
View file @ 1366152e
{
"keys": {},
"groups": {
"pluginmanager": {
"keys": {
"running-plugins": {
"type": "repr",
"repr": "{u'MultiplotPlugin': [1], u'rqt_publisher/Publisher': [1]}"
}
},
"groups": {
"plugin__MultiplotPlugin__1": {
"keys": {},
"groups": {
"dock_widget__MultiplotWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'Multiplot - [file:///home/d-fall/rqt_multiplot.xml]*'"
}
},
"groups": {}
},
"plugin": {
"keys": {},
"groups": {
"history": {
"keys": {
"max_length": {
"type": "repr",
"repr": "10"
},
"config_0": {
"type": "repr",
"repr": "u'file:///home/d-fall/rqt_multiplot.xml'"
}
},
"groups": {}
}
}
}
}
},
"plugin__rqt_plot__Plot__1": {
"keys": {},
"groups": {
"plugin": {
"keys": {
"autoscroll": {
"type": "repr",
"repr": "u'true'"
},
"plot_type": {
"type": "repr",
"repr": "u'1'"
},
"topics": {
"type": "repr",
"repr": "u''"
},
"y_limits": {
"type": "repr",
"repr": "[u'0', u'1']"
},
"x_limits": {
"type": "repr",
"repr": "[u'0', u'1']"
}
},
"groups": {}
}
}
},
"plugin__rqt_launch__Launch__1": {
"keys": {},
"groups": {
"dock_widget___plugincontainer_top_widget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "u'true'"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'ROS Launch GUI'"
}
},
"groups": {}
},
"plugin": {
"keys": {
"_splitter": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff000000010000000200000064000000640100000009010000000200')",
"pretty-print": " d d "
}
},
"groups": {}
}
}
},
"plugin__rqt_bag__Bag__1": {
"keys": {},
"groups": {
"dock_widget__onboard__Plot": {
"keys": {
"dockable": {
"type": "repr",
"repr": "u'true'"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'onboard__Plot'"
}
},
"groups": {}
},
"dock_widget__onboard__Raw": {
"keys": {
"dockable": {
"type": "repr",
"repr": "u'true'"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'onboard__Raw'"
}
},
"groups": {}
},
"dock_widget__BagWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "u'true'"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'Bag'"
}
},
"groups": {}
}
}
},
"plugin__rqt_publisher__Publisher__1": {
"keys": {},
"groups": {
"dock_widget__PublisherWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
},
"dock_widget_title": {
"type": "repr",
"repr": "u'Message Publisher'"
}
},
"groups": {}
},
"plugin": {
"keys": {
"publishers": {
"type": "repr",
"repr": "u\"[{'type_name': 'std_msgs/Int32', 'topic_name': '/PPSClient/Command', 'enabled': False, 'rate': 1.0, 'expressions': {u'/PPSClient/Command/data': '3'}, 'publisher_id': 0, 'counter': 0}]\""
}
},
"groups": {}
}
}
},
"plugin__rqt_shell__Shell__1": {
"keys": {},
"groups": {
"plugin": {
"keys": {
"shell_type": {
"type": "repr",
"repr": "u'2'"
}
},
"groups": {}
}
}
},
"plugin__rqt_console__Console__1": {
"keys": {},
"groups": {
"plugin": {
"keys": {
"show_highlighted_only": {
"type": "repr",
"repr": "u'false'"
},
"message_limit": {
"type": "repr",
"repr": "u'20000'"
},
"exclude_filters": {
"type": "repr",
"repr": "u'severity'"
},
"table_splitter": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000001000000020000005d000000e70100000009010000000200')",
"pretty-print": " ] "
},
"highlight_filters": {
"type": "repr",
"repr": "u'message'"
},
"paused": {
"type": "repr",
"repr": "u'false'"
},
"settings_exist": {
"type": "repr",
"repr": "u'true'"
},
"filter_splitter": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000001000000020000006f0000006f0100000009010000000200')",
"pretty-print": " o o "
}
},
"groups": {
"exclude_filter_0": {
"keys": {
"enabled": {
"type": "repr",
"repr": "u'true'"
},
"itemlist": {
"type": "repr",
"repr": "u''"
}
},
"groups": {}
},
"highlight_filter_0": {
"keys": {
"regex": {
"type": "repr",
"repr": "u'false'"
},
"text": {
"type": "repr",
"repr": "u''"
},
"enabled": {
"type": "repr",
"repr": "u'true'"
}
},
"groups": {}
}
}
}
}
}
}
},
"mainwindow": {
"keys": {
"geometry": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb0002000000000041000000180000077f0000043700000041000000340000077f0000043700000000000000000780')",
"pretty-print": " A 7 A 4 7 "
},
"state": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd00000001000000030000073f000003eefc0100000002fb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c006900730068006500720057006900640067006500740100000000000003ae0000026500fffffffb00000046004d0075006c007400690070006c006f00740050006c007500670069006e005f005f0031005f005f004d0075006c007400690070006c006f007400570069006400670065007401000003b40000038b0000038b00ffffff0000073f0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
"pretty-print": " Xrqt_publisher__Publisher__1__PublisherWidget FMultiplotPlugin__1__MultiplotWidget 6MinimizedDockWidgetsToolbar "
}
},
"groups": {
"toolbar_areas": {
"keys": {
"MinimizedDockWidgetsToolbar": {
"type": "repr",
"repr": "8"
}
},
"groups": {}
}
}
}
}
}
\ No newline at end of file
pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
View file @ 1366152e
......@@ -12,6 +12,7 @@ import struct
import logging
import rosbag
from rospkg import RosPack
from std_msgs.msg import Float32
from std_msgs.msg import String
......@@ -37,7 +38,9 @@ CONTROLLER_ANGLE = 1
CONTROLLER_RATE = 0
RAD_TO_DEG = 57.296
#bag = rosbag.Bag('~/D-Fall-System/pps_ws/src/d_fall_pps/test.bag', 'w')
rp = RosPack()
record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
bag = rosbag.Bag(record_file, 'w')
class PPSRadioClient:
"""
......@@ -71,15 +74,19 @@ class PPSRadioClient:
def _data_received_callback(self, timestamp, data, logconf):
print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
#print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
batteryVolt = Float32()
stabilizerYaw = Float32()
stabilizerPitch = Float32()
stabilizerRoll = Float32()
batteryVolt.data = data["pm.vbat"]
str = String()
str.data = 'foo'
bag.write('onboard', str)
bag.write('onboard', batteryVolt)
print "status check"
stabilizerYaw.data = data["stabilizer.yaw"]
stabilizerPitch.data = data["stabilizer.pitch"]
bag.write('batteryVoltage', batteryVolt)
bag.write('stabilizerYaw', stabilizerYaw)
bag.write('stabilizerPitch', stabilizerPitch)
bag.write('stabilizerRoll', stabilizerRoll)
def _logging_error(self, logconf, msg):
print "Error when logging %s" % logconf.name
......@@ -93,7 +100,7 @@ class PPSRadioClient:
# Config for Logging
logconf = LogConfig("LoggingTest", 1000)
logconf = LogConfig("LoggingTest", 100)
logconf.add_variable("stabilizer.roll", "float");
logconf.add_variable("stabilizer.pitch", "float");
logconf.add_variable("stabilizer.yaw", "float");
......@@ -147,12 +154,15 @@ if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
#wait until address parameter is set by PPSClient
while not rospy.has_param("~crazyFlieAddress"):
time.sleep(0.05)
#wait until address parameter is set by PPSClient
time.sleep(0.05)
radio_address = "radio://" + rospy.get_param("~crazyFlieAddress")
rospy.loginfo("Crazyradio connecting to %s" % radio_address)
#use this following two lines to connect without data from CentralManager
#radio_address = "radio://0/72/2M"
#rospy.loginfo("manual address loaded")
global cf_client
cf_client = PPSRadioClient(radio_address)
......
pps_ws/src/d_fall_pps/launch/Student.launch
View file @ 1366152e
......@@ -16,6 +16,8 @@
<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
</node>
<node pkg="rqt_gui" name="RqtDashboard" type="rqt_gui" args="-p PPS">
</node>
<!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
......
pps_ws/src/d_fall_pps/param/ClientConfig.yaml
View file @ 1366152e
safeController: "SafeControllerService/RateController"
customController: "CircleControllerService/CircleController"
customController: "FollowCrazyflieService/FollowController"
pps_ws/src/d_fall_pps/param/SafeController.yaml
View file @ 1366152e
......@@ -15,4 +15,4 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
defaultSetpoint: [0.0, 0.0, 0.0, 0.0]
defaultSetpoint: [-0.3, -0.3, -0.6, 0.0]
pps_ws/src/d_fall_pps/src/PPSClient.cpp
View file @ 1366152e
......@@ -110,15 +110,15 @@ void viconCallback(const ViconData& viconData) {
if(global.crazyflieName == context.crazyflieName) {
Controller controllerCall;
CrazyflieData local = global;
coordinatesToLocal(local);
controllerCall.request.ownCrazyflie = local;
if(crazyflieEnabled){
if(!usingSafeController) {
bool success = customController.call(controllerCall);
if(!success) {
ROS_ERROR("Failed to call custom controller, switching to safe controller");
ROS_ERROR_STREAM("custom controller status: valid: " << customController.isValid() << ", exists: " << customController.exists());
......@@ -126,19 +126,19 @@ void viconCallback(const ViconData& viconData) {
usingSafeController = true;
} else {
usingSafeController = !safetyCheck(global, controllerCall.response.controlOutput);
if(usingSafeController) {
ROS_INFO_STREAM("safety check failed, switching to safe controller");
}
ROS_INFO_STREAM("safety check failed, switching to safe controller");
}
}
if(usingSafeController) {
bool success = safeController.call(controllerCall);
if(!success) {
ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
}
}
//ROS_INFO_STREAM("safe controller active: " << usingSafeController);
controlCommandPublisher.publish(controllerCall.response.controlOutput);
......@@ -159,13 +159,16 @@ void viconCallback(const ViconData& viconData) {
void loadParameters(ros::NodeHandle& nodeHandle) {
if(!nodeHandle.getParam("studentID", studentID)) {
ROS_ERROR("Failed to get studentID");
studentID = 5;
}
}
void loadCrazyflieContext() {
CMQuery contextCall;
contextCall.request.studentID = studentID;
studentID = 5;
ROS_INFO_STREAM("myID:" << studentID);
centralManager.waitForExistence(ros::Duration(-1));
if(centralManager.call(contextCall)) {
......@@ -258,11 +261,11 @@ int main(int argc, char* argv[]){
crazyflieEnabled = true;
usingSafeController = true;
loadSafeController();
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO_STREAM(package_path);
std::string record_file = package_path + "datarecord.bag";
std::string record_file = package_path + "LoggingPPSClient.bag";
bag.open(record_file, rosbag::bagmode::Write);
ros::spin();
......
pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
View file @ 1366152e
......@@ -166,11 +166,7 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
CrazyflieData vicon = request.ownCrazyflie;
//bag.write("ViconData", ros::Time::now(), request.ownCrazyflie);
//trial>>>>>>>
float yaw_measured = request.ownCrazyflie.yaw;
//<<<<<<
//move coordinate system to make setpoint origin
request.ownCrazyflie.x -= setpoint[0];
......@@ -286,7 +282,7 @@ int main(int argc, char* argv[]) {
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO_STREAM(package_path);
std::string record_file = package_path + "recordSafeController.bag";
std::string record_file = package_path + "LoggingSafeController.bag";
bag.open(record_file, rosbag::bagmode::Write);
......
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