diff --git a/igvc_msgs/CMakeLists.txt b/igvc_msgs/CMakeLists.txt index 0a551db61..8d42b9dac 100644 --- a/igvc_msgs/CMakeLists.txt +++ b/igvc_msgs/CMakeLists.txt @@ -21,6 +21,7 @@ add_message_files( igvc_path.msg trajectory_point.msg trajectory.msg + motor_control.msg ) # Generate added messages and services with any dependencies listed here diff --git a/igvc_msgs/msg/motor_control.msg b/igvc_msgs/msg/motor_control.msg new file mode 100644 index 000000000..f412797df --- /dev/null +++ b/igvc_msgs/msg/motor_control.msg @@ -0,0 +1,4 @@ +Header header +uint8 left +uint8 right +duration duration diff --git a/igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.c b/igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.c index d19e7c004..b2f5bbb71 100644 --- a/igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.c +++ b/igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.c @@ -1,5 +1,5 @@ /* Automatically generated nanopb constant definitions */ -/* Generated by nanopb-0.4.0-dev */ +/* Generated by nanopb-0.3.9.3 at Tue Nov 5 17:37:54 2019. */ #include "igvc.pb.h" @@ -12,7 +12,7 @@ const float RequestMessage_speed_l_default = 0; const float RequestMessage_speed_r_default = 0; -const pb_field_t ResponseMessage_fields[14] = { +const pb_field_t ResponseMessage_fields[16] = { PB_FIELD( 1, FLOAT , OPTIONAL, STATIC , FIRST, ResponseMessage, p_l, p_l, 0), PB_FIELD( 2, FLOAT , OPTIONAL, STATIC , OTHER, ResponseMessage, p_r, p_l, 0), PB_FIELD( 3, FLOAT , OPTIONAL, STATIC , OTHER, ResponseMessage, i_l, p_r, 0), @@ -26,6 +26,8 @@ const pb_field_t ResponseMessage_fields[14] = { PB_FIELD( 11, BOOL , OPTIONAL, STATIC , OTHER, ResponseMessage, estop, voltage, 0), PB_FIELD( 12, FLOAT , OPTIONAL, STATIC , OTHER, ResponseMessage, kv_l, estop, 0), PB_FIELD( 13, FLOAT , OPTIONAL, STATIC , OTHER, ResponseMessage, kv_r, kv_l, 0), + PB_FIELD( 14, UINT32 , OPTIONAL, STATIC , OTHER, ResponseMessage, left_output, kv_r, 0), + PB_FIELD( 15, UINT32 , OPTIONAL, STATIC , OTHER, ResponseMessage, right_output, left_output, 0), PB_LAST_FIELD }; diff --git a/igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.h b/igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.h index a02a95c01..bb1fc88a4 100644 --- a/igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.h +++ b/igvc_platform/include/igvc_platform/nanopb/protos/igvc.pb.h @@ -1,5 +1,5 @@ /* Automatically generated nanopb header */ -/* Generated by nanopb-0.4.0-dev */ +/* Generated by nanopb-0.3.9.3 at Tue Nov 5 17:37:54 2019. */ #ifndef PB_IGVC_PB_H_INCLUDED #define PB_IGVC_PB_H_INCLUDED @@ -69,6 +69,10 @@ extern "C" float kv_l; bool has_kv_r; float kv_r; + bool has_left_output; + uint32_t left_output; + bool has_right_output; + uint32_t right_output; /* @@protoc_insertion_point(struct:ResponseMessage) */ } ResponseMessage; @@ -80,7 +84,7 @@ extern "C" #define ResponseMessage_init_default \ { \ false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, \ - false, 0, false, 0 \ + false, 0, false, 0, false, 0, false, 0 \ } #define RequestMessage_init_default \ { \ @@ -89,7 +93,7 @@ extern "C" #define ResponseMessage_init_zero \ { \ false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, false, 0, \ - false, 0, false, 0 \ + false, 0, false, 0, false, 0, false, 0 \ } #define RequestMessage_init_zero \ { \ @@ -120,13 +124,15 @@ extern "C" #define ResponseMessage_estop_tag 11 #define ResponseMessage_kv_l_tag 12 #define ResponseMessage_kv_r_tag 13 +#define ResponseMessage_left_output_tag 14 +#define ResponseMessage_right_output_tag 15 /* Struct field encoding specification for nanopb */ - extern const pb_field_t ResponseMessage_fields[14]; + extern const pb_field_t ResponseMessage_fields[16]; extern const pb_field_t RequestMessage_fields[11]; /* Maximum encoded size of messages (where known) */ -#define ResponseMessage_size 62 +#define ResponseMessage_size 74 #define RequestMessage_size 50 /* Message IDs (where set with "msgid" option) */ diff --git a/igvc_platform/src/motor_controller/motor_controller.cpp b/igvc_platform/src/motor_controller/motor_controller.cpp index 53642eb62..9d068b866 100644 --- a/igvc_platform/src/motor_controller/motor_controller.cpp +++ b/igvc_platform/src/motor_controller/motor_controller.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -23,6 +24,7 @@ MotorController::MotorController(ros::NodeHandle* nodehandle) : nh_(*nodehandle) enc_pub_ = nh_.advertise("/encoders", 1000); enabled_pub_ = nh_.advertise("/robot_enabled", 1); battery_pub_ = nh_.advertise("/battery", 1); + motor_output_pub_ = nh_.advertise("/motor_output", 10); // get server ip address and port number from the launch file assertions::getParam(pNh, std::string("ip_addr"), ip_addr_); @@ -277,6 +279,14 @@ void MotorController::publishResponse(const ResponseMessage& response) enc_msg.duration = response.dt_sec; enc_msg.header.stamp = ros::Time::now() - ros::Duration(response.dt_sec); enc_pub_.publish(enc_msg); + + // Publish motor output + igvc_msgs::motor_control motor_control; + motor_control.left = static_cast(response.left_output); + motor_control.right = static_cast(response.right_output); + motor_control.duration = ros::Duration(response.dt_sec); + motor_control.header.stamp = ros::Time::now() - motor_control.duration; + motor_output_pub_.publish(motor_control); } int main(int argc, char** argv) diff --git a/igvc_platform/src/motor_controller/motor_controller.h b/igvc_platform/src/motor_controller/motor_controller.h index 09aedc86f..5a0e0898f 100644 --- a/igvc_platform/src/motor_controller/motor_controller.h +++ b/igvc_platform/src/motor_controller/motor_controller.h @@ -45,6 +45,7 @@ class MotorController ros::Publisher enc_pub_; ros::Publisher enabled_pub_; ros::Publisher battery_pub_; + ros::Publisher motor_output_pub_; /** get current motor command from the /motors topic