Merge pull request #18 from nutansg/master

encoder counts
iscumd · Jan 8, 2019 · c7b5a33 · c7b5a33
2 parents 84bb13f + b641720
commit c7b5a33
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 0 deletions.
diff --git a/isc_roboteq_mdc2460/src/roboteq_mdc2460.cpp b/isc_roboteq_mdc2460/src/roboteq_mdc2460.cpp
@@ -1,5 +1,6 @@
 #include "ros/ros.h"
 #include "isc_shared_msgs/wheel_speeds.h"
+#include "isc_shared_msgs/EncoderCounts.h"
 
 #include <math.h>
 #include <serial/serial.h>
@@ -102,6 +103,8 @@ inline bool isPlusOrMinus(const string &token) {
 	return false;
 }
 
+int checkEncoderCount(int channel){return 0;}
+
 bool sendCommand(string command){
 	BufferedFilterPtr echoFilter = serialListener.createBufferedFilter(SerialListener::exactly(command));
 	if(enableLogging) ROS_INFO("Sending commend: %s", command.c_str());
@@ -155,6 +158,10 @@ int main(int argc, char **argv){
 	n.param("roboteq_enable_logging", enableLogging, false);
 
 	ros::Subscriber driveModeSub = n.subscribe("motors/wheel_speeds", 5, driveModeCallback);
+	ros::Publisher pub = n.advertise<isc_shared_msgs::EncoderCounts>("encoder_counts", 1000);
+	bool hasEncoder;
+	n.param("has_encoder", hasEncoder, false);
+	isc_shared_msgs::EncoderCounts count;
 
 	ros::Rate loopRate(100); //Hz
 	while(ros::ok()) {
@@ -169,6 +176,13 @@ int main(int argc, char **argv){
 		while(roboteqIsConnected && ros::ok()){
 			ros::spinOnce();
 			move();
+
+			if(hasEncoder){
+				count.left_count = checkEncoderCount(1);
+				count.right_count = checkEncoderCount(2);
+				pub.publish(count);
+			}
+
 			loopRate.sleep();
 		}
 

diff --git a/isc_shared_msgs/CMakeLists.txt b/isc_shared_msgs/CMakeLists.txt
@@ -52,6 +52,7 @@ add_message_files(
   FILES
   drive_mode.msg
   wheel_speeds.msg
+  EncoderCounts.msg
 )
 
 ## Generate services in the 'srv' folder

diff --git a/isc_shared_msgs/msg/EncoderCounts.msg b/isc_shared_msgs/msg/EncoderCounts.msg
@@ -0,0 +1,3 @@
+Header header
+int32 left_count
+int32 right_count