diff --git a/real_robot/coordination_formation_control_pkg/CMakeLists.txt b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
index eacdbb6c0a13c0bd6814c7c8c675757334906457..85257397996e2183d7ea01364ead6697c221e6df 100644
--- a/real_robot/coordination_formation_control_pkg/CMakeLists.txt
+++ b/real_robot/coordination_formation_control_pkg/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
dynamic_reconfigure
message_generation
+ rosbag
)
find_package(catkin)
@@ -32,16 +33,18 @@ add_message_files(
FILES
coordination.msg
waypoint.msg
+ logData.msg
)
generate_messages(
DEPENDENCIES
std_msgs
+ geometry_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES coordination_formation_control_pkg
- CATKIN_DEPENDS crazyswarm geometry_msgs roscpp dynamic_reconfigure roscpp crazyswarm message_runtime
+ CATKIN_DEPENDS crazyswarm geometry_msgs roscpp dynamic_reconfigure roscpp crazyswarm message_runtime rosbag
# DEPENDS system_lib
)
## Specify additional locations of header files
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index 3c5843c3bc8d207337a2319c0139b67dd64e2995..d9a7a3d6904517a740a1dfdb4c814a6acb63a140 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -4,6 +4,8 @@ hz_freq: 50 # controller frquency
include_obstacle: false
include_orientation: false
n_obs: 0
+log_name: "experiment_1"
+enable_log: false
# waypoint
# x: 0
diff --git a/real_robot/coordination_formation_control_pkg/msg/logData.msg b/real_robot/coordination_formation_control_pkg/msg/logData.msg
index cc85513eacb161d24ffe89ef9560d040be59afa7..71b98b6dc35ea651bd06c630995aadfe829b0016 100644
--- a/real_robot/coordination_formation_control_pkg/msg/logData.msg
+++ b/real_robot/coordination_formation_control_pkg/msg/logData.msg
@@ -1,6 +1,6 @@
-std_msgs/Header # time
-geometry_msgs/Pose[] pose
-geometry_msgs/Twist[] twist
+std_msgs/Header header # time
+geometry_msgs/Pose[] current_pose
+geometry_msgs/Twist[] current_twist
geometry_msgs/Pose2D command_pose
geometry_msgs/Twist command_twist
geometry_msgs/Pose2D input
diff --git a/real_robot/coordination_formation_control_pkg/package.xml b/real_robot/coordination_formation_control_pkg/package.xml
index 5791174e13faab074e5a468b29805c84530fa123..055559629935865823fec2dc329c7705483543cb 100644
--- a/real_robot/coordination_formation_control_pkg/package.xml
+++ b/real_robot/coordination_formation_control_pkg/package.xml
@@ -54,17 +54,20 @@
<build_depend>roscpp</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>message_generation</build_depend>
+ <build_depend>rosbag</build_depend>
<build_export_depend>crazyswarm</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>dynamic_reconfigure</build_export_depend>
+ <build_export_depend>rosbag</build_export_depend>
<exec_depend>crazyswarm</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>dynamic_reconfigure</exec_depend>
<exec_depend>message_runtime</exec_depend>
+ <exec_depend>rosbag</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
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 4adc2076d302d644d30d4e60505276d6d3d0fb87..5707b2d8809c5480d64d2d1a89887a8ed8426de0 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -6,6 +6,8 @@
#include "swamControllerAlg.h"
#include "coordination_formation_control_pkg/coordination.h"
#include "coordination_formation_control_pkg/waypoint.h"
+#include "coordination_formation_control_pkg/logData.h"
+#include <rosbag/bag.h>
#include "crazyswarm/Hover.h"
#include "crazyswarm/Land.h"
@@ -17,13 +19,16 @@
/* global variables*/
coordination_formation_control_pkg::coordination *coord_config;
coordination_formation_control_pkg::waypoint *next_waypoint;
+coordination_formation_control_pkg::logData *logdata;
+std::stringstream ss_log_naming;
+rosbag::Bag bag;
bool waypoint_received = false;
void configurationCallback(const coordination_formation_control_pkg::coordination::ConstPtr& msg );
void waypointCallback(const coordination_formation_control_pkg::waypoint::ConstPtr& msg);
-
+void logCallback(const ros::TimerEvent& event);
int main(int argc, char **argv) {
ros::init(argc, argv, "~");
int robotid = std::atoi(argv[1]);
@@ -48,6 +53,9 @@ int main(int argc, char **argv) {
coord_config->initialize_robot = false;
/* current waypoint*/
next_waypoint = new coordination_formation_control_pkg::waypoint();
+ logdata = new coordination_formation_control_pkg::logData();
+ logdata->current_pose.resize(n_drones);
+ logdata->current_pose.resize(n_drones);
/* current command*/
crazyswarm::Hover current_command;
current_command.vx = 0;
@@ -134,12 +142,27 @@ int main(int argc, char **argv) {
/* TODO publisher to publish results and rosbag it*/
+ ros::Timer writeTobagfileTimer = nh.createTimer(ros::Duration(1.0/10.0), logCallback);
+ std::string log_data_name;
+ bool enable_log;
+ nh.getParam("/enable_log", enable_log);
+ nh.getParam("/log_name", log_data_name);
+ ss_log_naming << log_data_name << "_crazyflie_" << std::to_string(robotid) << "_logdata.bag";
+
+ // bag_ptr = new rosbag::bag();
+
+ if(enable_log)
+ {
+ bag.open(log_data_name.c_str(),rosbag::bagmode::Write);
+ }
+
/*create a list of drones to subscribe to*/
std::vector<robotState *> uavs;
robot_ind_t uav_def;
bool sucess_initialization;
+ input_vector_t input;
for (int i = 1; i <= n_drones; i++)
{
uav_def.robot_name = "cf";
@@ -228,7 +251,7 @@ int main(int argc, char **argv) {
}
/* call controller*/
- input_vector_t input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
+ input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
/* integrate acceleration*/
Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0));
/* reached a certain waypoint and we want to stop sending command*/
@@ -283,6 +306,38 @@ int main(int argc, char **argv) {
break;
+ }
+
+ if(enable_log)
+ {
+ logdata->header.stamp = ros::Time::now();
+ logdata->header.frame_id = robot_ind.robot_name;
+ for(int i = 0; i <n_drones; i++)
+ {
+ logdata->current_pose[i].position.x = uav_state_q(0,i);
+ logdata->current_pose[i].position.y = uav_state_q(1,i);
+ logdata->current_twist[i].linear.x = uav_state_p(0,i);
+ logdata->current_twist[i].linear.y = uav_state_p(1,i);
+ }
+ logdata->command_pose.x = current_waypoint.q(0);
+ logdata->command_pose.y = current_waypoint.q(1);
+ logdata->command_pose.theta = current_waypoint.orientation;
+ logdata->command_twist.linear.x = current_waypoint.p(0);
+ logdata->command_twist.linear.y = current_waypoint.p(1);
+ logdata->input.x = input(0,0);
+ logdata->input.y = input(1,0);
+ logdata->input.x = input(0,1);
+ logdata->input_formation.y = input(1,1);
+ logdata->input_orientation.x = input(0,2);
+ logdata->input_orientation.y = input(1,2);
+ logdata->input_navigation.x = input(0,3);
+ logdata->input_navigation.y = input(1,3);
+ logdata->input_obstacle.x = input(0,4);
+ logdata->input_obstacle.y = input(1,4);
+ logdata->input_integration.x = input(0,5);
+ logdata->input_integration.y = input(1,5);
+
+
}
ros::spinOnce();
@@ -298,6 +353,10 @@ int main(int argc, char **argv) {
std_msgs::Empty stop_drone;
stop_robot_publisher.publish(stop_drone);
}
+ if(enable_log)
+ {
+ bag.close();
+ }
return 0;
}
@@ -316,3 +375,10 @@ void waypointCallback(const coordination_formation_control_pkg::waypoint::ConstP
waypoint_received = false;
*next_waypoint = *msg;
}
+
+void logCallback(const ros::TimerEvent& event)
+{
+ /* this can be an issue, concurrent read write can be a problem*/
+ bag.write(ss_log_naming.str(),logdata->header.stamp, *logdata);
+
+}
diff --git a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
index 5f5d1f4ca59ce274617745cd56b94109d832eb04..2b935494d60c52587bb2eabc883506965c9d824f 100644
--- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
@@ -2,18 +2,25 @@
#include "geometry_msgs/Twist.h"
#include "coordination_formation_control_pkg/coordination.h"
#include "coordination_formation_control_pkg/waypoint.h"
+#include "coordination_formation_control_pkg/logData.h"
+
#include "robot_state.h"
#include "shared_definitions.h"
#include "swamControllerAlg.h"
+#include <rosbag/bag.h>
/* global variables*/
coordination_formation_control_pkg::coordination *coord_config;
coordination_formation_control_pkg::waypoint *next_waypoint;
+coordination_formation_control_pkg::logData *logdata;
+std::stringstream ss_log_naming;
+rosbag::Bag bag;
bool waypoint_received = false;
void configurationCallback(const coordination_formation_control_pkg::coordination::ConstPtr& msg );
void waypointCallback(const coordination_formation_control_pkg::waypoint::ConstPtr& msg);
+void logCallback(const ros::TimerEvent& event);
@@ -38,7 +45,9 @@ int main(int argc, char **argv) {
coord_config->initialize_robot = false;
/* current waypoint*/
next_waypoint = new coordination_formation_control_pkg::waypoint();
-
+ logdata = new coordination_formation_control_pkg::logData();
+ logdata->current_pose.resize(n_ugvs);
+ logdata->current_pose.resize(n_ugvs);
/*current command*/
geometry_msgs::Twist current_command;
current_command.linear.x = 0;
@@ -104,6 +113,20 @@ int main(int argc, char **argv) {
ss << "/omniwheel" << std::to_string(robotid) << "/vel_cmd";
ros::Publisher vel_cmd_publisher = nh.advertise<geometry_msgs::Twist>(ss.str(),1);
+ /* TODO publisher to publish results and rosbag it*/
+ ros::Timer writeTobagfileTimer = nh.createTimer(ros::Duration(1.0/10.0), logCallback);
+ std::string log_data_name;
+ bool enable_log;
+ nh.getParam("/enable_log", enable_log);
+ nh.getParam("/log_name", log_data_name);
+ if(enable_log)
+ {
+ bag.open(log_data_name.c_str(),rosbag::bagmode::Write);
+ }
+ ss_log_naming << log_data_name << "_omniwheel_" << std::to_string(robotid) << "_logdata.bag";
+
+
+
std::vector<robotState *> ugvs;
robot_ind_t ugv_def;
bool sucess_initialization;
@@ -123,6 +146,8 @@ int main(int argc, char **argv) {
}
}
+ input_vector_t input;
+
ros::Rate loop_rate(hz_freq);
@@ -212,7 +237,7 @@ int main(int argc, char **argv) {
}
//
/* call controller*/
- input_vector_t input = swam_controller.controller(current_state_q, current_state_p,current_waypoint,current_obs);
+ input = swam_controller.controller(current_state_q, current_state_p,current_waypoint,current_obs);
/* integrate acceleration*/
if(next_waypoint->stop)
{
@@ -246,10 +271,47 @@ int main(int argc, char **argv) {
}
+ if(enable_log)
+ {
+ logdata->header.stamp = ros::Time::now();
+ logdata->header.frame_id = robot_ind.robot_name;
+ for(int i = 0; i <n_ugvs; i++)
+ {
+ logdata->current_pose[i].position.x = ugv_state_q(0,i);
+ logdata->current_pose[i].position.y = ugv_state_q(1,i);
+ logdata->current_twist[i].linear.x = ugv_state_p(0,i);
+ logdata->current_twist[i].linear.y = ugv_state_p(1,i);
+ }
+ logdata->command_pose.x = current_waypoint.q(0);
+ logdata->command_pose.y = current_waypoint.q(1);
+ logdata->command_pose.theta = current_waypoint.orientation;
+ logdata->command_twist.linear.x = current_waypoint.p(0);
+ logdata->command_twist.linear.y = current_waypoint.p(1);
+ logdata->input.x = input(0,0);
+ logdata->input.y = input(1,0);
+ logdata->input.x = input(0,1);
+ logdata->input_formation.y = input(1,1);
+ logdata->input_orientation.x = input(0,2);
+ logdata->input_orientation.y = input(1,2);
+ logdata->input_navigation.x = input(0,3);
+ logdata->input_navigation.y = input(1,3);
+ logdata->input_obstacle.x = input(0,4);
+ logdata->input_obstacle.y = input(1,4);
+ logdata->input_integration.x = input(0,5);
+ logdata->input_integration.y = input(1,5);
+
+
+ }
+
+
ros::spinOnce();
loop_rate.sleep();
}
+ if(enable_log)
+ {
+ bag.close();
+ }
return 0;
}
@@ -267,3 +329,10 @@ void waypointCallback(const coordination_formation_control_pkg::waypoint::ConstP
waypoint_received = false;
*next_waypoint = *msg;
}
+
+void logCallback(const ros::TimerEvent& event)
+{
+ /* this can be an issue, concurrent read write can be a problem*/
+ bag.write(ss_log_naming.str(),logdata->header.stamp, *logdata);
+
+}