From 1f9ef1a7cadb7b4ad103d4ada48d282080c4296f Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Wed, 2 Oct 2019 14:31:26 +0200
Subject: [PATCH] Add encoder data publish
---
CMakeLists.txt | 1 +
RosAria.cpp | 16 ++++++++++++++++
msg/Encoders.msg | 3 +++
3 files changed, 20 insertions(+)
create mode 100644 msg/Encoders.msg
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0a823c1..a91c949 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,6 +23,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation roscpp nav_msgs geome
add_message_files(
FILES
BumperState.msg
+ Encoders.msg
)
#uncomment if you have defined services
#add_service_files(
diff --git a/RosAria.cpp b/RosAria.cpp
index 4dfe42c..d11adba 100644
--- a/RosAria.cpp
+++ b/RosAria.cpp
@@ -20,6 +20,7 @@
#include "tf/transform_datatypes.h"
#include <dynamic_reconfigure/server.h>
#include <rosaria/RosAriaConfig.h>
+#include "rosaria/Encoders.h"
#include "std_msgs/Float64.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Int8.h"
@@ -133,6 +134,9 @@ class RosAriaNode
// whether to publish aria lasers
bool publish_aria_lasers;
+
+ ros::Publisher encoder_pub;
+ rosaria::Encoders encoders;
};
void RosAriaNode::readParameters()
@@ -337,11 +341,15 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
veltime = ros::Time::now();
+
+ encoder_pub = n.advertise<rosaria::Encoders>("encoders",1);
}
RosAriaNode::~RosAriaNode()
{
// disable motors and sonar.
+ robot->stopEncoderPackets();
+
robot->disableMotors();
robot->disableSonar();
@@ -520,6 +528,8 @@ int RosAriaNode::Setup()
if (cmdvel_timeout_param > 0.0)
cmdvel_watchdog_timer = n.createTimer(ros::Duration(0.1), &RosAriaNode::cmdvel_watchdog, this);
+ robot->requestEncoderPackets();
+
ROS_INFO_NAMED("rosaria", "rosaria: Setup complete");
return 0;
}
@@ -690,6 +700,12 @@ void RosAriaNode::publish()
sonar_pub.publish(cloud);
}
} // end if sonar_enabled
+ // send the encoder information
+ encoders.left_encoder=robot->getLeftEncoder();
+ encoders.right_encoder=robot->getRightEncoder();
+ encoders.header.stamp=ros::Time::now();
+ encoders.header.frame_id=frame_id_odom;
+ encoder_pub.publish(encoders);
}
bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
diff --git a/msg/Encoders.msg b/msg/Encoders.msg
new file mode 100644
index 0000000..2dae240
--- /dev/null
+++ b/msg/Encoders.msg
@@ -0,0 +1,3 @@
+Header header
+int64 left_encoder
+int64 right_encoder
--
GitLab