From 66301060355b632fb7553166f2de7621acb194e5 Mon Sep 17 00:00:00 2001
From: Anton Tetov <anton@tetov.se>
Date: Fri, 15 Oct 2021 16:13:15 +0200
Subject: [PATCH] ugly python version
---
CMakeLists.txt | 206 +++----------------------------------
launch/publish.launch | 6 +-
nodes/jr3_comedi_publisher | 47 ---------
nodes/publisher | 44 ++++++++
package.xml | 61 ++---------
requirements.in | 1 +
6 files changed, 69 insertions(+), 296 deletions(-)
delete mode 100644 nodes/jr3_comedi_publisher
create mode 100644 nodes/publisher
create mode 100644 requirements.in
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f1d369e..9f50750 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,202 +1,20 @@
cmake_minimum_required(VERSION 3.0.2)
project(ros_jr3_comedi)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+find_package(catkin REQUIRED catkin_virtualenv)
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-## * add a build_depend tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-## * add "message_generation" and every package in MSG_DEP_SET to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * add "message_runtime" and every package in MSG_DEP_SET to
-## catkin_package(CATKIN_DEPENDS ...)
-## * uncomment the add_*_files sections below as needed
-## and list every .msg/.srv/.action file to be processed
-## * uncomment the generate_messages entry below
-## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-# FILES
-# Action1.action
-# Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# std_msgs # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-# INCLUDE_DIRS include
-# LIBRARIES ros_jr3_comedi
-# CATKIN_DEPENDS other_catkin_pkg
-# DEPENDS system_lib
+catkin_generate_virtualenv(
+ INPUT_REQUIREMENTS requirements.in
+ USE_SYSTEM_PACKAGES TRUE
)
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
-# ${catkin_INCLUDE_DIRS}
+catkin_install_python(
+ PROGRAMS
+ nodes/publisher
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/ros_jr3_comedi.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/ros_jr3_comedi_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-# FILES_MATCHING PATTERN "*.h"
-# PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-# # myfile1
-# # myfile2
-# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_jr3_comedi.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+install(FILES
+ requirements.txt
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/launch/publish.launch b/launch/publish.launch
index 78d56a9..a5332e8 100644
--- a/launch/publish.launch
+++ b/launch/publish.launch
@@ -1,6 +1,6 @@
<launch>
- <node name="jr3_comedi_publisher" pkg="ros_jr3_comedi" type="jr3_comedi_publisher">
- <param name="executable" value="/home/robot/project/extctrl/labcomm_sensors/jr3_sensors/test_JR3_comedi" />
- <param name="rate" value="10.0" type="double" />
+ <node name="jr3_comedi_publisher" pkg="ros_jr3_comedi" type="publisher">
+ <!-- <param name="executable" value="/home/robot/project/extctrl/labcomm_sensors/jr3_sensors/test_JR3_comedi" />
+ <param name="rate" value="10.0" type="double" /> -->
</node>
</launch>
diff --git a/nodes/jr3_comedi_publisher b/nodes/jr3_comedi_publisher
deleted file mode 100644
index 037de3f..0000000
--- a/nodes/jr3_comedi_publisher
+++ /dev/null
@@ -1,47 +0,0 @@
-#!/usr/bin/env python
-from subprocess import Popen, PIPE, STDOUT
-
-import rospy
-from geometry_msgs import WrenchStamped
-from std_msgs import Vector3
-
-
-def publisher():
-
- rospy.init_node('jr3_publisher', anonymous=True)
- rate = rospy.Rate(rospy.get_param('~rate', 10)) # default is 10hz
- executable = rospy.get_param('~executable', "/home/robot/project/extctrl/labcomm_sensors/jr3_sensors/test_JR3_comedi")
-
- pub = rospy.Publisher('jr3_sensor', WrenchStamped)
-
- with Popen(rospy.get_param('executable', default, stdout=PIPE, stderr=PIPE) as proc:
-
- while not rospy.is_shutdown():
-
- out, err = proc.communicate(timeout=15)
-
- if err:
- raise Exception(err)
-
- line = out.decode('ascii')
- line = line.strip()
- parts = [float(p) for p in line.split()]
-
- msg = WrenchStamped()
- msg.header.stamp = rospy.Time.now()
-
- force = Vector3(*parts[:3])
- torque = Vector3(*parts[3:])
-
- msg.wrench.force = force
- msg.wrench.torque = torque
-
- pub.publish(msg)
-
- rate.sleep()
-
-if __name__ == '__main__':
- try:
- publisher()
- except rospy.ROSInterruptException:
- pass
diff --git a/nodes/publisher b/nodes/publisher
new file mode 100644
index 0000000..2b9a438
--- /dev/null
+++ b/nodes/publisher
@@ -0,0 +1,44 @@
+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 2b6ea42..9eeb6b5 100644
--- a/package.xml
+++ b/package.xml
@@ -1,76 +1,33 @@
<?xml version="1.0"?>
<package format="2">
<name>ros_jr3_comedi</name>
- <version>0.0.0</version>
+ <version>0.1.0</version>
<description>The ros_jr3_comedi package</description>
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
- <maintainer email="tetov@todo.todo">tetov</maintainer>
+ <maintainer email="anton.johansson@control.lth.se">Anton Tetov</maintainer>
+ <license>GPLv3</license>
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
- <license>TODO</license>
-
-
- <!-- Url tags are optional, but multiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/ros_jr3_comedi</url> -->
-
-
- <!-- Author tags are optional, multiple are allowed, one per tag -->
- <!-- Authors do not have to be maintainers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
- <!-- The *depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
- <!-- <depend>roscpp</depend> -->
- <!-- Note that this is equivalent to the following: -->
- <!-- <build_depend>roscpp</build_depend> -->
- <!-- <exec_depend>roscpp</exec_depend> -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use build_export_depend for packages you need in order to build against this package: -->
- <!-- <build_export_depend>message_generation</build_export_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use exec_depend for packages you need at runtime: -->
- <!-- <exec_depend>message_runtime</exec_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <!-- Use doc_depend for packages you need only for building documentation: -->
- <!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>geometry_msg</build_depend>
- <build_depend>std_msgs</build_depend>
-
+ <build_depend>std_msgs</build_depend>
+ <build_depend>catkin_virtualenv</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>geometry_msg</build_export_depend>
- <build_export_depend>std_msgs</build_export_depend>
-
+ <build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>geometry_msg</exec_depend>
- <exec_depend>std_msgs</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+ <depend>pexpect</depend>
-
- <!-- The export tag contains other, unspecified, tags -->
<export>
- <!-- Other tools can request additional information be placed here -->
-
+ <pip_requirements>requirements.txt</pip_requirements>
</export>
</package>
diff --git a/requirements.in b/requirements.in
new file mode 100644
index 0000000..dcf2886
--- /dev/null
+++ b/requirements.in
@@ -0,0 +1 @@
+pexpect >= 4.8.0
--
GitLab