diff --git a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
index b0351bc64287ef0936897141ae92e27f3703ff35..8402bf54cac54e8ee2421fcf6b7c1809712bee1f 100644
--- a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
@@ -3,15 +3,15 @@ crazyflies:
channel: 90
initialPosition: [0.0, 0.0, 0.0]
type: default
-- id: 2
- channel: 90
- initialPosition: [0.0, 0.0, 0.0]
- type: default
-- id: 3
- channel: 80
- initialPosition: [0.0, 0.0, 0.0]
- type: default
-- id: 4
- channel: 80
- initialPosition: [0.0, 0.0, 0.0]
- type: default
+# - id: 2
+# channel: 90
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
+# - id: 3
+# channel: 80
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
+# - id: 4
+# channel: 80
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index 8a262b5c31df9a34281fce532bc5e8bd51e1ec93..33751a56556ff3110b4542e5f1f4917426756dc9 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -1,5 +1,5 @@
-n_drones: 2
-n_ugvs: 1
+n_drones: 1
+n_ugvs: 0
hz_freq: 50 # controller frquency
include_obstacle: false
include_orientation: true
diff --git a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
index 4527b06c1b028a40002456e372e3f46840edd459..3b1088be723146b205343e41ab724faff44fae2f 100644
--- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
@@ -4,16 +4,19 @@
<rosparam command="load" file="$(find coordination_formation_control_pkg)/config/missionConfig.yaml" />
<rosparam command="load" file="$(find coordination_formation_control_pkg)/config/formationConfig.yaml" />
+ <node name="mission_controller" pkg="coordination_formation_control_pkg" type="mission_controller" respawn="false"
+ output="screen"/>
+
<!-- Initialize all the drones first -->
<group ns="uav_1">
- <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="1" launch-prefix="xterm -e gdb --args">
+ <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="1" launch-prefix="xterm -e gdb --args">
</node>
</group>
- <group ns="uav_2">
- <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="2" launch-prefix="xterm -e gdb --args">
+ <!-- <group ns="uav_2">
+ <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="2" launch-prefix="xterm -e gdb ">
</node>
- </group>
+ </group> -->
<!--
<group ns="uav_3">
<node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="3" launch-prefix="xterm -e">
@@ -30,13 +33,12 @@
- <node name="mission_controller" pkg="coordination_formation_control_pkg" type="mission_controller" respawn="false"
- output="screen"/>
+<!--
<group ns="omniwheel_1">
- <node name="omniwheel_node" pkg="coordination_formation_control_pkg" type="omniwheel_node" output="screen" args="1" launch-prefix="xterm -e gdb --args">
+ <node name="omniwheel_node" pkg="coordination_formation_control_pkg" type="omniwheel_node" output="screen" args="1" launch-prefix="xterm -e gdb ">
</node>
- </group>
+ </group> -->
<!-- launch dynamic reconfigure -->
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_2_crazyflie_1_logdata.bag
index 14a63027a907094e84930280178541d44831e739..8c354463c59f8bc0190ad41612ebfdb5dadf7fe4 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_2_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_2_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
index 0bac116c3d89931368a2fe901f098c583f02da0d..a4fd07197a8257c77cc0229d39e453c6855a40ca 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -210,7 +210,7 @@ int main(int argc, char **argv) {
else if(coord_config->initialize_robot && coord_config->init_formation && coord_config->stop_demo == false)
{
/* start formation controll*/
- if (waypoint_received)
+ if (false)//waypoint_received)
{
/*get current state of all the robot*/
@@ -285,6 +285,22 @@ int main(int argc, char **argv) {
}
+ /* testing velocity command*/
+ robot_state_t current_state_;
+ float vx = 0.05;
+ float vy = 0;
+ current_state_ = uavs[robotid-1]->get_state();
+ float yaw = current_state_.q(3)*M_PI/180.0; // TODO confirm that indeed it is in degrees
+ float vxl = vx*std::cos(yaw) + 0*std::sin(yaw) ;
+ float vyl = -vx*std::sin(yaw) + 0*std::cos(yaw) ;
+
+ current_command.vx = vxl;
+ current_command.vy = vyl;
+ // std::cout << "vx: normal: " << vx << "vy normal: " << vy << std::endl;
+ // std::cout << "***********************************" << std::endl;
+ // std::cout << "vy: transformed: " << vxl << "vy transformed: " << vyl << std::endl;
+
+
vel_cmd_publisher.publish(current_command);
}
diff --git a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
index 513df262263ec7da7f372bc8e3740a370955e6b6..989f71e64430a3f1b215b60b84874bba84b340a7 100644
--- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
@@ -210,30 +210,30 @@ for (int i = 0; i ++; i < n_ugvs)
break;
case START_FORMATION:
/* testint*/
- uav_state_q(0, 1-1) =0;
- uav_state_q(1, 1-1) =0;
- uav_state_p(0, 1-1) =0;
- uav_state_p(1, 1-1) = 0;
-
- uav_state_q(0, 2-1) =1;
- uav_state_q(1, 2-1) =0;
- uav_state_p(0, 2-1) = 0;
- uav_state_p(1, 2-1) = 0;
-
- // uav_state_q(0, 3-1) =0;
- // uav_state_q(1, 3-1) =1;
- // uav_state_p(0, 3-1) = 0;
- // uav_state_p(1, 3-1) = 0;
-
- // uav_state_q(0, 4-1) =1;
- // uav_state_q(1, 4-1) =1;
- // uav_state_p(0, 4-1) = 0;
- // uav_state_p(1, 4-1) = 0;
+ // uav_state_q(0, 1-1) =0;
+ // uav_state_q(1, 1-1) =0;
+ // uav_state_p(0, 1-1) =0;
+ // uav_state_p(1, 1-1) = 0;
//
- ugv_state_q(0, 1-1) =-1.5;
- ugv_state_q(1, 1-1) =-0.5;
- ugv_state_p(0, 1-1) = 0;
- ugv_state_p(1, 1-1) =0;
+ // uav_state_q(0, 2-1) =1;
+ // uav_state_q(1, 2-1) =0;
+ // uav_state_p(0, 2-1) = 0;
+ // uav_state_p(1, 2-1) = 0;
+ //
+ // // uav_state_q(0, 3-1) =0;
+ // // uav_state_q(1, 3-1) =1;
+ // // uav_state_p(0, 3-1) = 0;
+ // // uav_state_p(1, 3-1) = 0;
+ //
+ // // uav_state_q(0, 4-1) =1;
+ // // uav_state_q(1, 4-1) =1;
+ // // uav_state_p(0, 4-1) = 0;
+ // // uav_state_p(1, 4-1) = 0;
+ // //
+ // ugv_state_q(0, 1-1) =-1.5;
+ // ugv_state_q(1, 1-1) =-0.5;
+ // ugv_state_p(0, 1-1) = 0;
+ // ugv_state_p(1, 1-1) =0;
// ugv_state_q(0, 2-1) =1.5;
// ugv_state_q(1, 2-1) =-0.5;