diff --git a/real_robot/coordination_formation_control_pkg/include/robot_state.h b/real_robot/coordination_formation_control_pkg/include/robot_state.h index 1e3ce92c63244f99d36ffd7a68002858eed1f506..1ebaf40383a5454fe27b9580a05a9eeddc2c3165 100644 --- a/real_robot/coordination_formation_control_pkg/include/robot_state.h +++ b/real_robot/coordination_formation_control_pkg/include/robot_state.h @@ -23,6 +23,7 @@ private: ros::Subscriber state_sub_pose; ros::Subscriber state_sub_vel; ros::Subscriber state_sub_acc; + ros::Subscriber state_sub_ang; ros::NodeHandle nh; /* not an effective way*/ @@ -40,6 +41,7 @@ private: void stateCallbackUavVel(const crazyswarm::GenericLogDataConstPtr &vel_msg); void stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &pose_msg); void stateCallbackUavAcc(const crazyswarm::GenericLogDataConstPtr &Acc_msg); + void stateCallbackUavAng(const crazyswarm::GenericLogDataConstPtr &Ang_msg); public: diff --git a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h index f5d7f3b94025911ce47fc8e194d6107a4c340edd..bf3ae2ade49c36ac4d3fb34974769be8ba4bb146 100644 --- a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h +++ b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h @@ -28,6 +28,7 @@ typedef enum formation_matrix typedef struct robot_state { Vector4d q; /*position 3 position 1 orientation yaw*/ + Vector3d o; /* orientation */ Vector3d p; /* velocity*/ Vector3d a; /* acceleration*/ }robot_state_t; diff --git a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch index 279a0cb28bc17f0d2be76804b7179efa6baf8de2..f5bfd85f40f78dcbf0b44013a5b6791dc0f18d30 100644 --- a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch +++ b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch @@ -9,11 +9,12 @@ <node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" > <rosparam> # Logging configuration (Use enable_logging to actually enable logging) - genericLogTopics: ["position", "velocity","acceleration"] + genericLogTopics: ["position", "velocity","acceleration", "angle"] genericLogTopicFrequencies: [50,50,50] genericLogTopic_position_Variables: ["stateEstimateZ.x", "stateEstimateZ.y","stateEstimateZ.z", "stateEstimate.yaw"] genericLogTopic_velocity_Variables: ["stateEstimateZ.vx", "stateEstimateZ.vy","stateEstimateZ.vz"] genericLogTopic_acceleration_Variables: ["stateEstimateZ.ax", "stateEstimateZ.ay","stateEstimateZ.az"] + genericLogTopic_orientation_Variables: [ "stateEstimate.roll", "stateEstimate.pitch", "stateEstimate.yaw"] # firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or # allCrazyflies.yaml to set per drone) firmwareParams: diff --git a/real_robot/coordination_formation_control_pkg/msg/logData.msg b/real_robot/coordination_formation_control_pkg/msg/logData.msg index 9816f0758f69bcdece5b74d3229f16f1438ea873..d579484745da76df1edf7cc2a5a54f4940e2b3d1 100644 --- a/real_robot/coordination_formation_control_pkg/msg/logData.msg +++ b/real_robot/coordination_formation_control_pkg/msg/logData.msg @@ -11,3 +11,4 @@ geometry_msgs/Vector3 input_navigation geometry_msgs/Vector3 input_obstacle geometry_msgs/Vector3 input_integration geometry_msgs/Vector3 input_to_system +geometry_msgs/Vector3 angle diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py index 65f3a9c7a69b18bfdd83aa1063e6e05f6f91b35c..6f8aab6d82e502d8c9a880fd4f6ef8475e496f3b 100644 --- a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py +++ b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py @@ -21,6 +21,7 @@ t_ = [] pose_data = [] vel_data = [] acc_data = [] +ang_data = [] command_pose_data = [] command_twist_data = [] input_data = [] @@ -34,6 +35,7 @@ input_to_system_data = [] pose_row_ = [0] * n_robots_logged*2; twist_row_ = [0] * n_robots_logged*2; acc_row_ = [0] * n_robots_logged*2; +# ang_row_ = [0] * n_robots_logged*3; # converting data to numpy for easy plotting @@ -49,6 +51,10 @@ for topics, msg, t in bag.read_messages(topics =[topic_name]): twist_row_[ind*2+1] = msg.current_twist[ind].linear.y acc_row_[ind*2] = msg.current_acc[ind].linear.x acc_row_[ind*2+1] = msg.current_acc[ind].linear.y + # ang_row_[ind*2] = msg.angle[ind].x + # ang_row_[ind*2+1] = msg.angle[ind].y + # ang_row_[ind*2+2] = msg.angle[ind].z + # print(pose_row_) # print(pose_row_) t_.append(msg.header.stamp.secs + msg.header.stamp.nsecs*1e-9); @@ -58,6 +64,7 @@ for topics, msg, t in bag.read_messages(topics =[topic_name]): # time.sleep(1); vel_data.append(twist_row_[:]); acc_data.append(acc_row_[:]); + ang_data.append([msg.angle.x, msg.angle.y]); command_pose_data.append([msg.command_pose.x, msg.command_pose.y, msg.command_pose.theta]); command_twist_data.append([msg.command_twist.linear.x, msg.command_twist.linear.y]); input_data.append([msg.input.x, msg.input.y]) @@ -79,6 +86,8 @@ pose_data_np = np.array(pose_data); # print(pose_data_np) vel_data_np = np.array(vel_data); acc_data_np = np.array(acc_data); +ang_data_np = np.array(ang_data); + x_list = range(0,n_robots_logged*2,2); y_list = range(1,n_robots_logged*2,2); x_list_np = np.array(pose_data_np[:,x_list]); @@ -281,3 +290,23 @@ axs6[1, 0].set_ylabel("v(m/s)") axs6[1, 0].set_xlabel("time(s)") axs6[1, 0].legend(robot_names) plt.show() + +####################### roll vs pitch ######################################## +# Start plotting +####################### command vs centroid ################################### +fig1, axs1 = plt.subplots(nrows=2, ncols=1, figsize=(7, 7)) +st = fig1.suptitle("Roll vs pitch command sent to drone and current value", fontsize="x-large") + +# vx +axs1[0, 0].plot(t_np, command_pose_np[:,0]) +axs1[0, 0].plot(t_np, angle[:,0]) +axs1[0, 0].set_ylabel("roll(m)") +axs1[0, 0].set_xlabel("time(s)") +axs1[0, 0].legend(["command", "state"]) + + +axs1[0, 1].plot(t_np, command_pose_np[:,1]) +axs1[0, 1].plot(t_np, angle[:,1]) +axs1[0, 1].set_ylabel("pitch(m)") +axs1[0, 1].set_xlabel("time(s)") +axs1[0, 1].legend(["command", "state"]) 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 d651e99bb6f4d20a008041de92d673269a9e91a6..3b9ab7a4869b251753b4504269371aa40778a648 100644 --- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp +++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp @@ -91,6 +91,7 @@ int main(int argc, char **argv) { logdata->current_pose.resize(n_drones); logdata->current_twist.resize(n_drones); logdata->current_acc.resize(n_drones); + // logdata->angle.resize(n_drones); /* current command*/ crazyswarm::ZDistance current_command; @@ -104,6 +105,7 @@ int main(int argc, char **argv) { Eigen::MatrixXf uav_state_q(2,n_drones); Eigen::MatrixXf uav_state_p(2,n_drones); Eigen::MatrixXf uav_state_a(2,n_drones); + Eigen::MatrixXf uav_state_o(3,n_drones); // obstacle_t *current_obs = new obstacle_t[n_obs](); std::vector<obstacle_t> current_obs; @@ -233,6 +235,7 @@ int main(int argc, char **argv) { ros::Rate loop_rate(hz_freq); + float input_to_system_x = 0.0, input_to_system_y = 0.0; /* main loop*/ @@ -251,6 +254,10 @@ int main(int argc, char **argv) { uav_state_p(1, i-1) = current_state.p(1); uav_state_a(0, i-1) = current_state.a(0); uav_state_a(1, i-1) = current_state.a(1); + uav_state_a(0, i-1) = current_state.o(0); + uav_state_a(1, i-1) = current_state.o(1); + uav_state_a(2, i-1) = current_state.o(2); + } // std::cout << uav_state_q << std::endl; if(coord_config->initialize_robot && coord_config->init_formation ==false && coord_config->stop_demo == false) @@ -343,8 +350,11 @@ int main(int argc, char **argv) { // we are saturating the acceleration range << -2, 2; row_vector_2d_t out = saturate(in, range); - current_command.roll = -out(1)*180.0/(mass*M_PI); // conver it to the desired angle - current_command.pitch = out(0)*180.0/(mass*M_PI); + input_to_system_x = -out(1)*180.0/(mass*M_PI); + input_to_system_y = out(0)*180.0/(mass*M_PI); + // current_command.roll = input_to_system_x; // conver it to the desired angle + // current_command.pitch =input_to_system_y; + // vxx = vxl; // vyy = vyl; } @@ -409,12 +419,17 @@ int main(int argc, char **argv) { { logdata->current_pose[i].position.x = uav_state_q(0,i); logdata->current_pose[i].position.y = uav_state_q(1,i); + + // std::cout <<(uav_state_q(1,i)) << std::endl; logdata->current_twist[i].linear.x = uav_state_p(0,i); logdata->current_twist[i].linear.y = uav_state_p(1,i); logdata->current_acc[i].linear.x = uav_state_a(0,i); logdata->current_acc[i].linear.y = uav_state_a(1,i); } + logdata->angle.x = uav_state_o(0,robotid-1); + logdata->angle.y = uav_state_o(1,robotid-1); + logdata->angle.z = uav_state_o(2,robotid-1); logdata->command_pose.x = current_waypoint.q(0); logdata->command_pose.y = current_waypoint.q(1); logdata->command_pose.theta = current_waypoint.orientation; @@ -432,8 +447,8 @@ int main(int argc, char **argv) { logdata->input_orientation.y = input(1,4); logdata->input_integration.x = input(0,5); logdata->input_integration.y = input(1,5); - logdata->input_to_system.x = current_command.roll; - logdata->input_to_system.y = current_command.pitch; + logdata->input_to_system.x = input_to_system_x; + logdata->input_to_system.y =input_to_system_y; diff --git a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp index 2b8d78300e2c6b6146dbf8e38aaf6496270e5508..936d9d23bbadeebe670b8babfd5d46b0b30fd3a4 100644 --- a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp +++ b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp @@ -12,22 +12,29 @@ t(robot_ind.t) std::stringstream ss_pose; std::stringstream ss_vel; std::stringstream ss_acc; + std::stringstream ss_ang; if (this->t == CRAZYFLIE) { ss_pose << "/" << robot_name << std::to_string(this->id) << "/position"; ss_vel << "/" << robot_name << std::to_string(this->id) << "/velocity"; ss_acc << "/" << robot_name << std::to_string(this->id) << "/acceleration"; + ss_ang << "/" << robot_name << std::to_string(this->id) << "/angle"; // uav_sync.registerCallback( boost::bind( &robotState::stateCallbackUav, this, _1, _2 ) ); this->state_sub_pose = nh.subscribe(ss_pose.str(), 1,&robotState::stateCallbackUavPos, this); this->state_sub_vel = nh.subscribe(ss_vel.str(), 1,&robotState::stateCallbackUavVel, this); this->state_sub_acc = nh.subscribe(ss_acc.str(), 1,&robotState::stateCallbackUavAcc, this); + this->state_sub_ang = nh.subscribe(ss_ang.str(), 1,&robotState::stateCallbackUavAng, this); ROS_INFO("Crazyflie %d state subcribed", this->id ); ros::topic::waitForMessage< crazyswarm::GenericLogData >(ss_vel.str(), ros::Duration(5)); ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_pose.str(), ros::Duration(5)); ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_acc.str(), ros::Duration(5)); + ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_ang.str(), ros::Duration(5)); + + + sucess_initialization = true; @@ -86,6 +93,10 @@ void robotState::stateCallbackUgvPose(const geometry_msgs::PoseConstPtr &pose_ms tf::Matrix3x3(quat).getRPY(roll, pitch,yaw); this->state.q(3) = yaw*180.0/M_PI; // degres to mantain consistency + this->state.o(0) = 0;//yaw*180.0/M_PI; // degres to mantain consistency + this->state.o(1) = 0; // yaw*180.0/M_PI; // degres to mantain consistency + this->state.o(2) = yaw*180.0/M_PI; // yaw*180.0/M_PI; // degres to mantain consistency + } void robotState::stateCallbackUavVel(const crazyswarm::GenericLogDataConstPtr &vel_msg) { @@ -102,6 +113,15 @@ void robotState::stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &p } +void robotState::stateCallbackUavAng(const crazyswarm::GenericLogDataConstPtr &ang_msg) +{ + // all in degrees + this->state.o(0) = ang_msg->values[0]; + this->state.o(1) = ang_msg->values[1]; + this->state.o(2) = ang_msg->values[2]; + +} + void robotState::stateCallbackUgvAcc(const geometry_msgs::Vector3ConstPtr &acc_msg) {