Commit 530c1f72 authored by beuchatp's avatar beuchatp
Browse files

Convert all occurances of PPS Client to Flying Agenet Client. And adjusted all...

Convert all occurances of PPS Client to Flying Agenet Client. And adjusted all other occurances of PPS accordingly
parent 2eceb1d3
......@@ -327,7 +327,7 @@ if(VICON_LIBRARY)
add_executable(ViconDataPublisher src/nodes/ViconDataPublisher.cpp)
endif()
add_executable(PPSClient src/nodes/PPSClient.cpp
add_executable(FlyingAgentClient src/nodes/FlyingAgentClient.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp
src/classes/GetParamtersAndNamespaces.cpp)
......@@ -420,7 +420,7 @@ if(VICON_LIBRARY)
add_dependencies(ViconDataPublisher dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
endif()
add_dependencies(PPSClient dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(FlyingAgentClient dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(BatteryMonitor dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(DefaultControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(SafeControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -472,7 +472,7 @@ if(VICON_LIBRARY)
target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
endif()
target_link_libraries(PPSClient ${catkin_LIBRARIES})
target_link_libraries(FlyingAgentClient ${catkin_LIBRARIES})
target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
target_link_libraries(DefaultControllerService ${catkin_LIBRARIES})
target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
......
......@@ -742,17 +742,17 @@ void MainGUIWindow::on_refresh_student_ids_button_clicked()
#ifdef CATKIN_MAKE
ui->list_discovered_student_ids->clear();
// \/(\d)\/PPSClient
// \/(\d)\/FlyingAgentClient
ros::V_string v_str;
ros::master::getNodes(v_str);
for(int i = 0; i < v_str.size(); i++)
{
std::string s = v_str[i];
std::smatch m;
//std::regex e ("\\/(\\d)\\/PPSClient");
std::regex e ("\\/agent(\\d\\d\\d)\\/PPSClient");
//std::regex e ("\\/(\\d)\\/FlyingAgentClient");
std::regex e ("\\/agent(\\d\\d\\d)\\/FlyingAgentClient");
// std::regex e("\\/PPSClien(.)");
// std::regex e("\\/FlyingAgentClient(.)");
// while(std::regex_search(s, m, e))
// {
......@@ -1314,7 +1314,6 @@ void MainGUIWindow::customSendYamlAsMessageTimerCallback(const ros::TimerEvent&)
// load parameters from corresponding YAML file
//
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
float MainGUIWindow::getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
{
float val;
......@@ -1324,7 +1323,7 @@ float MainGUIWindow::getParameterFloat(ros::NodeHandle& nodeHandle, std::string
}
return val;
}
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
void MainGUIWindow::getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
......@@ -1334,7 +1333,7 @@ void MainGUIWindow::getParameterFloatVector(ros::NodeHandle& nodeHandle, std::st
ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
}
}
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
int MainGUIWindow::getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
{
int val;
......@@ -1344,7 +1343,7 @@ int MainGUIWindow::getParameterInt(ros::NodeHandle& nodeHandle, std::string name
}
return val;
}
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
void MainGUIWindow::getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
......@@ -1354,7 +1353,7 @@ void MainGUIWindow::getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHa
ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
}
}
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
int MainGUIWindow::getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
{
if(!nodeHandle.getParam(name, val)){
......@@ -1362,7 +1361,7 @@ int MainGUIWindow::getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeH
}
return val.size();
}
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
bool MainGUIWindow::getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
{
bool val;
......
......@@ -81,7 +81,7 @@
<string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
</property>
<property name="text">
<string>IDYYY , PPS_CFXX</string>
<string>IDYYY , CFXX</string>
</property>
<property name="checkable">
<bool>true</bool>
......
......@@ -81,7 +81,27 @@
// Types PPS firmware
// These constants define the modes that can be used for controller this is
// running on-board the Crazyflie 2.0.
// Therefore, the constants defined here need to be in agreement with those
// defined in the firmware running on-board the Crazyflie 2.0.
// The following is a short description about each mode:
//
// CF_COMMAND_TYPE_MOTORS
// In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors
//
// CF_COMMAND_TYPE_RATE
// In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angular rates from the PID rate
// controllers implemented in the Crazyflie 2.0 firmware.
//
// CF_COMMAND_TYPE_ANGLE
// In this mode the Crazyflie will apply the requested 16-bit per motor
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angles from the PID attitude
// controllers implemented in the Crazyflie 2.0 firmware.
#define CF_COMMAND_TYPE_MOTORS 6
#define CF_COMMAND_TYPE_RATE 7
#define CF_COMMAND_TYPE_ANGLE 8
......
......@@ -157,11 +157,11 @@ private:
void loadCrazyflieContext();
// > For requesting the current flying state
// i.e., using the service advertised by the PPS client
// i.e., using the service advertised by the Flying Agent Client
void getCurrentFlyingState();
// > For requesting the current state of the Crazy Radio
// i.e., using the service advertised by the PPS client
// i.e., using the service advertised by the Flying Agent Client
void getCurrentCrazyRadioState();
// > For updating the text in the UI element of
......
......@@ -38,7 +38,7 @@
// COMMANDS FOR THE FLYING STATE/CONTROLLER USED
// The constants that "command" changes in the
// operation state of this agent. These "commands"
// are sent from this GUI node to the "PPSClient"
// are sent from this GUI node to the "FlyingAgentClient"
// node where the command is enacted
// #define CMD_USE_SAFE_CONTROLLER 1
// #define CMD_USE_DEMO_CONTROLLER 2
......
......@@ -106,10 +106,10 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
// SUBSCRIBERS AND PUBLISHERS:
// > For Crazyradio commands based on button clicks
crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("PPSClient/crazyRadioCommand", 1);
crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/crazyRadioCommand", 1);
// > For Flying state commands based on button clicks
flyingStateCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("PPSClient/Command", 1);
flyingStateCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1);
if (m_type == TYPE_AGENT)
{
......@@ -126,7 +126,7 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
batteryLevelSubscriber = base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &ConnectStartStopBar::batteryLevelCallback, this);
// > For updating the "flying_state_label" picture
flyingStateSubscriber = base_nodeHandle.subscribe("PPSClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
flyingStateSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
}
#endif
......@@ -614,7 +614,7 @@ void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
// > Request the current flying state
ros::ServiceClient getCurrentFlyingStateService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("PPSClient/getCurrentFlyingState", false);
ros::ServiceClient getCurrentFlyingStateService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/getCurrentFlyingState", false);
dfall_pkg::IntIntService getFlyingStateCall;
getFlyingStateCall.request.data = 0;
getCurrentFlyingStateService.waitForExistence(ros::Duration(2.0));
......@@ -654,7 +654,7 @@ void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
batteryLevelSubscriber = agent_base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &ConnectStartStopBar::batteryLevelCallback, this);
// > For updating the "flying_state_label" picture
flyingStateSubscriber = agent_base_nodeHandle.subscribe("PPSClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
flyingStateSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
}
else
{
......
......@@ -128,7 +128,7 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
// Only if this is an agent GUI
if (m_type == TYPE_AGENT)
{
controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("PPSClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
}
......@@ -457,7 +457,7 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should
// SUBSCRIBERS
// > For receiving message that the setpoint was changed
controllerUsedSubscriber = agent_base_nodeHandle.subscribe("PPSClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
controllerUsedSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
}
else
{
......
......@@ -26,7 +26,7 @@ void Coordinator::on_refresh_button_clicked()
#ifdef CATKIN_MAKE
// USE A REGULAR EXPRESSION TO IDENTIFY NODES THAT EXIST
// > The regular expression we use is: \/agent(\d\d\d)\/PPSClient
// > The regular expression we use is: \/agent(\d\d\d)\/FlyingAgentClient
// with the different that the escaped character is \\ instead of \ only
// GET A "V_string" OF ALL THE NODES CURRENTLY IN EXISTENCE
......@@ -39,7 +39,7 @@ void Coordinator::on_refresh_button_clicked()
// TEST THE NAME OF THIS NODE AGAINST THE REGULAR EXPRESSION
std::string s = v_str[i];
std::smatch m;
std::regex e ("\\/agent(\\d\\d\\d)\\/PPSClient");
std::regex e ("\\/agent(\\d\\d\\d)\\/FlyingAgentClient");
if(std::regex_search(s, m, e))
{
......
......@@ -107,7 +107,7 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
// SUBSCRIBERS AND PUBLISHERS:
// > For Crazyradio commands based on button clicks
crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("PPSClient/crazyRadioCommand", 1);
crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/crazyRadioCommand", 1);
// > For updating the "rf_status_label" picture
crazyRadioStatusSubscriber = base_nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &CoordinatorRow::crazyRadioStatusCallback, this);
......@@ -119,20 +119,20 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
batteryLevelSubscriber = base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &CoordinatorRow::batteryLevelCallback, this);
// > For Flying state commands based on button clicks
flyingStateCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("PPSClient/Command", 1);
flyingStateCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1);
// > For updating the "flying_state_label" picture
flyingStateSubscriber = base_nodeHandle.subscribe("PPSClient/flyingState", 1, &CoordinatorRow::flyingStateChangedCallback, this);
flyingStateSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &CoordinatorRow::flyingStateChangedCallback, this);
// > For changes in the database that defines {agentID,cfID,flying zone} links
databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &CoordinatorRow::databaseChangedCallback, this);;
centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<dfall_pkg::CMQuery>("CentralManagerService/Query", false);
// > For updating the controller that is currently operating
controllerUsedSubscriber = base_nodeHandle.subscribe("PPSClient/controllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this);
controllerUsedSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/controllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this);
// > For requesting the current flying state,
// this is used only for initialising the icon
getCurrentFlyingStateService = base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("PPSClient/getCurrentFlyingState", false);
getCurrentFlyingStateService = base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/getCurrentFlyingState", false);
// > For requesting the current state of the Crazy Radio,
// this is used only for initialising the icon
......
......@@ -30,7 +30,7 @@ EnableControllerLoadYamlBar::EnableControllerLoadYamlBar(QWidget *parent) :
ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
// CREATE THE COMMAND PUBLISHER
commandPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::IntWithHeader>("PPSClient/Command", 1);
commandPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1);
// CREATE THE REQUEST LOAD YAML FILE PUBLISHER
// Get the node handle to this parameter service
......
......@@ -69,7 +69,7 @@
// The constants that "command" changes in the
// operation state of this agent. These "commands"
// are sent from this GUI node to the "PPSClient"
// are sent from this GUI node to the "FlyingAgentClient"
// node where the command is enacted
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_DEMO_CONTROLLER 2
......@@ -288,7 +288,7 @@ private:
ros::Publisher crazyRadioCommandPublisher;
ros::Subscriber crazyRadioStatusSubscriber;
ros::Publisher PPSClientCommandPublisher;
ros::Publisher FlyingAgentClientCommandPublisher;
ros::Subscriber CFBatterySubscriber;
ros::Subscriber flyingStateSubscriber;
ros::Subscriber batteryStateSubscriber;
......
......@@ -153,11 +153,11 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);
flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
flyingStateSubscriber = nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
batteryStateSubscriber = nodeHandle.subscribe("PPSClient/batteryState", 1, &MainWindow::batteryStateChangedCallback, this);
batteryStateSubscriber = nodeHandle.subscribe("FlyingAgentClient/batteryState", 1, &MainWindow::batteryStateChangedCallback, this);
controllerUsedSubscriber = nodeHandle.subscribe("PPSClient/controllerUsed", 1, &MainWindow::controllerUsedChangedCallback, this);
controllerUsedSubscriber = nodeHandle.subscribe("FlyingAgentClient/controllerUsed", 1, &MainWindow::controllerUsedChangedCallback, this);
safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
......@@ -167,18 +167,18 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
// communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
//ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
ros::NodeHandle nh_PPSClient("PPSClient");
crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
// communication with Flying Agent Client, just to make it possible to communicate through terminal also we use FlyingAgentClient's name
//ros::NodeHandle nh_FlyingAgentClient(m_ros_namespace + "/FlyingAgentClient");
ros::NodeHandle nh_FlyingAgentClient("FlyingAgentClient");
crazyRadioCommandPublisher = nh_FlyingAgentClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
FlyingAgentClientCommandPublisher = nh_FlyingAgentClient.advertise<std_msgs::Int32>("Command", 1);
// > For publishing a message that requests the
// YAML parameters to be re-loaded from file
// > The message contents specify which controller
// the parameters should be re-loaded for
requestLoadControllerYamlPublisher = nh_PPSClient.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
requestLoadControllerYamlPublisher = nh_FlyingAgentClient.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
// Subscriber for locking the load the controller YAML
......@@ -186,7 +186,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
requestLoadControllerYaml_from_my_GUI_Subscriber = nodeHandle.subscribe("/my_GUI/requestLoadControllerYaml", 1, &MainWindow::requestLoadControllerYaml_from_my_GUI_Callback, this);
// First get student ID
if(!nh_PPSClient.getParam("agentID", m_student_id))
if(!nh_FlyingAgentClient.getParam("agentID", m_student_id))
{
ROS_ERROR("Failed to get agentID");
}
......@@ -966,21 +966,21 @@ void MainWindow::on_take_off_button_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_CRAZYFLY_TAKE_OFF;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
void MainWindow::on_land_button_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_CRAZYFLY_LAND;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
void MainWindow::on_motors_OFF_button_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_CRAZYFLY_MOTORS_OFF;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
......@@ -1487,49 +1487,49 @@ void MainWindow::on_enable_safe_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_SAFE_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
void MainWindow::on_enable_demo_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_DEMO_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
void MainWindow::on_enable_student_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_STUDENT_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
void MainWindow::on_enable_mpc_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_MPC_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
void MainWindow::on_enable_remote_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_REMOTE_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
void MainWindow::on_enable_tuning_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_TUNING_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
void MainWindow::on_enable_picker_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_PICKER_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
this->FlyingAgentClientCommandPublisher.publish(msg);
}
......
......@@ -191,7 +191,7 @@
"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}]\""
"repr": "u\"[{'type_name': 'std_msgs/Int32', 'topic_name': '/FlyingAgentClient/Command', 'enabled': False, 'rate': 1.0, 'expressions': {u'/FlyingAgentClient/Command/data': '3'}, 'publisher_id': 0, 'counter': 0}]\""
}
},
"groups": {}
......
......@@ -72,10 +72,9 @@ logging.basicConfig(level=logging.ERROR)
# CONTROLLER_ANGLE = 1
# CONTROLLER_RATE = 0
TYPE_PPS_MOTORS = 6
TYPE_PPS_RATE = 7
TYPE_PPS_ANGLE = 8
CF_COMMAND_TYPE_MOTORS = 6
CF_COMMAND_TYPE_RATE = 7
CF_COMMAND_TYPE_ANGLE = 8
RAD_TO_DEG = 57.296
......@@ -88,7 +87,7 @@ DISCONNECTED = 2
CMD_RECONNECT = 0
CMD_DISCONNECT = 1
# Commands for PPSClient
# Commands for FlyingAgentClient
#CMD_USE_SAFE_CONTROLLER = 1
#CMD_USE_DEMO_CONTROLLER = 2
#CMD_USE_STUDENT_CONTROLLER = 3
......@@ -106,7 +105,7 @@ rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
rospy.loginfo(record_file)
bag = rosbag.Bag(record_file, 'w')
class PPSRadioClient:
class CrazyRadioClient:
"""
CrazyRadio client that recieves the commands from the controller and
sends them in a CRTP package to the crazyflie with the specified
......@@ -126,7 +125,7 @@ class PPSRadioClient:
self.link_uri = ""
self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', IntWithHeader, queue_size=1)
self.FlyingAgentClient_command_pub = rospy.Publisher('FlyingAgentClient/Command', IntWithHeader, queue_size=1)
time.sleep(1.0)
# Initialize the CrazyFlie and add callbacks
......@@ -173,7 +172,7 @@ class PPSRadioClient:
msg = IntWithHeader()
msg.shouldCheckForID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
self.PPSClient_command_pub.publish(msg)
self.FlyingAgentClient_command_pub.publish(msg)
time.sleep(0.1)
print "[CRAZY RADIO] Disconnecting from %s" % self.link_uri
self._cf.close_link()
......@@ -233,7 +232,7 @@ class PPSRadioClient:
msg = IntWithHeader()
msg.shouldCheckForID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
cf_client.PPSClient_command_pub.publish(msg)
cf_client.FlyingAgentClient_command_pub.publish(msg)
rospy.loginfo("[CRAZY RADIO] Connection to %s successful: " % link_uri)
# Config for Logging
......@@ -263,7 +262,7 @@ class PPSRadioClient:
msg = IntWithHeader()
msg.shouldCheckForID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
self.PPSClient_command_pub.publish(msg)
self.FlyingAgentClient_command_pub.publish(msg)
self.logconf.stop()
rospy.loginfo("logconf stopped")
......@@ -273,19 +272,19 @@ class PPSRadioClient:
def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHH', TYPE_PPS_MOTORS, cmd1, cmd2, cmd3, cmd4)
pk.data = struct.pack('<BHHHH', CF_COMMAND_TYPE_MOTORS, cmd1, cmd2, cmd3, cmd4)
self._cf.send_packet(pk)
def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHHfff', TYPE_PPS_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate * RAD_TO_DEG, pitch_rate * RAD_TO_DEG, yaw_rate * RAD_TO_DEG)
pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate * RAD_TO_DEG, pitch_rate * RAD_TO_DEG, yaw_rate * RAD_TO_DEG)
self._cf.send_packet(pk)
def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BHHHHfff', TYPE_PPS_ANGLE, cmd1, cmd2, cmd3, cmd4, roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG)
pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2, cmd3, cmd4, roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG)
self._cf.send_packet(pk)
# def _send_to_commander(self,roll, pitch, yaw, thrust, cmd1, cmd2, cmd3, cmd4, mode):
......@@ -350,13 +349,13 @@ def controlCommandCallback(data):
#cmd1..4 must not be 0, as crazyflie onboard controller resets!
#pitch and yaw are inverted on crazyflie controller
if data.onboardControllerType == TYPE_PPS_MOTORS:
if data.onboardControllerType == CF_COMMAND_TYPE_MOTORS:
cf_client._send_to_commander_motor(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)
elif data.onboardControllerType == TYPE_PPS_RATE:
elif data.onboardControllerType == CF_COMMAND_TYPE_RATE:
cf_client._send_to_commander_rate(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
elif data.onboardControllerType == TYPE_PPS_ANGLE:
elif data.onboardControllerType == CF_COMMAND_TYPE_ANGLE:
cf_client._send_to_commander_angle(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
# cf_client._send_to_commander(data.roll, -data.pitch, -data.yaw, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
......@@ -380,7 +379,7 @@ 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
#wait until address parameter is set by FlyingAgentClient
while not rospy.has_param("~crazyFlieAddress"):
time.sleep(0.05)
......@@ -394,8 +393,8 @@ if __name__ == '__main__':
# Fetch the YAML paramter "agentID" and "coordID"
global m_agentID
m_agentID = rospy.get_param(ros_namespace + "/PPSClient/agentID")
coordID = rospy.get_param(ros_namespace + "/PPSClient/coordID")
m_agentID = rospy.get_param(ros_namespace + "/FlyingAgentClient/agentID")
coordID = rospy.get_param(ros_namespace + "/FlyingAgentClient/coordID")
# Convert the coordinator ID to a zero-padded string
coordID_as_string = format(coordID, '03')
......@@ -404,19 +403,19 @@ if __name__ == '__main__':
global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
# Initialise a "PPSRadioClient" type variable that handles
# Initialise a "CrazyRadioClient" type variable that handles
# all communication over the CrazyRadio
global cf_client
cf_client = PPSRadioClient()
cf_client = CrazyRadioClient()
print "[CRAZY RADIO] DEBUG 2"
# Subscribe to the commands for when to (dis-)connect the
# CrazyRadio connection with the Crazyflie