diff --git a/CMakeLists.txt b/CMakeLists.txt
index f64da4b571ec0081c871c220f0374b0611d3cba2..ef6745abbf31ee96b9e4e1a1766502516f98ee0b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,20 +1,21 @@
cmake_minimum_required(VERSION 3.0.2)
project(ros_jr3_comedi)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(COMEDILIB REQUIRED comedilib)
+
find_package(catkin REQUIRED COMPONENTS
roscpp
- rospy
- std_msgs
geometry_msgs
)
-catkin_package(CATKIN_DEPENDS
- roscpp
- rospy
- std_msgs
- geometry_msgs
-)
+catkin_package(
+ CATKIN_DEPENDS roscpp geometry_msgs
+ DEPENDS COMEDILIB
+ )
-catkin_install_python(PROGRAMS nodes/publisher
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+include_directories(include ${catkin_INCLUDE_DIRS} include ${COMEDILIB_INCLUDE_DIRS})
+
+add_executable(publisher src/publisher.cpp)
+add_library(jr3_comedi_reader src/jr3_comedi_reader.cpp)
+target_link_libraries(publisher jr3_comedi_reader)
diff --git a/nodes/publisher b/nodes/publisher
deleted file mode 100755
index 16ef47286b937bf353ee964abf8c6d805bd51a19..0000000000000000000000000000000000000000
--- a/nodes/publisher
+++ /dev/null
@@ -1,46 +0,0 @@
-#!/usr/bin/env python
-
-import rospy
-from geometry_msgs.msg import Vector3, Wrench, WrenchStamped
-
-import pexpect
-
-def stdout_to_wrench(binary_string):
- string = binary_string.decode('ascii')
-
- if string.startswith("channel"):
- return
-
- string_parts = string.strip().split()
- float_parts = [float(p) for p in string_parts]
-
- return Wrench(force=Vector3(*float_parts[:3]), torque=Vector3(*float_parts[3:]))
-
-
-def publisher():
-
- rospy.init_node("jr3_publisher", anonymous=True)
-
- executable = (
- "/home/robot/project/extctrl/labcomm_sensors/jr3_sensors/test_JR3_comedi"
- )
- rospy.loginfo(f"Executable: {executable}")
-
- pub = rospy.Publisher("jr3_sensor", WrenchStamped, queue_size=10)
-
- msg = WrenchStamped()
-
- child = pexpect.spawn(executable, timeout=None)
-
- while not rospy.is_shutdown():
- wrench = stdout_to_wrench(next(child.__iter__()))
- if wrench:
- msg.header.stamp = rospy.Time.now()
- msg.wrench = wrench
- pub.publish(msg)
-
-if __name__ == "__main__":
- try:
- publisher()
- except rospy.ROSInterruptException:
- pass
diff --git a/package.xml b/package.xml
index 41559eb21efa40ab19eabaed3393704a83898318..17e0d37aea1fedd35c944831e6142f49de3011da 100644
--- a/package.xml
+++ b/package.xml
@@ -12,15 +12,15 @@
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>geometry_msgs</build_depend>
- <build_depend>std_msgs</build_depend>
+ <build_depend>comedilib-devel</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
- <build_export_depend>std_msgs</build_export_depend>
+ <build_export_depend>comedilib-devel</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
- <exec_depend>std_msgs</exec_depend>
+ <exec_depend>comedilib</exec_depend>
</package>
diff --git a/src/jr3_comedi_reader.cpp b/src/jr3_comedi_reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b25748de8ab53d2c6bfcf5b5e9cf5ff562fa0d0
--- /dev/null
+++ b/src/jr3_comedi_reader.cpp
@@ -0,0 +1,55 @@
+#include <comedilib.h>
+
+#include <system_error>
+#include <vector>
+#include <string>
+#include "jr3_comedi_reader.hpp"
+
+const int NUM_CHANNELS = 6;
+
+JR3ComediReader::JR3ComediReader(char devicePath[], int subdeviceArg)
+{
+ cf_jr3 = comedi_open(devicePath);
+
+ if (!cf_jr3)
+ {
+ throw std::system_error();
+ }
+
+ subdev = subdeviceArg;
+
+ comedi_range *range;
+ lsampl_t maxdata;
+
+ for (int chan = 0; chan < NUM_CHANNELS; chan++)
+ {
+ range = comedi_get_range(cf_jr3, subdev, chan, 0);
+ maxdata = comedi_get_maxdata(cf_jr3, subdev, chan);
+ info[chan].min = range->min;
+ info[chan].max = range->max;
+ info[chan].maxdata = maxdata;
+ info[chan].delta = (range->max - range->min) / maxdata;
+ printf("channel maxdata min max delta %d %d %f %f %f \n", chan, maxdata, range->min, range->max, info[chan].delta);
+ }
+}
+
+std::vector<double> JR3ComediReader::getReading()
+{
+ std::vector<double> reading;
+ lsampl_t data;
+
+ for (int chan = 0; chan < NUM_CHANNELS; chan++)
+ {
+ int err = comedi_data_read(cf_jr3, subdev, chan, 0, 0, &data);
+ if (err < 0)
+ {
+ printf("comedi_data_read error: err=%d subdev=%d %d %d\n", err, subdev, chan, data);
+ }
+ else
+ {
+ reading[chan] = (info[chan].min + (double)data * info[chan].delta) * 1000.0;
+ }
+ }
+
+ return reading;
+};
diff --git a/src/jr3_comedi_reader.hpp b/src/jr3_comedi_reader.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3361a2b28564ce3e4728647d6729804d3d4a9b03
--- /dev/null
+++ b/src/jr3_comedi_reader.hpp
@@ -0,0 +1,23 @@
+#include <string>
+#include "comedilib.h"
+#include <vector>
+
+typedef struct
+{
+ double min;
+ double max;
+ double delta;
+ double maxdata;
+} info_t;
+
+class JR3ComediReader
+{
+private:
+ comedi_t *cf_jr3;
+ int subdev;
+ info_t info[6];
+
+public:
+ JR3ComediReader(char devicePath[], int subdeviceArg);
+ std::vector<double> getReading();
+};
diff --git a/src/publisher.cpp b/src/publisher.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3815351bd1e236c6e83322d642330b24759b38de
--- /dev/null
+++ b/src/publisher.cpp
@@ -0,0 +1,41 @@
+#include <geometry_msgs/WrenchStamped.h>
+#include <ros/ros.h>
+
+#include <vector>
+
+#include "jr3_comedi_reader.hpp"
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "jr3_publisher");
+
+ ros::NodeHandle n;
+
+ ros::Publisher publisher = n.advertise<geometry_msgs::WrenchStamped>("jr3_sensor", 1);
+
+ ros::Rate loop_rate(500);
+
+ geometry_msgs::WrenchStamped msg;
+
+ JR3ComediReader jr3("/etc/comedi1", 0);
+
+ while (ros::ok())
+ {
+ std::vector readings = jr3.getReading();
+
+ msg.wrench.force.x = readings[0];
+ msg.wrench.force.y = readings[1];
+ msg.wrench.force.z = readings[2];
+ msg.wrench.torque.x = readings[3];
+ msg.wrench.torque.y = readings[4];
+ msg.wrench.torque.z = readings[5];
+
+ msg.header.stamp = ros::Time::now();
+
+ publisher.publish(msg);
+
+ ros::spinOnce();
+
+ loop_rate.sleep();
+ }
+}