working velocity controller drone and ugv, test swam controller

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

real_robot/coordination_formation_control_pkg/config/missionConfig.yaml

-n_drones: 2
-n_ugvs: 1
+n_drones: 1
+n_ugvs: 0
 hz_freq: 50 # controller frquency
 include_obstacle: false
 include_orientation: true

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>
    </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 -->

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);
 
     }

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;