Diff: FreeFlyerHardware.cpp

Revision:

7:794bd40830c1

Parent:

5:864709d3eb76

--- a/FreeFlyerHardware.cpp	Mon Jul 02 23:38:21 2018 +0000
+++ b/FreeFlyerHardware.cpp	Tue Mar 02 22:49:24 2021 +0000
@@ -45,21 +45,29 @@
     pid_param_msg_.layout.data_offset = 0;
     pid_param_msg_.data_length = 4;
     pid_param_msg_.data = (float *) malloc(sizeof(float) * 4);
+   // 
+//    thruster_pwm_msg_.layout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension));
+//    thruster_pwm_msg_.layout.dim[0].size = 8;
+//    thruster_pwm_msg_.layout.dim[0].stride = 1*8;
+//    thruster_pwm_msg_.layout.data_offset = 0;
+//    thruster_pwm_msg_.data_length = 8;
+//    thruster_pwm_msg_.data = (float *) malloc(sizeof(float) * 8);
     
-    velocity_sns_pub_ = new ros::Publisher("wheel/sensors/velocity", &velocity_sns_msg_);
-    setpoint_pub_ = new ros::Publisher("wheel/setpoint", &setpoint_msg_);
-    acc_error_pub_ = new ros::Publisher("wheel/acc_error", &acc_error_msg_);
-    pid_param_pub_ = new ros::Publisher("wheel/pid_params", &pid_param_msg_);
+    velocity_sns_pub_ = new ros::Publisher("/mbed/wheel/sensors/velocity", &velocity_sns_msg_);
+    setpoint_pub_ = new ros::Publisher("/mbed/wheel/setpoint", &setpoint_msg_);
+    acc_error_pub_ = new ros::Publisher("/mbed/wheel/acc_error", &acc_error_msg_);
+    pid_param_pub_ = new ros::Publisher("/mbed/wheel/pid_params", &pid_param_msg_);
+//    thrusters_pub_ = new ros::Publisher("/mbed/thruster/commands/duty_cycle", &thruster_pwm_msg_);
     
     root_nh_->advertise(*velocity_sns_pub_);
     root_nh_->advertise(*setpoint_pub_);
     root_nh_->advertise(*acc_error_pub_);
     root_nh_->advertise(*pid_param_pub_);
     
-    wheel_vel_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/velocity", &FreeFlyerHardware::wheelVelCmdCallback, this);
-    wheel_duty_cycle_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("wheel/commands/duty_cycle", &FreeFlyerHardware::wheelDutyCycleCmdCallback, this);
-    wheel_pid_params_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("wheel/commands/pid_params", &FreeFlyerHardware::wheelPidParamsCmdCallback, this);
-    thruster_duty_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("thruster/commands/duty_cycle", &FreeFlyerHardware::thrusterDutyCmdCallback, this);
+    wheel_vel_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("/free_flyer/enterprise/wheel/commands/velocity", &FreeFlyerHardware::wheelVelCmdCallback, this);
+    wheel_duty_cycle_cmd_sub_ = new ros::Subscriber<std_msgs::Float32, FreeFlyerHardware>("/free_flyer/enterprise/wheel/commands/duty_cycle", &FreeFlyerHardware::wheelDutyCycleCmdCallback, this);
+    wheel_pid_params_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("/free_flyer/enterprise/wheel/commands/pid_params", &FreeFlyerHardware::wheelPidParamsCmdCallback, this);
+    thruster_duty_cmd_sub_ = new ros::Subscriber<std_msgs::Float32MultiArray, FreeFlyerHardware>("/free_flyer/enterprise/thruster/commands/duty_cycle", &FreeFlyerHardware::thrusterDutyCmdCallback, this);
     
     root_nh_->subscribe(*wheel_vel_cmd_sub_);
     root_nh_->subscribe(*wheel_pid_params_cmd_sub_);
@@ -88,6 +96,11 @@
 void FreeFlyerHardware::thrusterDutyCmdCallback(const std_msgs::Float32MultiArray& msg) {
     for (int i = 0; i < NUM_THRUSTERS; i++)
         thruster_pwm_stop_[i] = ceil(msg.data[i]*THRUST_PWM_N);
+        
+    // debug republish thruster//s commands
+//    for (int i = 0; i < NUM_THRUSTERS; i++)
+//        thruster_pwm_msg_.data[i] = msg.data[i];
+//    thrusters_pub_->publish(&thruster_pwm_msg_);
 }
 
 void FreeFlyerHardware::updatePID() {