diff --git a/include/ubiquity_motor/motor_hardware.h b/include/ubiquity_motor/motor_hardware.h index 6856678..5f5fa8f 100644 --- a/include/ubiquity_motor/motor_hardware.h +++ b/include/ubiquity_motor/motor_hardware.h @@ -89,27 +89,6 @@ class MotorHardware : public hardware_interface::RobotHW { ros::Publisher leftError; ros::Publisher rightError; - ros::Publisher pubU50; - ros::Publisher pubS50; - ros::Publisher pubU51; - ros::Publisher pubS51; - ros::Publisher pubU52; - ros::Publisher pubS52; - ros::Publisher pubU53; - ros::Publisher pubS53; - ros::Publisher pubU54; - ros::Publisher pubS54; - ros::Publisher pubU55; - ros::Publisher pubS55; - ros::Publisher pubU56; - ros::Publisher pubS56; - ros::Publisher pubU57; - ros::Publisher pubS57; - ros::Publisher pubU58; - ros::Publisher pubS58; - ros::Publisher pubU59; - ros::Publisher pubS59; - MotorSerial* motor_serial_; FRIEND_TEST(MotorHardwareTests, nonZeroWriteSpeedsOutputs); diff --git a/src/motor_hardware.cc b/src/motor_hardware.cc index 9f64ae1..1c4d1d9 100644 --- a/src/motor_hardware.cc +++ b/src/motor_hardware.cc @@ -68,27 +68,6 @@ MotorHardware::MotorHardware(ros::NodeHandle nh, CommsParams serial_params, leftError = nh.advertise("left_error", 1); rightError = nh.advertise("right_error", 1); - pubU50 = nh.advertise("u50", 1); - pubS50 = nh.advertise("s50", 1); - pubU51 = nh.advertise("u51", 1); - pubS51 = nh.advertise("s51", 1); - pubU52 = nh.advertise("u52", 1); - pubS52 = nh.advertise("s52", 1); - pubU53 = nh.advertise("u53", 1); - pubS53 = nh.advertise("s53", 1); - pubU54 = nh.advertise("u54", 1); - pubS54 = nh.advertise("s54", 1); - pubU55 = nh.advertise("u55", 1); - pubS55 = nh.advertise("s55", 1); - pubU56 = nh.advertise("u56", 1); - pubS56 = nh.advertise("s56", 1); - pubU57 = nh.advertise("u57", 1); - pubS57 = nh.advertise("s57", 1); - pubU58 = nh.advertise("u58", 1); - pubS58 = nh.advertise("s58", 1); - pubU59 = nh.advertise("u59", 1); - pubS59 = nh.advertise("s59", 1); - sendPid_count = 0; pid_params = firmware_params; @@ -164,50 +143,7 @@ void MotorHardware::readInputs() { break; } default: - uint8_t reg = mm.getRegister(); - int32_t data = mm.getData(); - std_msgs::UInt32 umsg; - std_msgs::Int32 smsg; - umsg.data = data; - smsg.data = data; - switch (reg) { - case 0x50: - pubU50.publish(umsg); - pubS50.publish(smsg); - break; - case 0x51: - pubU51.publish(umsg); - pubS51.publish(smsg); - break; - case 0x52: - pubU52.publish(umsg); - pubS52.publish(smsg); - break; - case 0x53: - pubU53.publish(umsg); - pubS53.publish(smsg); - break; - case 0x54: - pubU54.publish(umsg); - pubS54.publish(smsg); - break; - case 0x55: - pubU55.publish(umsg); - pubS55.publish(smsg); - break; - case 0x56: - pubU56.publish(umsg); - pubS56.publish(smsg); - break; - case 0x57: - pubU57.publish(umsg); - pubS57.publish(smsg); - break; - case 0x58: - pubU58.publish(umsg); - pubS58.publish(smsg); - break; - } + break; } } } @@ -226,7 +162,6 @@ void MotorHardware::writeSpeeds() { std_msgs::Int32 smsg; smsg.data = left_tics; - pubS59.publish(smsg); motor_serial_->transmitCommand(both); @@ -363,4 +298,4 @@ int16_t MotorHardware::calculateTicsFromRadians(double radians) const { double MotorHardware::calculateRadiansFromTics(int16_t tics) const { return (tics * VELOCITY_READ_PER_SECOND / QTICS_PER_RADIAN); -} \ No newline at end of file +} diff --git a/test/motor_hardware_test.cc b/test/motor_hardware_test.cc index ba7f5f0..2df4b43 100644 --- a/test/motor_hardware_test.cc +++ b/test/motor_hardware_test.cc @@ -304,6 +304,7 @@ void callbackS(const std_msgs::Int32 &data) { called = true; } +/* TEST_F(MotorHardwareTests, debugRegisterUnsignedPublishes) { called = false; ros::Subscriber sub = nh.subscribe("u50", 1, callbackU); @@ -345,9 +346,10 @@ TEST_F(MotorHardwareTests, debugRegisterSignedPublishes) { called = false; } +*/ int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "param_test"); return RUN_ALL_TESTS(); -} \ No newline at end of file +}