diff --git a/scripts/crazyflie_logger_node.py b/scripts/crazyflie_logger_node.py index 68da3972511577e81b78fd4e5aa706be47336ca4..affb01890d98640f92cc0286e342cd9c8fa6a181 100755 --- a/scripts/crazyflie_logger_node.py +++ b/scripts/crazyflie_logger_node.py @@ -21,7 +21,7 @@ from std_msgs.msg import String from omniwheels_controller.msg import motorCommand from geometry_msgs.msg import Twist from geometry_msgs.msg import Pose - +from geometry_msgs.msg import Vector3 uri = uri_helper.uri_from_env(default='usb://0') # Connection-uri for crazyflie via USB @@ -57,9 +57,12 @@ class ParameterLogging: # ros stuff self._pose_data_ = Pose() self._vel_data_ = Twist() + self._acc_data_ = Vector3() # create publishers self._position_pub = rospy.Publisher("pose", Pose,queue_size=100) self._velocity_pub = rospy.Publisher("vel", Twist,queue_size=100) + self._acceleration_pub = rospy.Publisher("acc", Vector3,queue_size=100) + robot_name = rospy.get_param("/name") self._position_offset = rospy.get_param("/position_offset") # @@ -99,24 +102,34 @@ class ParameterLogging: self._lg_est_vel.add_variable('stateEstimateZ.vy') self._lg_est_vel.add_variable('stateEstimateZ.rateYaw') + print("velocity configuraion finished") + + self._lg_est_acc = LogConfig(name='formation_test_acceleration', period_in_ms=self.period_in_ms) + self._lg_est_acc.add_variable('stateEstimateZ.ax') + self._lg_est_acc.add_variable('stateEstimateZ.ay') + self._lg_est_acc.add_variable('stateEstimateZ.az') + print("velocity configuraion finished") try: self._cf.log.add_config(self._lg_stab) self._cf.log.add_config(self._lg_est_vel) + self._cf.log.add_config(self._lg_est_acc) # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._pos_log_data) self._lg_est_vel.data_received_cb.add_callback(self._vel_log_data) - + self._lg_est_acc.data_received_cb.add_callback(self._acc_log_data) + # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._log_error) self._lg_est_vel.error_cb.add_callback(self._log_error) - + self._lg_est_acc.error_cb.add_callback(self._log_error) + # Start the logging self._lg_stab.start() self._lg_est_vel.start() - + self._lg_est_acc.start() print("Completed configuration") except KeyError as e: print('Could not start log configuration,' @@ -219,7 +232,12 @@ class ParameterLogging: self._vel_data_.linear.y = data['stateEstimateZ.vy']/1000.0 self._vel_data_.angular.z = data['stateEstimateZ.rateYaw']/1000.0 self._velocity_pub.publish(self._vel_data_) - + + def _acc_log_data(self, timestamp,data, logconf): + self._acc_data_.x = data['stateEstimateZ.ax']/1000.0 + self._acc_data_.y = data['stateEstimateZ.ay']/1000.0 + self._acc_data_.z = data['stateEstimateZ.az']/1000.0 + self._acceleration_pub.publish(self._acc_data_) def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie diff --git a/utils/configure_ros_master.bash b/utils/configure_ros_master.bash index e8838a3de655ac8254c2ebf5d208462df467d0c2..0e7012f7e9b8e74240204dee4b32e6b256f33803 100644 --- a/utils/configure_ros_master.bash +++ b/utils/configure_ros_master.bash @@ -1,2 +1,2 @@ -export ROS_MASTER_URI=http://192.168.1.3:11311 +export ROS_MASTER_URI=http://192.168.1.4:11311 # this configures the ros master and makes the main compute the ros master