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 4f9550bdef79435e642a205e2b07e1d1bfc62f14..1e3ce92c63244f99d36ffd7a68002858eed1f506 100644
--- a/real_robot/coordination_formation_control_pkg/include/robot_state.h
+++ b/real_robot/coordination_formation_control_pkg/include/robot_state.h
@@ -5,6 +5,7 @@
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose.h"
+#include "geometry_msgs/Vector3.h"
#include "crazyswarm/GenericLogData.h"
#include "robot_state.h"
#include "tf/transform_datatypes.h"
@@ -21,6 +22,7 @@ private:
robot_type_t t;
ros::Subscriber state_sub_pose;
ros::Subscriber state_sub_vel;
+ ros::Subscriber state_sub_acc;
ros::NodeHandle nh;
/* not an effective way*/
@@ -34,8 +36,10 @@ private:
/* not effective, there should be synchrnied*/
void stateCallbackUgvVel(const geometry_msgs::TwistConstPtr &vel_msg);
void stateCallbackUgvPose(const geometry_msgs::PoseConstPtr &pose_msg);
+ void stateCallbackUgvAcc(const geometry_msgs::Vector3ConstPtr &acc_msg);
void stateCallbackUavVel(const crazyswarm::GenericLogDataConstPtr &vel_msg);
void stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &pose_msg);
+ void stateCallbackUavAcc(const crazyswarm::GenericLogDataConstPtr &Acc_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 bb0c98914b691fdc195a4fd3f0ab177d0770e640..ab0bf2dc3c8e89c636052a0c0ba6e702e23bbfa0 100644
--- a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
+++ b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
@@ -20,6 +20,7 @@ typedef struct robot_state
{
Vector4d q; /*position 3 position 1 orientation yaw*/
Vector3d p; /* velocity*/
+ Vector3d a; /* acceleration*/
}robot_state_t;
/* indentify the robot*/
@@ -99,7 +100,7 @@ typedef struct formation_config
-typedef Eigen::Matrix<double, 2,5> input_vector_t;
+typedef Eigen::Matrix<double, 2,6> input_vector_t;
/* input vector conatains the following
* totall input x y
* ofr debugging I print the remaining input
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 0c023f14e47def5e6a92c81607a363bd4038292c..15a1ad9b5a54c8af2358f21227bb27e4762da3e9 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,10 +9,11 @@
<node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
<rosparam>
# Logging configuration (Use enable_logging to actually enable logging)
- genericLogTopics: ["position", "velocity"]
- genericLogTopicFrequencies: [10,10]
+ genericLogTopics: ["position", "velocity","acceleration"]
+ genericLogTopicFrequencies: [10,10,10]
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"]
# 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
new file mode 100644
index 0000000000000000000000000000000000000000..cc85513eacb161d24ffe89ef9560d040be59afa7
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/msg/logData.msg
@@ -0,0 +1,11 @@
+std_msgs/Header # time
+geometry_msgs/Pose[] pose
+geometry_msgs/Twist[] twist
+geometry_msgs/Pose2D command_pose
+geometry_msgs/Twist command_twist
+geometry_msgs/Pose2D input
+geometry_msgs/Vector3 input_formation
+geometry_msgs/Vector3 input_orientation
+geometry_msgs/Vector3 input_navigation
+geometry_msgs/Vector3 input_obstacle
+geometry_msgs/Vector3 input_integration
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 4f33c59846c5eb1b48447a68a16bfad95cd0479b..ce0145cbaf507b9811ed88cd68adc042c6679c66 100644
--- a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
@@ -11,17 +11,24 @@ t(robot_ind.t)
{
std::stringstream ss_pose;
std::stringstream ss_vel;
+ std::stringstream ss_acc;
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";
+
// 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);
+
ROS_INFO("Robot %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));
+
sucess_initialization = true;
@@ -30,13 +37,18 @@ t(robot_ind.t)
{
ss_pose << "/" << robot_name << std::to_string(this->id) << "/pose";
ss_vel << "/" << robot_name << std::to_string(this->id) << "/vel";
+ ss_acc << "/" << robot_name << std::to_string(this->id) << "/acc";
+
// ugv_sync.registerCallback( boost::bind( &robotState::stateCallbackUgv, this, _1, _2 ) );
this->state_sub_pose = nh.subscribe(ss_pose.str(), 1,&robotState::stateCallbackUgvPose, this);
this->state_sub_vel = nh.subscribe(ss_vel.str(), 1,&robotState::stateCallbackUgvVel, this);
+ this->state_sub_acc = nh.subscribe(ss_acc.str(), 1,&robotState::stateCallbackUgvAcc, this);
ROS_INFO("Robot %d state subcribed", this->id );
ros::topic::waitForMessage<geometry_msgs::Twist>(ss_vel.str(), ros::Duration(5));
ros::topic::waitForMessage<geometry_msgs::Pose>(ss_pose.str(), ros::Duration(5));
+ ros::topic::waitForMessage<geometry_msgs::Vector3>(ss_acc.str(), ros::Duration(5));
+
sucess_initialization = true;
}
@@ -89,3 +101,19 @@ void robotState::stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &p
this->state.q(3) = pose_msg->values[3]; // degres
}
+
+void robotState::stateCallbackUgvAcc(const geometry_msgs::Vector3ConstPtr &acc_msg)
+{
+
+ this->state.a(0) = acc_msg->x;
+ this->state.a(1) = acc_msg->y;
+ this->state.a(2) = acc_msg->z;
+
+}
+
+void robotState::stateCallbackUavAcc(const crazyswarm::GenericLogDataConstPtr &Acc_msg)
+{
+ this->state.a(0) = Acc_msg->values[0];
+ this->state.a(1) = Acc_msg->values[1];
+ this->state.a(2) = Acc_msg->values[2];
+}