From ec15a5ec3d9b5355accd524b94bcbfa7050cc2c4 Mon Sep 17 00:00:00 2001 From: Stephen Hart Date: Fri, 20 Feb 2015 13:49:15 -0600 Subject: [PATCH] TRACLabs version of the ros2lcm pronto translator nodes --- CMakeLists.txt | 188 +++++++++++++++++ cmake/pods.cmake | 363 ++++++++++++++++++++++++++++++++ nodes/pronto_helper.py | 52 +++++ package.xml | 67 ++++++ src/lcm2ros.cpp | 71 +++++++ src/ros2lcm.cpp | 460 +++++++++++++++++++++++++++++++++++++++++ 6 files changed, 1201 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 cmake/pods.cmake create mode 100644 nodes/pronto_helper.py create mode 100644 package.xml create mode 100644 src/lcm2ros.cpp create mode 100644 src/ros2lcm.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..49be64f --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,188 @@ +cmake_minimum_required(VERSION 2.8.3) + +include(cmake/pods.cmake) +set(POD_NAME pronto_translators) + +project(pronto_translators) + +## 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 COMPONENTS + roscpp + rospy + std_msgs + sensor_msgs + nav_msgs + atlas_hardware_interface +) + +## 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 and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_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 +# ) + +################################### +## 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 you 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 beginner_tutorials +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(beginner_tutorials +# src/${PROJECT_NAME}/beginner_tutorials.cpp +# ) + +## Declare a cpp executable +# add_executable(beginner_tutorials_node src/beginner_tutorials_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(beginner_tutorials_node beginner_tutorials_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(beginner_tutorials_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 +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS beginner_tutorials beginner_tutorials_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_beginner_tutorials.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) + + +include_directories(include ${catkin_INCLUDE_DIRS}) + + +add_executable(lcm2ros src/lcm2ros.cpp) +target_link_libraries(lcm2ros ${catkin_LIBRARIES}) +pods_use_pkg_config_packages(lcm2ros lcm lcmtypes_pronto-lcmtypes bot2-core) + +add_executable(ros2lcm src/ros2lcm.cpp) +target_link_libraries(ros2lcm ${catkin_LIBRARIES}) +pods_use_pkg_config_packages(ros2lcm lcm lcmtypes_pronto-lcmtypes bot2-core eigen3) +add_dependencies(ros2lcm trooper_mlc_msgs_generate_messages_cpp) + +# add_executable(ros2lcm_tl src/ros2lcm_tl.cpp) +# target_link_libraries(ros2lcm_tl ${catkin_LIBRARIES}) +# pods_use_pkg_config_packages(ros2lcm_tl lcm lcmtypes_pronto-lcmtypes bot2-core eigen3) +# add_dependencies(ros2lcm_tl atlas_hardware_interface_generate_messages_cpp) \ No newline at end of file diff --git a/cmake/pods.cmake b/cmake/pods.cmake new file mode 100644 index 0000000..4926213 --- /dev/null +++ b/cmake/pods.cmake @@ -0,0 +1,363 @@ +# Macros to simplify compliance with the pods build policies. +# +# To enable the macros, add the following lines to CMakeLists.txt: +# set(POD_NAME ) +# include(cmake/pods.cmake) +# +# If POD_NAME is not set, then the CMake source directory is used as POD_NAME +# +# Next, any of the following macros can be used. See the individual macro +# definitions in this file for individual documentation. +# +# C/C++ +# pods_install_headers(...) +# pods_install_libraries(...) +# pods_install_executables(...) +# pods_install_pkg_config_file(...) +# +# pods_use_pkg_config_packages(...) +# +# Python +# pods_install_python_packages(...) +# pods_install_python_script(...) +# +# Java +# None yet +# +# ---- +# File: pods.cmake +# Distributed with pods version: 11.03.11 + +# pods_install_headers( ... DESTINATION ) +# +# Install a (list) of header files. +# +# Header files will all be installed to include/ +# +# example: +# add_library(perception detector.h sensor.h) +# pods_install_headers(detector.h sensor.h DESTINATION perception) +# +function(pods_install_headers) + list(GET ARGV -2 checkword) + if(NOT checkword STREQUAL DESTINATION) + message(FATAL_ERROR "pods_install_headers missing DESTINATION parameter") + endif() + + list(GET ARGV -1 dest_dir) + list(REMOVE_AT ARGV -1) + list(REMOVE_AT ARGV -1) + #copy the headers to the INCLUDE_OUTPUT_PATH (${CMAKE_BINARY_DIR}/include) + foreach(header ${ARGV}) + get_filename_component(_header_name ${header} NAME) + configure_file(${header} ${INCLUDE_OUTPUT_PATH}/${dest_dir}/${_header_name} COPYONLY) + endforeach(header) + #mark them to be installed + install(FILES ${ARGV} DESTINATION include/${dest_dir}) + + +endfunction(pods_install_headers) + +# pods_install_executables( ...) +# +# Install a (list) of executables to bin/ +function(pods_install_executables) + install(TARGETS ${ARGV} RUNTIME DESTINATION bin) +endfunction(pods_install_executables) + +# pods_install_libraries( ...) +# +# Install a (list) of libraries to lib/ +function(pods_install_libraries) + install(TARGETS ${ARGV} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) +endfunction(pods_install_libraries) + + +# pods_install_pkg_config_file( +# [VERSION ] +# [DESCRIPTION ] +# [CFLAGS ...] +# [LIBS ...] +# [REQUIRES ...]) +# +# Create and install a pkg-config .pc file. +# +# example: +# add_library(mylib mylib.c) +# pods_install_pkg_config_file(mylib LIBS -lmylib REQUIRES glib-2.0) +function(pods_install_pkg_config_file) + list(GET ARGV 0 pc_name) + # TODO error check + + set(pc_version 0.0.1) + set(pc_description ${pc_name}) + set(pc_requires "") + set(pc_libs "") + set(pc_cflags "") + set(pc_fname "${PKG_CONFIG_OUTPUT_PATH}/${pc_name}.pc") + + set(modewords LIBS CFLAGS REQUIRES VERSION DESCRIPTION) + set(curmode "") + + # parse function arguments and populate pkg-config parameters + list(REMOVE_AT ARGV 0) + foreach(word ${ARGV}) + list(FIND modewords ${word} mode_index) + if(${mode_index} GREATER -1) + set(curmode ${word}) + elseif(curmode STREQUAL LIBS) + set(pc_libs "${pc_libs} ${word}") + elseif(curmode STREQUAL CFLAGS) + set(pc_cflags "${pc_cflags} ${word}") + elseif(curmode STREQUAL REQUIRES) + set(pc_requires "${pc_requires} ${word}") + elseif(curmode STREQUAL VERSION) + set(pc_version ${word}) + set(curmode "") + elseif(curmode STREQUAL DESCRIPTION) + set(pc_description "${word}") + set(curmode "") + else(${mode_index} GREATER -1) + message("WARNING incorrect use of pods_add_pkg_config (${word})") + break() + endif(${mode_index} GREATER -1) + endforeach(word) + + # write the .pc file out + file(WRITE ${pc_fname} + "prefix=${CMAKE_INSTALL_PREFIX}\n" + "exec_prefix=\${prefix}\n" + "libdir=\${exec_prefix}/lib\n" + "includedir=\${prefix}/include\n" + "\n" + "Name: ${pc_name}\n" + "Description: ${pc_description}\n" + "Requires: ${pc_requires}\n" + "Version: ${pc_version}\n" + "Libs: -L\${libdir} ${pc_libs}\n" + "Cflags: -I\${includedir} ${pc_cflags}\n") + + # mark the .pc file for installation to the lib/pkgconfig directory + install(FILES ${pc_fname} DESTINATION lib/pkgconfig) + + # find targets that this pkg-config file depends on + if (pc_libs) + string(REPLACE " " ";" split_lib ${pc_libs}) + foreach(lib ${split_lib}) + string(REGEX REPLACE "^-l" "" libname ${lib}) + get_target_property(IS_TARGET ${libname} LOCATION) + if (NOT IS_TARGET STREQUAL "IS_TARGET-NOTFOUND") + set_property(GLOBAL APPEND PROPERTY "PODS_PKG_CONFIG_TARGETS-${pc_name}" ${libname}) + endif() + endforeach() + endif() + +endfunction(pods_install_pkg_config_file) + + +# pods_install_python_script( ) +# +# Create and install a script that invokes the python interpreter with a +# specified module. +# +# A script will be installed to bin/. The script simply +# adds /lib/pythonX.Y/site-packages to the python path, and +# then invokes `python -m `. +# +# example: +# pods_install_python_script(run-pdb pdb) +function(pods_install_python_script script_name py_module) + find_package(PythonInterp REQUIRED) + + # which python version? + execute_process(COMMAND + ${PYTHON_EXECUTABLE} -c "import sys; sys.stdout.write(sys.version[:3])" + OUTPUT_VARIABLE pyversion) + + # where do we install .py files to? + set(python_install_dir + ${CMAKE_INSTALL_PREFIX}/lib/python${pyversion}/site-packages) + + # write the script file + file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/${script_name} "#!/bin/sh\n" + "export PYTHONPATH=${python_install_dir}:\${PYTHONPATH}\n" + "exec ${PYTHON_EXECUTABLE} -m ${py_module} $*\n") + + # install it... + install(PROGRAMS ${CMAKE_CURRENT_BINARY_DIR}/${script_name} DESTINATION bin) +endfunction() + +# pods_install_python_packages() +# +# Install python packages to lib/pythonX.Y/site-packages, where X.Y refers to +# the current python version (e.g., 2.6) +# +# Recursively searches for .py files, byte-compiles them, and +# installs them +function(pods_install_python_packages py_src_dir) + find_package(PythonInterp REQUIRED) + + # which python version? + execute_process(COMMAND + ${PYTHON_EXECUTABLE} -c "import sys; sys.stdout.write(sys.version[:3])" + OUTPUT_VARIABLE pyversion) + + # where do we install .py files to? + set(python_install_dir + ${CMAKE_INSTALL_PREFIX}/lib/python${pyversion}/site-packages) + + if(ARGC GREATER 1) + message(FATAL_ERROR "NYI") + else() + # get a list of all .py files + file(GLOB_RECURSE py_files RELATIVE ${py_src_dir} ${py_src_dir}/*.py) + + # add rules for byte-compiling .py --> .pyc + foreach(py_file ${py_files}) + get_filename_component(py_dirname ${py_file} PATH) + add_custom_command(OUTPUT "${py_src_dir}/${py_file}c" + COMMAND ${PYTHON_EXECUTABLE} -m py_compile ${py_src_dir}/${py_file} + DEPENDS ${py_src_dir}/${py_file}) + list(APPEND pyc_files "${py_src_dir}/${py_file}c") + + # install python file and byte-compiled file + install(FILES ${py_src_dir}/${py_file} ${py_src_dir}/${py_file}c + DESTINATION "${python_install_dir}/${py_dirname}") +# message("${py_src_dir}/${py_file} -> ${python_install_dir}/${py_dirname}") + endforeach() + string(REGEX REPLACE "[^a-zA-Z0-9]" "_" san_src_dir "${py_src_dir}") + add_custom_target("pyc_${san_src_dir}" ALL DEPENDS ${pyc_files}) + endif() +endfunction() + + +# pods_use_pkg_config_packages( ...) +# +# Convenience macro to get compiler and linker flags from pkg-config and apply them +# to the specified target. +# +# Invokes `pkg-config --cflags-only-I ...` and adds the result to the +# include directories. +# +# Additionally, invokes `pkg-config --libs ...` and adds the result to +# the target's link flags (via target_link_libraries) +# +# example: +# add_executable(myprogram main.c) +# pods_use_pkg_config_packages(myprogram glib-2.0 opencv) +macro(pods_use_pkg_config_packages target) + if(${ARGC} LESS 2) + message(WARNING "Useless invocation of pods_use_pkg_config_packages") + return() + endif() + find_package(PkgConfig REQUIRED) + execute_process(COMMAND + ${PKG_CONFIG_EXECUTABLE} --cflags-only-I ${ARGN} + OUTPUT_VARIABLE _pods_pkg_include_flags) + string(STRIP ${_pods_pkg_include_flags} _pods_pkg_include_flags) + string(REPLACE "-I" "" _pods_pkg_include_flags "${_pods_pkg_include_flags}") + separate_arguments(_pods_pkg_include_flags) + # message("include: ${_pods_pkg_include_flags}") + execute_process(COMMAND + ${PKG_CONFIG_EXECUTABLE} --libs ${ARGN} + OUTPUT_VARIABLE _pods_pkg_ldflags) + string(STRIP ${_pods_pkg_ldflags} _pods_pkg_ldflags) + # message("ldflags: ${_pods_pkg_ldflags}") + include_directories(${_pods_pkg_include_flags}) + target_link_libraries(${target} ${_pods_pkg_ldflags}) + + # make the target depend on libraries being installed by this source build + foreach(_pkg ${ARGN}) + get_property(_has_dependencies GLOBAL PROPERTY "PODS_PKG_CONFIG_TARGETS-${_pkg}" SET) + if(_has_dependencies) + get_property(_dependencies GLOBAL PROPERTY "PODS_PKG_CONFIG_TARGETS-${_pkg}") + add_dependencies(${target} ${_dependencies}) + # message("Found dependencies for ${_pkg}: ${dependencies}") + endif() + unset(_has_dependencies) + unset(_dependencies) + endforeach() + + unset(_pods_pkg_include_flags) + unset(_pods_pkg_ldflags) +endmacro() + + +# pods_config_search_paths() +# +# Setup include, linker, and pkg-config paths according to the pods core +# policy. This macro is automatically invoked, there is no need to do so +# manually. +macro(pods_config_search_paths) + if(NOT DEFINED __pods_setup) + #set where files should be output locally + set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib) + set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin) + set(INCLUDE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/include) + set(PKG_CONFIG_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib/pkgconfig) + + #set where files should be installed to + set(LIBRARY_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/lib) + set(EXECUTABLE_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/bin) + set(INCLUDE_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/include) + set(PKG_CONFIG_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/lib/pkgconfig) + + + # add build/lib/pkgconfig to the pkg-config search path + set(ENV{PKG_CONFIG_PATH} ${PKG_CONFIG_INSTALL_PATH}:$ENV{PKG_CONFIG_PATH}) + set(ENV{PKG_CONFIG_PATH} ${PKG_CONFIG_OUTPUT_PATH}:$ENV{PKG_CONFIG_PATH}) + + # add build/include to the compiler include path + include_directories(BEFORE ${INCLUDE_OUTPUT_PATH}) + include_directories(${INCLUDE_INSTALL_PATH}) + + # add build/lib to the link path + link_directories(${LIBRARY_OUTPUT_PATH}) + link_directories(${LIBRARY_INSTALL_PATH}) + + + # abuse RPATH + if(${CMAKE_INSTALL_RPATH}) + set(CMAKE_INSTALL_RPATH ${LIBRARY_INSTALL_PATH}:${CMAKE_INSTALL_RPATH}) + else(${CMAKE_INSTALL_RPATH}) + set(CMAKE_INSTALL_RPATH ${LIBRARY_INSTALL_PATH}) + endif(${CMAKE_INSTALL_RPATH}) + + # for osx, which uses "install name" path rather than rpath + #set(CMAKE_INSTALL_NAME_DIR ${LIBRARY_OUTPUT_PATH}) + set(CMAKE_INSTALL_NAME_DIR ${CMAKE_INSTALL_RPATH}) + + # hack to force cmake always create install and clean targets + install(FILES DESTINATION) + add_custom_target(tmp) + + set(__pods_setup true) + endif(NOT DEFINED __pods_setup) +endmacro(pods_config_search_paths) + +macro(enforce_out_of_source) + if(CMAKE_BINARY_DIR STREQUAL PROJECT_SOURCE_DIR) + message(FATAL_ERROR + "\n + Do not run cmake directly in the pod directory. + use the supplied Makefile instead! You now need to + remove CMakeCache.txt and the CMakeFiles directory. + + Then to build, simply type: + $ make + ") + endif() +endmacro(enforce_out_of_source) + +#set the variable POD_NAME to the directory path, and set the cmake PROJECT_NAME +if(NOT POD_NAME) + get_filename_component(POD_NAME ${CMAKE_SOURCE_DIR} NAME) + message(STATUS "POD_NAME is not set... Defaulting to directory name: ${POD_NAME}") +endif(NOT POD_NAME) +project(${POD_NAME}) + +#make sure we're running an out-of-source build +enforce_out_of_source() + +#call the function to setup paths +pods_config_search_paths() \ No newline at end of file diff --git a/nodes/pronto_helper.py b/nodes/pronto_helper.py new file mode 100644 index 0000000..a6d61e2 --- /dev/null +++ b/nodes/pronto_helper.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python +import argparse + +import rospy +import roslib; roslib.load_manifest("pronto_helper") + +import math +import copy +import threading +import tf + +from sensor_msgs.msg import JointState +from geometry_msgs.msg import Pose, PoseStamped +from nav_msgs.msg import Odometry +from atlas_hardware_interface import * + +class ProntoHelper: + + def __init__(self): + + self.atlas_data_sub = rospy.Subscriber('/atlas_hardware/data/full', AtlasControlDataFromRobot, self.atlas_data_cb) + + footstep_pub = rospy.Publisher('/foot_contact_service/foot_sensor', AtlasFootSensor, queue_size=10) + + + def atlas_data_cb(self, data) : + print data.walk_feedback + + fs = AtlasFootSensor() + fs.left_fz = 0.0 + fs.left_mx = 0.0 + fs.left_my = 0.0 + fs.right_fz = 0.0 + fs.right_mx = 0.0 + fs.right_my = 0.0 + footstep_pub.pubish(fs) + + +if __name__=="__main__": + + parser = argparse.ArgumentParser(description='Pronto Helper') + # parser.add_argument('-r, --robot', dest='robot', help='e.g. r2') + + args = parser.parse_args() + + rospy.init_node("pronto_helper") + + robot = ProntoHelper() + + r = rospy.Rate(50.0) + while not rospy.is_shutdown(): + r.sleep() diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..67ef74b --- /dev/null +++ b/package.xml @@ -0,0 +1,67 @@ + + + pronto_translators + 0.0.0 + The pronto_translators package + + + + + swhart + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + atlas_hardware_interface + geometry_msgs + nav_msgs + roscpp + rospy + sensor_msgs + std_msgs + atlas_hardware_interface + geometry_msgs + nav_msgs + roscpp + rospy + sensor_msgs + std_msgs + + + + + + + + + + + \ No newline at end of file diff --git a/src/lcm2ros.cpp b/src/lcm2ros.cpp new file mode 100644 index 0000000..1fd4489 --- /dev/null +++ b/src/lcm2ros.cpp @@ -0,0 +1,71 @@ +#include +#include +#include +#include + +#include "lcmtypes/bot_core/pose_t.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace std; + +class LCM2ROS{ + public: + LCM2ROS(boost::shared_ptr &lcm_, ros::NodeHandle &nh_); + ~LCM2ROS() {} + + private: + boost::shared_ptr lcm_; + ros::NodeHandle nh_; + + void poseBodyHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const bot_core::pose_t* msg); + ros::Publisher pose_body_pub_; + ros::NodeHandle* rosnode; +}; + +LCM2ROS::LCM2ROS(boost::shared_ptr &lcm_, ros::NodeHandle &nh_): lcm_(lcm_),nh_(nh_) { + lcm_->subscribe("POSE_BODY",&LCM2ROS::poseBodyHandler, this); + pose_body_pub_ = nh_.advertise("/pose_body",10); + rosnode = new ros::NodeHandle(); +} + +void LCM2ROS::poseBodyHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel, const bot_core::pose_t* msg) { + //ROS_ERROR("LCM2ROS got pose_t"); + nav_msgs::Odometry msgout; + msgout.header.stamp= ros::Time().fromSec(msg->utime*1E-6); + msgout.pose.pose.position.x = msg->pos[0]; + msgout.pose.pose.position.y = msg->pos[1]; + msgout.pose.pose.position.z = msg->pos[2]; + msgout.pose.pose.orientation.w = msg->orientation[0]; + msgout.pose.pose.orientation.x = msg->orientation[1]; + msgout.pose.pose.orientation.y = msg->orientation[2]; + msgout.pose.pose.orientation.z = msg->orientation[3]; + msgout.twist.twist.linear.x = msg->vel[0]; + msgout.twist.twist.linear.y = msg->vel[1]; + msgout.twist.twist.linear.z = msg->vel[2]; + msgout.twist.twist.angular.x = msg->rotation_rate[0]; + msgout.twist.twist.angular.y = msg->rotation_rate[1]; + msgout.twist.twist.angular.z = msg->rotation_rate[2]; + pose_body_pub_.publish(msgout); +} + +int main(int argc,char** argv) { + ros::init(argc,argv,"lcm2ros",ros::init_options::NoSigintHandler); + boost::shared_ptr lcm(new lcm::LCM); + if(!lcm->good()){ + std::cerr <<"ERROR: lcm is not good()" <handle()); + return 0; +} \ No newline at end of file diff --git a/src/ros2lcm.cpp b/src/ros2lcm.cpp new file mode 100644 index 0000000..ed1a5e2 --- /dev/null +++ b/src/ros2lcm.cpp @@ -0,0 +1,460 @@ +// Selective ros2lcm translator +// mfallon +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include "lcmtypes/pronto/atlas_behavior_t.hpp" +#include "lcmtypes/pronto/force_torque_t.hpp" +#include "lcmtypes/pronto/atlas_state_t.hpp" +#include "lcmtypes/pronto/utime_t.hpp" +#include "lcmtypes/pronto/atlas_raw_imu_batch_t.hpp" + +#include "lcmtypes/pronto/multisense_state_t.hpp" +//#include +//#include "lcmtypes/pronto/imu_t.hpp" + +using namespace std; + +class ROS_2_LCM{ + +public: + + ROS_2_LCM(ros::NodeHandle node_, bool simulation_); + + ~ROS_2_LCM(); + +private: + + bool simulation_; + + lcm::LCM lcm_publish_ ; + + ros::NodeHandle node_; + + // data subscribers + ros::Subscriber atlas_control_data_sub_; + ros::Subscriber joint_states_sub_; + ros::Subscriber head_joint_states_sub_; + ros::Subscriber pose_bdi_sub_; + ros::Subscriber lidar_sub_; + + // Basic Atals Control data: + void atlas_control_data_cb(const atlas_hardware_interface::AtlasControlDataFromRobotConstPtr& msg); + + // Joint Angles: + void joint_states_cb(const sensor_msgs::JointStateConstPtr& msg); + + // The position and orientation from BDI's own estimator: + void pose_bdi_cb(const atlas_hardware_interface::AtlasOdometryConstPtr& msg); + + // Multisense Joint Angles: + void head_joint_states_cb(const sensor_msgs::JointStateConstPtr& msg); + + // LIDAR data: + void lidar_cb(const sensor_msgs::LaserScanConstPtr& msg); + + // data storage + atlas_hardware_interface::AtlasControlDataFromRobot atlas_control_data_; + sensor_msgs::Imu imu_data_; + int64_t last_joint_state_utime_; + + // LCM publishers + void publish_imu_batch(); + void publish_behavior(); + void publish_joint_state(); + void publish_lidar(const sensor_msgs::LaserScanConstPtr& msg,string channel ); + void publish_multisense_state(int64_t utime, float position, float velocity); + + void store_imu_raw(); + void append_foot_sensor_data(pronto::force_torque_t& msg_out); + + bool verbose_; + + boost::mutex control_data_mutex_; + boost::mutex imu_data_mutex_; + +}; + +ROS_2_LCM::ROS_2_LCM(ros::NodeHandle node_, bool simulation_) : + simulation_(simulation_), node_(node_){ + + ROS_INFO("Initializing Translator"); + + if(!lcm_publish_.good()){ + std::cerr <<"ERROR: lcm is not good()" < scoped_lock(control_data_mutex_); + atlas_control_data_ = *msg; // not thread safe, FIXME + + store_imu_raw(); + publish_imu_batch(); + publish_behavior(); + +} + +void ROS_2_LCM::store_imu_raw() { + + // copy filtered IMU Data + boost::unique_lock scoped_lock(imu_data_mutex_); + imu_data_.header = atlas_control_data_.filtered_imu.header; + imu_data_.orientation = atlas_control_data_.filtered_imu.orientation; + imu_data_.orientation_covariance = atlas_control_data_.filtered_imu.orientation_covariance; + imu_data_.angular_velocity = atlas_control_data_.filtered_imu.angular_velocity; + imu_data_.angular_velocity_covariance = atlas_control_data_.filtered_imu.angular_velocity_covariance; + imu_data_.linear_acceleration = atlas_control_data_.filtered_imu.linear_acceleration; + imu_data_.linear_acceleration_covariance = atlas_control_data_.filtered_imu.linear_acceleration_covariance; + +} + +int behavior_counter=0; +void ROS_2_LCM::publish_behavior() { + + if (behavior_counter%100 ==0){ + ROS_INFO("BEHAVIOR ID: %d", (int) atlas_control_data_.current_behavior.state); + } + behavior_counter++; + + pronto::atlas_behavior_t msg_out; + msg_out.utime = last_joint_state_utime_; + msg_out.behavior = (int) atlas_control_data_.current_behavior.state; + + lcm_publish_.publish("ATLAS_BEHAVIOR", &msg_out); + +} + + +int scan_counter=0; +void ROS_2_LCM::lidar_cb(const sensor_msgs::LaserScanConstPtr& msg){ + + if (scan_counter%100 ==0){ // 80 + ROS_INFO("LSCAN [%d]", scan_counter ); + } + scan_counter++; + + publish_lidar(msg, "SCAN"); + +} + + +int li_counter =0; +void ROS_2_LCM::publish_lidar(const sensor_msgs::LaserScanConstPtr& msg,string channel ){ + + if (li_counter%10 ==0){ + ROS_INFO("LIDAR [%d]", li_counter ); + } + li_counter++; + + bot_core::planar_lidar_t scan_out; + scan_out.ranges = msg->ranges; + scan_out.intensities = msg->intensities; + scan_out.utime = (int64_t) floor(msg->header.stamp.toNSec()/1000); + scan_out.nranges =msg->ranges.size(); + scan_out.nintensities=msg->intensities.size(); + scan_out.rad0 = msg->angle_min; + scan_out.radstep = msg->angle_increment; + lcm_publish_.publish(channel.c_str(), &scan_out); + +} + +int gt_counter = 0; +void ROS_2_LCM::pose_bdi_cb(const atlas_hardware_interface::AtlasOdometryConstPtr& msg) { + + if (gt_counter%100 ==0){ + ROS_INFO("BDI POSE [%d]", gt_counter ); + } + + gt_counter++; + + bot_core::pose_t pose_msg; + pose_msg.utime = (int64_t) floor(atlas_control_data_.header.stamp.toNSec()/1000); + + pose_msg.pos[0] = atlas_control_data_.pos_est.position.x; + pose_msg.pos[1] = atlas_control_data_.pos_est.position.y; + pose_msg.pos[2] = atlas_control_data_.pos_est.position.z; + + pose_msg.orientation[0] = imu_data_.orientation.w; + pose_msg.orientation[1] = imu_data_.orientation.x; + pose_msg.orientation[2] = imu_data_.orientation.y; + pose_msg.orientation[3] = imu_data_.orientation.z; + + pose_msg.vel[0] = atlas_control_data_.pos_est.velocity.x; + pose_msg.vel[1] = atlas_control_data_.pos_est.velocity.y; + pose_msg.vel[2] = atlas_control_data_.pos_est.velocity.z; + + pose_msg.rotation_rate[0] = imu_data_.angular_velocity.x; + pose_msg.rotation_rate[1] = imu_data_.angular_velocity.y; + pose_msg.rotation_rate[2] = imu_data_.angular_velocity.z; + + pose_msg.accel[0] = imu_data_.linear_acceleration.x; + pose_msg.accel[1] = imu_data_.linear_acceleration.y; + pose_msg.accel[2] = imu_data_.linear_acceleration.z; + + lcm_publish_.publish("POSE_BDI", &pose_msg); + lcm_publish_.publish("POSE_BODY", &pose_msg); // for now + +} + +void ROS_2_LCM::publish_imu_batch(){ + + pronto::atlas_raw_imu_batch_t imu; + imu.utime = (int64_t) floor(atlas_control_data_.header.stamp.toNSec()/1000); + + imu.num_packets = 15; + for (size_t i=0; i < 15 ; i++){ + + /* std::cout << i + << " | " << atlas_control_data_.raw_imu[i].imu_timestamp + << " | " << atlas_control_data_.raw_imu[i].packet_count + << " | " << atlas_control_data_.raw_imu[i].da.x << " " << atlas_control_data_.raw_imu[i].da.y << " " << atlas_control_data_.raw_imu[i].da.z + << " | " << atlas_control_data_.raw_imu[i].dd.x << " " << atlas_control_data_.raw_imu[i].dd.y << " " << atlas_control_data_.raw_imu[i].dd.z << "\n";*/ + + pronto::atlas_raw_imu_t raw; + //raw.utime = atlas_control_data_.raw_imu[i].imu_timestamp; + raw.utime = (int64_t) floor(atlas_control_data_.raw_imu[i].imu_timestamp.toNSec()/1000); //atlas_control_data_.raw_imu[i].imu_timestamp; + raw.packet_count = atlas_control_data_.raw_imu[i].packet_count; + raw.delta_rotation[0] = atlas_control_data_.raw_imu[i].da.x; + raw.delta_rotation[1] = atlas_control_data_.raw_imu[i].da.y; + raw.delta_rotation[2] = atlas_control_data_.raw_imu[i].da.z; + + raw.linear_acceleration[0] = atlas_control_data_.raw_imu[i].dd.x; + raw.linear_acceleration[1] = atlas_control_data_.raw_imu[i].dd.y; + raw.linear_acceleration[2] = atlas_control_data_.raw_imu[i].dd.z; + imu.raw_imu.push_back( raw ); + } + lcm_publish_.publish( ("ATLAS_IMU_BATCH") , &imu); + + +} + + +void ROS_2_LCM::head_joint_states_cb(const sensor_msgs::JointStateConstPtr& msg){ + int64_t utime = (int64_t) floor(msg->header.stamp.toNSec()/1000); + float position = msg->position[0]; + float velocity = msg->velocity[0]; + publish_multisense_state(utime, position, velocity); +} + +void ROS_2_LCM::publish_multisense_state(int64_t utime, float position, float velocity){ + pronto::multisense_state_t msg_out; + msg_out.utime = utime; + for (std::vector::size_type i = 0; i < 13; i++) { + msg_out.joint_name.push_back("z"); + msg_out.joint_position.push_back(0); + msg_out.joint_velocity.push_back(0); + msg_out.joint_effort.push_back(0); + } + msg_out.num_joints = 13; + + msg_out.joint_position[0] = position; + msg_out.joint_velocity[0] = velocity; + msg_out.joint_name[0] = "hokuyo_joint"; + + msg_out.joint_name[1] = "pre_spindle_cal_x_joint"; + msg_out.joint_name[2] = "pre_spindle_cal_y_joint"; + msg_out.joint_name[3] = "pre_spindle_cal_z_joint"; + + msg_out.joint_name[4] = "pre_spindle_cal_roll_joint"; + msg_out.joint_name[5] = "pre_spindle_cal_pitch_joint"; + msg_out.joint_name[6] = "pre_spindle_cal_yaw_joint"; + + msg_out.joint_name[7] = "post_spindle_cal_x_joint"; + msg_out.joint_name[8] = "post_spindle_cal_x_joint"; + msg_out.joint_name[9] = "post_spindle_cal_x_joint"; + + msg_out.joint_name[10] = "post_spindle_cal_roll_joint"; + msg_out.joint_name[11] = "post_spindle_cal_pitch_joint"; + msg_out.joint_name[12] = "post_spindle_cal_yaw_joint"; + + lcm_publish_.publish("MULTISENSE_STATE", &msg_out); +} + + +inline int getIndex(const std::vector &vec, const std::string &str) { + return std::find(vec.begin(), vec.end(), str) - vec.begin(); +} + +int js_counter=0; +void ROS_2_LCM::joint_states_cb(const sensor_msgs::JointStateConstPtr& msg) { + + if (js_counter%500 ==0){ + ROS_INFO("Got JointState[%d]", js_counter); + } + js_counter++; + + std::vector< std::pair > jm; + + jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_shx") , 16 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_elx") , 17 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_akx") , 15 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "back_bkx") , 2 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_wry") , 18 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_hpy") , 12 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_wry") , 19 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_kny") , 7 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_elx") , 20 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_aky") , 14 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_shy") , 21 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_kny") , 13 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_wrx") , 22 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_akx") , 9 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_ely") , 23 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_wrx") , 24 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_hpx") , 5 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_hpy") , 6 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_hpz") , 4 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_hpx") , 11 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_shx") , 25 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "back_bky") , 1 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_shy") , 26 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "neck_ry") , 3 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_hpz") , 10 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "back_bkz") , 0 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_aky") , 8 )); + jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_ely") , 27 )); + + int n_joints = jm.size(); + + pronto::atlas_state_t msg_out; + msg_out.utime = (int64_t) atlas_control_data_.header.stamp.toNSec()/1000; // from nsec to usec + + if (verbose_) + std::cout << msg_out.utime << " jnt\n"; + + double elapsed_utime = (msg_out.utime - last_joint_state_utime_)*1E-6; + if (elapsed_utime>0.004) + std::cout << elapsed_utime << " is elapsed_utime in sec\n"; + + msg_out.joint_position.assign(n_joints , std::numeric_limits::min() ); + msg_out.joint_velocity.assign(n_joints , std::numeric_limits::min() ); + msg_out.joint_effort.assign(n_joints , std::numeric_limits::min() ); + msg_out.num_joints = n_joints; + + for (std::vector::size_type i = 0; i < jm.size(); i++) { + //std::cout << jm[i].first << " and " << jm[i].second << "\n"; + msg_out.joint_position[ jm[i].second ] = msg->position[ jm[i].first ]; + msg_out.joint_velocity[ jm[i].second ] = msg->velocity[ jm[i].first ]; + msg_out.joint_effort[ jm[i].second ] = msg->effort[ jm[i].first ]; + } + + // App end FT sensor info + pronto::force_torque_t force_torque; + append_foot_sensor_data(force_torque); + + msg_out.force_torque = force_torque; + lcm_publish_.publish("ATLAS_STATE", &msg_out); + + pronto::utime_t utime_msg; + int64_t joint_utime = (int64_t) msg->header.stamp.toNSec()/1000; // from nsec to usec + utime_msg.utime = joint_utime; + lcm_publish_.publish("ROBOT_UTIME", &utime_msg); + + last_joint_state_utime_ = joint_utime; +} + + +void ROS_2_LCM::append_foot_sensor_data(pronto::force_torque_t& msg_out){ + + msg_out.l_foot_force_z = atlas_control_data_.foot_sensors[0].force.z; + msg_out.l_foot_torque_x = atlas_control_data_.foot_sensors[0].torque.x; + msg_out.l_foot_torque_y = atlas_control_data_.foot_sensors[0].torque.y; + msg_out.r_foot_force_z = atlas_control_data_.foot_sensors[1].force.z; + msg_out.r_foot_torque_x = atlas_control_data_.foot_sensors[1].torque.x; + msg_out.r_foot_torque_y = atlas_control_data_.foot_sensors[1].torque.y; + + msg_out.l_hand_force[0] = 0; + msg_out.l_hand_force[1] = 0; + msg_out.l_hand_force[2] = 0; + msg_out.l_hand_torque[0] = 0; + msg_out.l_hand_torque[1] = 0; + msg_out.l_hand_torque[2] = 0; + msg_out.r_hand_force[0] = 0; + msg_out.r_hand_force[1] = 0; + msg_out.r_hand_force[2] = 0; + msg_out.r_hand_torque[0] = 0; + msg_out.r_hand_torque[1] = 0; + msg_out.r_hand_torque[2] = 0; + +} + + +int main(int argc, char **argv){ + + bool simulation = false; + + ros::init(argc, argv, "ros2lcm"); + + ros::NodeHandle nh; + + new ROS_2_LCM(nh, simulation); + + ROS_INFO("ROS2LCM Translator Ready"); + + ros::spin(); + + return 0; +}