From 0435bc70ab76f5f53375511385e89e109a7715ec Mon Sep 17 00:00:00 2001 From: Ethan Fowler Date: Thu, 1 Dec 2022 17:54:07 +0000 Subject: [PATCH] Removing cyberglove artefacts. --- cyberglove/CMakeLists.txt | 174 ------- cyberglove/README.md | 29 -- .../include/cyberglove/cyberglove_publisher.h | 111 ---- .../include/cyberglove/cyberglove_service.h | 57 -- cyberglove/include/cyberglove/serial_glove.h | 236 --------- .../cyberglove/xml_calibration_parser.h | 130 ----- cyberglove/launch/cyberglove.launch | 50 -- cyberglove/model/cyberglove.xml | 140 ----- cyberglove/package.xml | 51 -- cyberglove/python_lib/cyberglove_calibrer.py | 347 ------------- cyberglove/python_lib/cyberglove_library.py | 174 ------- cyberglove/python_lib/cyberglove_mapper.py | 379 -------------- cyberglove/src/cyberglove_node.cpp | 59 --- cyberglove/src/cyberglove_publisher.cpp | 253 --------- cyberglove/src/cyberglove_service.cpp | 66 --- cyberglove/src/serial_glove.cpp | 397 -------------- cyberglove/src/xml_calibration_parser.cpp | 309 ----------- cyberglove/srv/Calibration.srv | 3 - cyberglove/srv/Start.srv | 3 - cyberglove/test/cyberglove_test.cal | 19 - cyberglove/test/test_calibration.cpp | 158 ------ cyberglove/test/test_calibration.test | 3 - cyberglove_trajectory/CMakeLists.txt | 178 ------- cyberglove_trajectory/README.md | 13 - .../cyberglove_trajectory_publisher.h | 151 ------ .../launch/cyberglove.launch | 49 -- cyberglove_trajectory/package.xml | 45 -- cyberglove_trajectory/setup.py | 14 - .../src/cyberglove_trajectory/__init__.py | 0 .../cyberglove_trajectory/cyberglove_mock.py | 102 ---- .../src/cyberglove_trajectory_node.cpp | 62 --- .../src/cyberglove_trajectory_publisher.cpp | 486 ------------------ repository.rosinstall | 10 - sr_remappers/CMakeLists.txt | 139 ----- sr_remappers/README.md | 27 - .../include/sr_remappers/calibration_parser.h | 90 ---- .../shadowhand_to_cyberglove_remapper.h | 99 ---- .../shadowhand_to_cybergrasp_remapper.h | 82 --- sr_remappers/launch/remapper_glove.launch | 27 - sr_remappers/package.xml | 43 -- sr_remappers/param/cyb.py | 40 -- sr_remappers/param/cyberglovetoshadowhand.map | 21 - .../cyberglovetoshadowhand_transposed.map | 22 - sr_remappers/param/shadowhandtocybergrasp.cal | 31 -- sr_remappers/param/test.cal | 6 - sr_remappers/param/tmp.py | 55 -- sr_remappers/src/calibration_parser.cpp | 135 ----- .../src/shadowhand_to_cyberglove_remapper.cpp | 175 ------- ...shadowhand_to_cyberglove_remapper_node.cpp | 38 -- .../src/shadowhand_to_cybergrasp_remapper.cpp | 108 ---- ...shadowhand_to_cybergrasp_remapper_node.cpp | 44 -- 51 files changed, 5440 deletions(-) delete mode 100644 cyberglove/CMakeLists.txt delete mode 100644 cyberglove/README.md delete mode 100644 cyberglove/include/cyberglove/cyberglove_publisher.h delete mode 100644 cyberglove/include/cyberglove/cyberglove_service.h delete mode 100644 cyberglove/include/cyberglove/serial_glove.h delete mode 100644 cyberglove/include/cyberglove/xml_calibration_parser.h delete mode 100644 cyberglove/launch/cyberglove.launch delete mode 100644 cyberglove/model/cyberglove.xml delete mode 100644 cyberglove/package.xml delete mode 100644 cyberglove/python_lib/cyberglove_calibrer.py delete mode 100644 cyberglove/python_lib/cyberglove_library.py delete mode 100644 cyberglove/python_lib/cyberglove_mapper.py delete mode 100644 cyberglove/src/cyberglove_node.cpp delete mode 100644 cyberglove/src/cyberglove_publisher.cpp delete mode 100644 cyberglove/src/cyberglove_service.cpp delete mode 100644 cyberglove/src/serial_glove.cpp delete mode 100644 cyberglove/src/xml_calibration_parser.cpp delete mode 100644 cyberglove/srv/Calibration.srv delete mode 100644 cyberglove/srv/Start.srv delete mode 100644 cyberglove/test/cyberglove_test.cal delete mode 100644 cyberglove/test/test_calibration.cpp delete mode 100644 cyberglove/test/test_calibration.test delete mode 100644 cyberglove_trajectory/CMakeLists.txt delete mode 100644 cyberglove_trajectory/README.md delete mode 100644 cyberglove_trajectory/include/cyberglove_trajectory/cyberglove_trajectory_publisher.h delete mode 100644 cyberglove_trajectory/launch/cyberglove.launch delete mode 100644 cyberglove_trajectory/package.xml delete mode 100755 cyberglove_trajectory/setup.py delete mode 100644 cyberglove_trajectory/src/cyberglove_trajectory/__init__.py delete mode 100755 cyberglove_trajectory/src/cyberglove_trajectory/cyberglove_mock.py delete mode 100644 cyberglove_trajectory/src/cyberglove_trajectory_node.cpp delete mode 100644 cyberglove_trajectory/src/cyberglove_trajectory_publisher.cpp delete mode 100644 sr_remappers/CMakeLists.txt delete mode 100644 sr_remappers/README.md delete mode 100644 sr_remappers/include/sr_remappers/calibration_parser.h delete mode 100644 sr_remappers/include/sr_remappers/shadowhand_to_cyberglove_remapper.h delete mode 100644 sr_remappers/include/sr_remappers/shadowhand_to_cybergrasp_remapper.h delete mode 100644 sr_remappers/launch/remapper_glove.launch delete mode 100644 sr_remappers/package.xml delete mode 100644 sr_remappers/param/cyb.py delete mode 100644 sr_remappers/param/cyberglovetoshadowhand.map delete mode 100644 sr_remappers/param/cyberglovetoshadowhand_transposed.map delete mode 100644 sr_remappers/param/shadowhandtocybergrasp.cal delete mode 100644 sr_remappers/param/test.cal delete mode 100644 sr_remappers/param/tmp.py delete mode 100644 sr_remappers/src/calibration_parser.cpp delete mode 100644 sr_remappers/src/shadowhand_to_cyberglove_remapper.cpp delete mode 100644 sr_remappers/src/shadowhand_to_cyberglove_remapper_node.cpp delete mode 100644 sr_remappers/src/shadowhand_to_cybergrasp_remapper.cpp delete mode 100644 sr_remappers/src/shadowhand_to_cybergrasp_remapper_node.cpp diff --git a/cyberglove/CMakeLists.txt b/cyberglove/CMakeLists.txt deleted file mode 100644 index 417e541e..00000000 --- a/cyberglove/CMakeLists.txt +++ /dev/null @@ -1,174 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(cyberglove) - -## 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 - roslib - roslint - roscpp - rospy - std_msgs - sensor_msgs - diagnostic_msgs - genmsg - sr_cyberglove_config - cereal_port - message_generation -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED system filesystem date_time thread) - -roslint_cpp() -roslint_python() - -## 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 ## -################################################ - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -add_service_files( - FILES - Calibration.srv - Start.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 - sensor_msgs - diagnostic_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 cyberglove -CATKIN_DEPENDS roslib roscpp rospy std_msgs sensor_msgs diagnostic_msgs genmsg sr_cyberglove_config cereal_port message_runtime -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(SYSTEM ${Boost_INCLUDE_DIR}) -include_directories(include) -include_directories(${catkin_INCLUDE_DIRS}) - -## Declare a cpp library -# add_library(cyberglove -# src/${PROJECT_NAME}/cyberglove.cpp -# ) -add_library(cyberglove - src/cyberglove_publisher.cpp - src/serial_glove.cpp - src/xml_calibration_parser.cpp - src/cyberglove_service.cpp -) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(cyberglove - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_generate_messages_cpp -) - -## Specify libraries to link a library or executable target against -target_link_libraries(cyberglove - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - tinyxml -) - - -## Declare a cpp executable -add_executable(cyberglove_node - src/cyberglove_publisher.cpp - src/cyberglove_node.cpp - src/serial_glove.cpp - src/xml_calibration_parser.cpp - src/cyberglove_service.cpp -) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(cyberglove_node - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_generate_messages_cpp -) - -## Specify libraries to link a library or executable target against -target_link_libraries(cyberglove_node - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - tinyxml -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -install(TARGETS cyberglove - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/cyberglove/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -############# -## Testing ## -############# -if(CATKIN_ENABLE_TESTING) - find_package(rostest) - add_rostest_gtest(test_cyberglove - test/test_calibration.test - test/test_calibration.cpp - src/xml_calibration_parser.cpp - ) - target_link_libraries(test_cyberglove - tinyxml - ${catkin_LIBRARIES} - ${GTEST_LIBRARIES} - ${Boost_LIBRARIES} - ) - -endif() diff --git a/cyberglove/README.md b/cyberglove/README.md deleted file mode 100644 index b2530d82..00000000 --- a/cyberglove/README.md +++ /dev/null @@ -1,29 +0,0 @@ -**cyberglove** is a generic ROS interface to Immersion's Cyberglove dataglove. It reads the data from the Cyberglove, calibrate them using a calibration file and stream them to 2 different `/joint_states` topics: one for the raw data the other one for the calibrated data. There's a utility in `sr_control_gui` which can be used to generate a calibration file for a specific user in a few steps. - -If the button on the wrist is off, the glove won't publish any data. - -The calibration file can't be dynamically loaded for the time being, so if you change the calibration then don't forget to restart the cyberglove node. - -How To Use ----------- - -To run the cyberglove node, just run: - -``` -$ roslaunch cyberglove cyberglove.launch -``` - -You can specify some parameters in the launch file: - -* cyberglove_prefix The prefix to put in front of the joint_states published by the glove. -* publish_frequency The frequency at which you want to publish the data. -* path_to_glove The path to the port on which the Cyberglove is connected (usually `/dev/ttyS0`) -* path_to_calibration The path to the calibration file for the Cyberglove - -Code API --------- - -* serial_glove.h The C interface to interact with the cyberglove -* xml_calibration_parser::XmlCalibrationParser The calibration file parser. -* cyberglove_service::CybergloveService A service which can stop / start the Cyberglove publisher. -* cyberglove_publisher::CyberglovePublisher The actual publisher streaming the data from the cyberglove. diff --git a/cyberglove/include/cyberglove/cyberglove_publisher.h b/cyberglove/include/cyberglove/cyberglove_publisher.h deleted file mode 100644 index 3aefd7ea..00000000 --- a/cyberglove/include/cyberglove/cyberglove_publisher.h +++ /dev/null @@ -1,111 +0,0 @@ -/* -* @file cyberglove_publisher.h -* @author Ugo Cupcic , Contact -* @date Thu Apr 22 10:25:55 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief The goal of this ROS publisher is to publish raw and calibrated -* joint positions from the cyberglove at a regular time interval. We're -* oversampling to get a better accuracy on our data. -* To publish those data, just call the publish() -* function. -* -* -*/ - -#ifndef CYBERGLOVE_CYBERGLOVE_PUBLISHER_H_ -#define CYBERGLOVE_CYBERGLOVE_PUBLISHER_H_ - -#include -#include -#include -#include - -#include "cyberglove/serial_glove.h" - -// messages -#include -#include "cyberglove/xml_calibration_parser.h" - -namespace cyberglove -{ - -class CyberglovePublisher -{ -public: - /// Constructor - CyberglovePublisher(); - - /// Destructor - ~CyberglovePublisher(); - - ros::Publisher cyberglove_pub; - void initialize_calibration(std::string path_to_calibration); - bool isPublishing(); - void setPublishing(bool value); -private: - ///////////////// - // CALLBACKS // - ///////////////// - - // ros node handle - ros::NodeHandle node, n_tilde; - unsigned int publish_counter_max, publish_counter_index; - - // the actual connection with the cyberglove is done here. - boost::shared_ptr serial_glove; - - /* - * The callback function: called each time a full message - * is received. This function is bound to the serial_glove - * object using boost::bind. - * - * @param glove_pos A vector containing the current raw joints positions. - * @param light_on true if the light is on, false otherwise. - */ - void glove_callback(std::vector glove_pos, bool light_on); - - std::string path_to_glove; - bool publishing; - - // the calibration parser - xml_calibration_parser::XmlCalibrationParser calibration_parser; - - ros::Publisher cyberglove_raw_pub; - - sensor_msgs::JointState jointstate_msg; - sensor_msgs::JointState jointstate_raw_msg; - - void add_jointstate(float position, std::string joint_name); - - std::vector calibration_values; - - std::vector > glove_positions; - - std::string cyberglove_version_; - std::string streaming_protocol_; -}; // end class CyberglovePublisher - -} // namespace cyberglove - -#endif // CYBERGLOVE_CYBERGLOVE_PUBLISHER_H_ - -/* For the emacs weenies in the crowd. -Local Variables: - c-basic-offset: 2 -End: -*/ diff --git a/cyberglove/include/cyberglove/cyberglove_service.h b/cyberglove/include/cyberglove/cyberglove_service.h deleted file mode 100644 index dcb10912..00000000 --- a/cyberglove/include/cyberglove/cyberglove_service.h +++ /dev/null @@ -1,57 +0,0 @@ -/* -* @file cyberglove_service.h -* @author Ugo Cupcic , Contact -* @date Thu Apr 22 10:25:55 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief A service which can stop / start the Cyberglove publisher. -* -* -*/ - - -#ifndef CYBERGLOVE_CYBERGLOVE_SERVICE_H_ -#define CYBERGLOVE_CYBERGLOVE_SERVICE_H_ - -#include -#include -#include "cyberglove/cyberglove_publisher.h" -#include "cyberglove/Start.h" -#include "cyberglove/Calibration.h" -#include - - -namespace cyberglove -{ - -class CybergloveService -{ -public: - explicit CybergloveService(boost::shared_ptr publish); - ~CybergloveService() {} - - bool start(cyberglove::Start::Request &req, cyberglove::Start::Response &res); - bool calibration(cyberglove::Calibration::Request &req, cyberglove::Calibration::Response &res); -private: - ros::NodeHandle node; - boost::shared_ptr pub; - ros::ServiceServer service_start; - ros::ServiceServer service_calibration; -}; - -} // namespace cyberglove -#endif // CYBERGLOVE_CYBERGLOVE_SERVICE_H_ diff --git a/cyberglove/include/cyberglove/serial_glove.h b/cyberglove/include/cyberglove/serial_glove.h deleted file mode 100644 index ed275ba2..00000000 --- a/cyberglove/include/cyberglove/serial_glove.h +++ /dev/null @@ -1,236 +0,0 @@ -/* -* @file serial_glove.h -* @author Ugo Cupcic -* @date Thu May 5 15:30:17 2011 -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief Communicate via the serial port with the Cyberglove. -* -* -* List of available commands, gathered from an old specification: -* (http://www.cyberglovesystems.com/forum/viewtopic.php?f=14&t=247) -* - 'F0' -> stops filter -* - 'F1' -> starts filter -* - '?F' -> current filter status -* -* - 'G' -> reads data from the cyberglove once -* - 'S' -> streams data from the cyberglove at given period -* - 't 1152 1' -> specifies period of 100Hz -* - '?t' -> get period -* - '^C' -> stop streaming ?? -* -* - 'L0' -> turns light off -* - 'L1' -> turns light on -* - '?L' -> light status -* -* - 'u1' -> transmits the status each time: -* o Bit 0: cyberglove plugged in -* o Bit 1: switch on/off -* o Bit 2: light on/off -* - 'u0' -> stop transmitting the status -* -* - 'd1' -> transmits the timestamp as well -* -* - '?r' -> right handed or not -* - '?n' -> number of sensors -* - '?i' -> info about the cyberglove -* - '^I' or '^R' -> reinitialize / restart the cyberglove firmware (wait 1-2s) -* -* -* -* Proposed strategy: -* - F,1 : set filtering to 0 (faster, we're going to do oversampling anyway) -* - U,1 : transmit the status each time -* - T,1152,1: set transmit period to 100Hz -* - S : streams data -* o If data[2] = 0 (light off), ignore -* o Else (light on): read the data (start from 5: 2 first letters are 'G' and ' ', -* then we have the status bits) -*/ - -#ifndef CYBERGLOVE_SERIAL_GLOVE_H -#define CYBERGLOVE_SERIAL_GLOVE_H - -#include -#include -#include -#include -#include - -namespace cyberglove_freq -{ -/* - * This structure contains the different strings which are written - * to the serial port in order to get a given streaming frequency. - */ -struct CybergloveFreq -{ - /* - *The fastest frequency is only used when testing the maximum transmission - * speed: it's not a stable frequency for the glove. - */ - static const std::string fastest; - static const std::string hundred_hz; - static const std::string fourtyfive_hz; - static const std::string ten_hz; - static const std::string one_hz; -}; -} // namespace cyberglove_freq - -namespace cyberglove -{ -enum reception_state -{ - INITIAL, - RECEIVING_FRAME -}; - -namespace reception_16bit -{ - enum reception_state_16bit - { - SYNCHRONIZATION_1, - SYNCHRONIZATION_2, - SYNCHRONIZATION_3, - TIMESTAMP, - RECEIVING_FRAME - }; -} // namespace reception_16bit -/* - * This class uses the Cereal Port ROS package to connect to - * and interact with the Cyberglove. - */ -class CybergloveSerial -{ -public: - /* - * Initializes the connection with the cyberglove through the given serial port. - * - * @param serial_port the path to the serial port, /dev/ttyS0 by default - * @param callback a pointer to a callback function, which will be called each time a - * complete joint message is received. - */ - CybergloveSerial(std::string serial_port, std::string cyberglove_version, std::string streaming_protocol, - boost::function, bool)> callback); - ~CybergloveSerial(); - - /* - * Turns on or off the filtering (done directly in the cyberglove). By default the filtering - * is activated. We recommend turning it off if you want to do oversampling, to get the fastest - * rate (the rate is divided by 2-3 if the filtering is on) - * - * @param value true if you want to turn it on. - * - * @return 0 if success - */ - int set_filtering(bool value); - - /* - * Turns on or off the status transmission: if it's on, then a char is added to the message - * to describe the current status of the glove. For this status byte, the bit 1 corresponds - * to the button status, and the bit 2 corresponds to the light status. - * - * @param value true if you want to turn it on. - * - * @return 0 if success - */ - int set_transmit_info(bool value); - - /* - * Set the transmit frequency for the cyberglove. - * - * @param frequency use the elements of the struct cyberglove_freq::CybergloveFreq - * - * @return 0 if success - */ - int set_frequency(std::string frequency); - - /* - * Start streaming the data from the cyberglove, calling the - * callback function each time the full message is received. - * - * @return 0 if success - */ - int start_stream(); - - /* - * We keep the count of all the messages received for the glove. - * - * @return the number of received messages. - */ - int get_nb_msgs_received(); - - /* - * The number of sensors in the glove. - */ - static const uint16_t glove_size; - - /* - * The length of the timestamp in the 16bit protocol. - */ - static const uint16_t timestamp_size; - -private: - /* - * CerealPort is the ROS library used to talk - * to the serial port. - */ - boost::shared_ptr cereal_port; - - /* - * The callback function for the raw data coming from the - * serial port, bound to the cereal_port callback. The data received - * here is not received message by messages: it's a stream of data, coming - * at different intervals (the whole messages are received at a given frequency - * though) - * - * @param world a table of char containing the binary values from the serial port - * @param length the length of the received message. - */ - void stream_callback(char* world, int length); - - int nb_msgs_received, glove_pos_index, timestamp_bytes_, byte_index_; - /// A vector containing the current joints positions. - std::vector glove_positions; - - unsigned int current_value; - unsigned int sensor_value_; - - /* - * The pointer to the function called each time a full message is received. - * This function is linked when instantiating the class. - */ - boost::function, bool)> callback_function; - - bool light_on, button_on; - - // Did we get any garbage in the received message? - bool no_errors; - - std::string cyberglove_version_; - std::string streaming_protocol_; - - unsigned int reception_state_; -}; -} // namespace cyberglove - -/* For the emacs weenies in the crowd. -Local Variables: - c-basic-offset: 2 -End: -*/ - -#endif // CYBERGLOVE_SERIAL_GLOVE_H diff --git a/cyberglove/include/cyberglove/xml_calibration_parser.h b/cyberglove/include/cyberglove/xml_calibration_parser.h deleted file mode 100644 index b5838b08..00000000 --- a/cyberglove/include/cyberglove/xml_calibration_parser.h +++ /dev/null @@ -1,130 +0,0 @@ -/* -* @file xml_calibration_parser.h -* @author Ugo Cupcic -* @date Tue Apr 27 11:30:41 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief This is a simple xml parser, used to parse the calibration -* file for the cyberglove. -* A Calibration file must have this format: -* -* -* -* -* -* -* -* The calibration will be used by the glove node to stream coherent -* angles (and not an uncalibrated sensor value between 0 and 1). -* -* -*/ - - -#ifndef CYBERGLOVE_XML_CALIBRATION_PARSER_H_ -#define CYBERGLOVE_XML_CALIBRATION_PARSER_H_ - -// xml parser library -#include -#include -#include -#include - -namespace xml_calibration_parser -{ - -class XmlCalibrationParser -{ - public: - XmlCalibrationParser() {} - explicit XmlCalibrationParser(std::string path_to_calibration); - ~XmlCalibrationParser() {} - - float get_calibration_value(float position, std::string joint_name); - - struct Calibration - { - float raw_value; - float calibrated_value; - }; - - struct JointCalibration - { - std::string name; - std::vector calibrations; - }; - - - std::vector getJointsCalibrations(); - - protected: - void parse_calibration_file(TiXmlNode* pParent); - std::vector parse_joint_attributes(TiXmlNode* pParent); - - /// The vector containing the calibration - std::vector jointsCalibrations; - // use a map to easily access the value - typedef std::map > mapType; - mapType joints_calibrations_map; - - int build_calibration_table(); - - std::vector calibration_to_lookup_table(std::vector calib); - - float compute_lookup_value(int index, std::vector calib); - - float linear_interpolate(float x , float x0, float y0, float x1, float y1); - - // consts for the lookup tables - static const float lookup_precision; - static const float lookup_offset; - - /* - * rounds the given number - * - * @param number a float - * - * @return the float rounded to the closest int - */ - int round(float number); - - /* - * inline function to convert a raw position to a valid index for - * our lookup table - * - * @param raw_position the raw position (directly read from the glove) - * - * @return the calibrated value - */ - int return_index_from_raw_position(float raw_position); - - /* - * inline function to convert an index of our lookup table to a raw - * position. - * - * @param lookup_index the index in the lookup table - * - * @return the corresponding raw position - */ - static inline float return_raw_position_from_index(int lookup_index) - { - return (static_cast(lookup_index))/lookup_precision; - }; -}; // end class XmlCalibrationParser - -} // namespace xml_calibration_parser -#endif // CYBERGLOVE_XML_CALIBRATION_PARSER_H_ diff --git a/cyberglove/launch/cyberglove.launch b/cyberglove/launch/cyberglove.launch deleted file mode 100644 index e07a88d4..00000000 --- a/cyberglove/launch/cyberglove.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/cyberglove/model/cyberglove.xml b/cyberglove/model/cyberglove.xml deleted file mode 100644 index a362650c..00000000 --- a/cyberglove/model/cyberglove.xml +++ /dev/null @@ -1,140 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/cyberglove/package.xml b/cyberglove/package.xml deleted file mode 100644 index 1db13d58..00000000 --- a/cyberglove/package.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - cyberglove - 0.0.0 - This is a generic ROS interface to the Cyberglove from Immersion. It reads data from the Cyberglove, calibrate them and streams them to two different /joint_states topic: calibrated and raw data. - - - Shadow Robot's software team - - GPL - - catkin - - roslib - roslint - roscpp - rospy - std_msgs - sensor_msgs - diagnostic_msgs - genmsg - sr_cyberglove_config - cereal_port - rostest - message_generation - - roslib - roscpp - rospy - std_msgs - sensor_msgs - diagnostic_msgs - genmsg - sr_cyberglove_config - cereal_port - rostest - message_runtime - - diff --git a/cyberglove/python_lib/cyberglove_calibrer.py b/cyberglove/python_lib/cyberglove_calibrer.py deleted file mode 100644 index 5eb9c25e..00000000 --- a/cyberglove/python_lib/cyberglove_calibrer.py +++ /dev/null @@ -1,347 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright 2011, 2022 Shadow Robot Company Ltd. -# -# This program is free software: you can redistribute it and/or modify it -# under the terms of the GNU General Public License as published by the Free -# Software Foundation version 2 of the License. -# -# This program is distributed in the hope that it will be useful, but WITHOUT -# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -# more details. -# -# You should have received a copy of the GNU General Public License along -# with this program. If not, see . -# - -# pylint: disable-all - -""" -Calibration utility for the cyberglove. - -@author: Ugo Cupcic -@contact: ugo@shadowrobot.com, contact@shadowrobot.com -""" - -from __future__ import absolute_import - -import os -from builtins import input - -import roslib -import rospy -from cyberglove.srv import Calibration as CalibrationSrv -from cyberglove_library import Cyberglove - -roslib.load_manifest('sr_control_gui') - - -class Joint: - """ - A class containing simple information regarding a joint: name, and calibrated value - """ - def __init__(self, name, min=0, max=90): - """ - @param name: the name of the joint - @param min: the min value to calibrate - @param max: the max value to calibrate - """ - self.name = name - self.min = min - self.max = max - - -class CalibrationStep: - """ - A class containing all the data for a given calibration step - """ - def __init__(self, step_name="", step_description=[""], joints=[]): - """ - @param step_name: A short name for this calibration step - @param step_description: A table containing 2 descriptions: the first one describes the min pose, the second - the max - @param joints: A table containing the joints to be calibrated - - """ - self.step_name = step_name - self.step_description = step_description - self.joints = joints - - -class Calibration: - """ - A class Containing all the calibration data for a given joint. - """ - def __init__(self, raw_min=0.0, raw_max=0.0, - calibrated_min=0.0, calibrated_max=0.0, - is_calibrated=0): - self.raw_min = raw_min - self.raw_max = raw_max - self.calibrated_min = calibrated_min - self.calibrated_max = calibrated_max - self.is_calibrated = is_calibrated - - -def default_description(step_name, max=0): - """ - The default description function for a step. Just prints a text for each step. - - @param step_name: the name of the step - @param max: if 0=>we're reading the min values, if 1=> max values - """ - if max == 0: - print(f"calibrating min values for: {step_name}") - else: - print(f"calibrating max values for: {step_name}") - - -def do_nothing(): - """ - A function that does nothing. Used to have an empty description function. - """ - return - - -class CybergloveCalibrer: - """ - A utility to calibrate the cyberglove. - """ - def __init__(self, description_function=default_description): - """ - Initialize some class variables: a table containing the calibration steps, a connection to the cyberglove - library and a description function for the calibration steps - - @param description_function: specify a function you want to use to describe the calibration steps ( text / - pictures / animation / ... ). Must take a joint name as parameter. - """ - self.calibration_steps = [] - - self.cyberglove = Cyberglove() - - # fill the table containing all the joints - self.joints = {} - for name in self.cyberglove.get_joints_names(): - self.joints[name] = Calibration() - - self.get_calibration_steps() - - if description_function is None: - description_function = do_nothing - self.description_function = description_function - - def get_calibration_steps(self): - """ - Read the calibration steps from the xml file. - - @return: 0 when the values were read. - """ - - # first step: calibrate 0s, 3s and TH4 - joints1 = [Joint("G_IndexMPJ", 0, 90), Joint("G_IndexPIJ", 0, 90), Joint("G_IndexDIJ", 0, 90), - Joint("G_MiddleMPJ", 0, 90), Joint("G_MiddlePIJ", 0, 90), Joint("G_MiddleDIJ", 0, 90), - Joint("G_RingMPJ", 0, 90), Joint("G_RingPIJ", 0, 90), Joint("G_RingDIJ", 0, 90), - Joint("G_PinkieMPJ", 0, 90), Joint("G_PinkiePIJ", 0, 90), Joint("G_PinkieDIJ", 0, 90), - Joint("G_ThumbAb", 50, 0)] - self.calibration_steps.append(CalibrationStep(step_name="Joints 0s, 3s and thumb abduction (THJ4)", - step_description=["Hand flat on a table, fingers joined, thumb " + - "opened", - "Hand forming a fist, thumb closed"], - joints=joints1)) - - # second step: calibrate 4s, TH1, TH2, TH5 and WR2 - joints2 = [Joint("G_ThumbIJ", 90, 0), Joint("G_ThumbMPJ", 30, -30), - Joint("G_MiddleIndexAb", 0, 50), Joint("G_RingMiddleAb", 0, 50), Joint("G_PinkieRingAb", 0, 50), - Joint("G_WristYaw", 10, -30)] - self.calibration_steps.append(CalibrationStep(step_name="Joints 4s + TH1, 2, and WR2", - step_description=["Hand flat fingers joined, thumb completely " + - "curled under the palm, wrist 2 bent to " + - "the left", - "Hand flat fingers apart, thumb completely " + - "opened, wrist bent 2 to the right"], - joints=joints2)) - - # third step: calibrate TH5 - joints3 = [Joint("G_ThumbRotate", 60, -60)] - self.calibration_steps.append(CalibrationStep(step_name="Joint TH5", - step_description=["thumb completely under the palm", - "thumb completely opened, curled over the " + - "palm"], - joints=joints3)) - - # fourth step: calibrate LF5 and WR1 - joints4 = [Joint("G_PalmArch", 0, 40), Joint("G_WristPitch", -30, 40)] - self.calibration_steps.append(CalibrationStep(step_name="LF5 and WR1", - step_description=["Hand flat, wrist 1 bent backward", - "Palm curled (LFJ5), wrist 1 bent forward"], - joints=joints4)) - - return 0 - - def do_step_min(self, index): - """ - Run the given step of the calibration, gets the min values. - - @param index: the index of the step in the calibration file - @return: 0 when the values were read. - """ - joints = self.calibration_steps[index].joints - # display the description - self.description_function(self.calibration_steps[index].step_description[0], 0) - - for joint in joints: - name = joint.name - # read the min values - self.joints[name].raw_min = self.cyberglove.read_raw_average_value(name) - self.joints[name].calibrated_min = joint.min - # still needs to read the max before fully calibrated - self.joints[name].is_calibrated += 0.5 - return 0 - - def do_step_max(self, index): - """ - Run the given step of the calibration, gets the max values. - As this method is called after the do_step_min() method, we don't display the description - - @param index: the index of the step in the calibration file - @return: 0 when the values were read. - """ - joints = self.calibration_steps[index].joints - # display the description - self.description_function(self.calibration_steps[index].step_description[1], 0) - - for joint in joints: - name = joint.name - # read the max values - self.joints[name].raw_max = self.cyberglove.read_raw_average_value(name) - self.joints[name].calibrated_max = joint.max - # still needs to read the max before fully calibrated - self.joints[name].is_calibrated += 0.5 - return 0 - - def is_step_done(self, joint_name): - """ - Check if the joint is calibrated. - - @param joint_name: the name of the joint - @return: 1 if the joint has already been calibrated. - """ - - return self.joints[joint_name].is_calibrated - - def all_steps_done(self): - """ - Check if all the steps were processed. - - @return: True if all the steps were processed. - """ - for calib in list(self.joints.values()): - if calib.is_calibrated != 1: - return False - - return True - - def reorder_calibration(self): - """ - Reorder the calibration: set the raw_min to the min raw_value - """ - for name in list(self.joints.keys()): - if self.joints[name].raw_min > self.joints[name].raw_max: - tmp_raw = self.joints[name].raw_min - tmp_cal = self.joints[name].calibrated_min - self.joints[name].raw_min = self.joints[name].raw_max - self.joints[name].calibrated_min = self.joints[name].calibrated_max - self.joints[name].raw_max = tmp_raw - self.joints[name].calibrated_max = tmp_cal - - def write_calibration_file(self, filepath): - """ - Checks if all the steps were processed by calling self.all_steps_done() - Reorder the calibration - Then writes the whole calibration to a given file. - - @param filepath: Where to write the calibration - @return: 0 if the file has been written, - -1 if the calibration is not finished yet, - -2 if other error - """ - - # Where all the steps processed? - if not self.all_steps_done(): - return -1 - - # reorder the calibration - self.reorder_calibration() - - ############### - # Write to an xml file - ############### - # store the text in a table - text = [] - - text.append("") - text.append("") - for name in self.joints: - # joint name - text.append("") - - cal = self.joints[name] - # min value - text.append("") - - # max value - text.append("") - - text.append("") - - text.append("") - - # write the text to a file - try: - output = open(filepath, "w") - for line in text: - output.write(line+"\n") - - output.close() - except Exception: - return -2 - - return 0 - - def load_calib(self, filename): - if filename == "": - return -1 - - rospy.wait_for_service('/cyberglove/calibration') - try: - calib = rospy.ServiceProxy('cyberglove/calibration', CalibrationSrv) - path = filename.encode("iso-8859-1") - resp = calib(path) - return resp.state - except rospy.ServiceException as e: - print('Failed to call start service') - return -2 - - -############## -# MAIN # -############## -def main(): - cyber_calib = CybergloveCalibrer() - for i in range(0, len(cyber_calib.calibration_steps)): - input(cyber_calib.calibration_steps[i].step_description[0]) - cyber_calib.do_step_min(i) - input(cyber_calib.calibration_steps[i].step_description[1]) - - cyber_calib.do_step_max(i) - error = cyber_calib.write_calibration_file("../../param/cyberglove.cal") - print(error) - - return 0 - - -# start the script -if __name__ == "__main__": - main() diff --git a/cyberglove/python_lib/cyberglove_library.py b/cyberglove/python_lib/cyberglove_library.py deleted file mode 100644 index a683fca7..00000000 --- a/cyberglove/python_lib/cyberglove_library.py +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright 2011, 2022 Shadow Robot Company Ltd. -# -# This program is free software: you can redistribute it and/or modify it -# under the terms of the GNU General Public License as published by the Free -# Software Foundation version 2 of the License. -# -# This program is distributed in the hope that it will be useful, but WITHOUT -# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -# more details. -# -# You should have received a copy of the GNU General Public License along -# with this program. If not, see . -# - -# pylint: disable-all - -from __future__ import absolute_import, division - -import threading -import time - -import rosgraph.masterapi -import roslib -import rospy -from sensor_msgs.msg import JointState - -roslib.load_manifest('cyberglove') - - -class Joint(): - def __init__(self, name="", motor="", min=0, max=90): - self.name = name - self.motor = motor - self.min = min - self.max = max - - -class Cyberglove: - """ - Interface to the Cyberglove publisher. - """ - def __init__(self, max_values=2): - self.joints = {"G_ThumbRotate": Joint(), - "G_ThumbMPJ": Joint(), - "G_ThumbIJ": Joint(), - "G_ThumbAb": Joint(), - "G_IndexMPJ": Joint(), - "G_IndexPIJ": Joint(), - "G_IndexDIJ": Joint(), - "G_MiddleMPJ": Joint(), - "G_MiddlePIJ": Joint(), - "G_MiddleDIJ": Joint(), - "G_MiddleIndexAb": Joint(), - "G_RingMPJ": Joint(), - "G_RingPIJ": Joint(), - "G_RingDIJ": Joint(), - "G_RingMiddleAb": Joint(), - "G_PinkieMPJ": Joint(), - "G_PinkiePIJ": Joint(), - "G_PinkieDIJ": Joint(), - "G_PinkieRingAb": Joint(), - "G_PalmArch": Joint(), - "G_WristPitch": Joint(), - "G_WristYaw": Joint()} - - self.raw_messages = [] - self.calibrated_messages = [] - self.max_values = max_values - self.map = {} - self.hasglove = 0 - self.isFirstMessage = True - self.liste = 0 - self.raw = rospy.Subscriber('/cyberglove/raw/joint_states', JointState, self.callback_raw) - self.calibrated = rospy.Subscriber('/cyberglove/calibrated/joint_states', JointState, self.callback_calibrated) - threading.Thread(None, rospy.spin) - if self.has_glove(): - time.sleep(1.0) - self.createMap() - else: - raise Exception("No glove found") - - def callback_raw(self, data): - """ - Adds the last values received to the list of raw values - @param data: the message which called the callback - """ - self.addValue(self.raw_messages, data) - - def callback_calibrated(self, data): - """ - Adds the last values received to the list of calibrated values - @param data: the message which called the callback - """ - self.addValue(self.calibrated_messages, data) - - def addValue(self, vector, value): - """ - Fills a vector with the received values, and replaces the old values - when the vector is full - @param vector : the vector to fill (raw or calibrated) - @param value : the value to add - """ - if len(vector) < self.max_values: - vector.append(value) - else: - vector.pop(0) - vector.append(value) - - def createMap(self): - """ - Maps the name of the joints to their index in the message - """ - for index in range(0, len(self.raw_messages[0].name)): - self.map[self.raw_messages[0].name[index]] = index - - def read_raw_average_value(self, joint_name): - """ - return the raw value of a given joint - - @param joint_name: the name of the glove of the Cyberglove - """ - raw_value = 0 - joint_index = self.map[joint_name] - for index in range(0, len(self.raw_messages)): - raw_value = raw_value + self.raw_messages[index].position[joint_index] - - raw_value = raw_value / len(self.raw_messages) - - return raw_value - - def read_calibrated_average_value(self, joint_name): - """ - return the current positions for the given joint_name - - @param joint_name: the name of the joint - @return: the corresponding position - """ - - calibrated_value = 0 - - joint_index = self.map[joint_name] - for index in range(0, len(self.calibrated_messages)): - calibrated_value = calibrated_value + self.calibrated_messages[index].position[joint_index] - - calibrated_value = calibrated_value / len(self.calibrated_messages) - - return calibrated_value - - def get_joints_names(self): - """ - Return an array containing the Cyberglove joints names - - @return: the joints names array - """ - return list(self.joints.keys()) - - def has_glove(self): - """ - @return: True if a cyberglove is detected by ROS - """ - if not self.hasglove == 0: - return self.hasglove - self.hasglove = False - if self.liste == 0: - master = rosgraph.masterapi.Master('/rostopic') - self.liste = master.getPublishedTopics('/') - for topic_typ in self.liste: - for topic in topic_typ: - if '/cyberglove' in topic: - self.hasglove = True - return self.hasglove diff --git a/cyberglove/python_lib/cyberglove_mapper.py b/cyberglove/python_lib/cyberglove_mapper.py deleted file mode 100644 index 58d949b7..00000000 --- a/cyberglove/python_lib/cyberglove_mapper.py +++ /dev/null @@ -1,379 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright 2011, 2022 Shadow Robot Company Ltd. -# -# This program is free software: you can redistribute it and/or modify it -# under the terms of the GNU General Public License as published by the Free -# Software Foundation version 2 of the License. -# -# This program is distributed in the hope that it will be useful, but WITHOUT -# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -# more details. -# -# You should have received a copy of the GNU General Public License along -# with this program. If not, see . -# - -# pylint: disable-all - -""" -Minimizes the mapping matrix using a simplex algorithm - -@author: Ugo Cupcic -@contact: ugo@shadowrobot.com, contact@shadowrobot.com -""" - -from __future__ import absolute_import - -from builtins import input - -import roslib -import rospy -from cyberglove_library import Cyberglove -from scipy.optimize import fmin - -roslib.load_manifest('sr_control_gui') - - -class MappingConfiguration: - def __init__(self, glove_data=[], hand_data=[], description=""): - self.glove_data = glove_data - self.hand_data = hand_data - self.description = description - - def copy(self): - tmp = MappingConfiguration(self.glove_data, self.hand_data, self.description) - return tmp - - -class MappingMinimizer: - def __init__(self, verbose=1): - rospy.init_node("cyberglove_mapper_minimizer") - # connect to the cyberglove - self.cyberglove = Cyberglove() - - self.verbose = verbose - - glove_sensors = ["G_ThumbRotate", "G_ThumbMPJ", "G_ThumbIJ", "G_ThumbAb", "G_IndexMPJ", "G_IndexPIJ", - "G_IndexDIJ", "G_MiddleMPJ", "G_MiddlePIJ", "G_MiddleDIJ", "G_MiddleIndexAb", "G_RingMPJ", - "G_RingPIJ", "G_RingDIJ", "G_RingMiddleAb", "G_PinkieMPJ", "G_PinkiePIJ", "G_PinkieDIJ", - "G_PinkieRingAb", "G_PalmArch", "G_WristPitch", "G_WristYaw"] - - index = 0 - self.glove_name_index_map = {} - for sensor in glove_sensors: - self.glove_name_index_map[sensor] = index - index += 1 - - # fill the table containing all the joints - hand_joints = ["TH1", "TH2", "TH3", "TH4", "TH5", "FF0", "FF3", "FF4", "MF0", "MF3", "MF4", "RF0", "RF3", "RF4", - "LF0", "LF3", "LF4", "LF5", "WR1", "WR2"] - - index = 0 - self.hand_name_index_map = {} - for sensor in hand_joints: - self.hand_name_index_map[sensor] = index - index += 1 - - self.thumb_glove = {"G_ThumbRotate": 0, - "G_ThumbMPJ": 1, - "G_ThumbAb": 2} - - self.thumb_hand = {"TH2": 0, - "TH4": 1, - "TH5": 2} - - self.configurations = [] - self.initialise_configurations() - - self.simplex_iteration_index = 0 - - if(self.verbose == 1): - for conf in self.configurations: - print("-------") - print(conf.description) - print(conf.glove_data) - print(conf.hand_data) - print("-------") - print("") - - self.minerror = 1000000 - self.maxerror = 0 - - def initialise_configurations(self): - configuration = MappingConfiguration() - - ### - # First configuration - configuration.description = "All fingers and thumb are curved maximum in a punch form ... " - configuration.glove_data = [] - # corresponding data for the hand - configuration.hand_data = [[30.0, 52.0, 5.0], - [30.0, 43.0, 0.0], - [30.0, 68.0, 16.0] - ] - - self.configurations.append(configuration.copy()) - - ### - # Second configuration - configuration.description = "Thumb touching first finger tip ... " - configuration.glove_data = [] - # corresponding data for the hand - configuration.hand_data = [[30.0, 54.0, -5.0], - [19.0, 58.0, 28.0]] - - self.configurations.append(configuration.copy()) - - ### - # Third configuration - configuration.description = "Thumb touching middle finger tip ... " - configuration.glove_data = [] - # corresponding data for the hand - configuration.hand_data = [[30.0, 64.0, 9.0], - [1.0, 68.0, 29.0]] - - self.configurations.append(configuration.copy()) - - ### - # Fourth configuration - configuration.description = "Thumb touching ring finger tip ... " - configuration.glove_data = [] - # corresponding data for the hand - configuration.hand_data = [[30.0, 75.0, 19.0], - [1.0, 75.0, 42.0]] - - self.configurations.append(configuration.copy()) - - ### - # Fifth configuration - configuration.description = "Thumb touching little finger tip ... " - configuration.glove_data = [] - # corresponding data for the hand - configuration.hand_data = [[30.0, 75.0, 3.0], - [1.0, 75.0, 35.0]] - - self.configurations.append(configuration.copy()) - - ### - # Sixth configuration - configuration.description = "Thumb touching all the finger tips ... " - configuration.glove_data = [] - # corresponding data for the hand - configuration.hand_data = [[-30.0, 75.0, 60.0], - [-7.0, 63.0, 60.0]] - - self.configurations.append(configuration.copy()) - - ### - # Seventh configuration - configuration.description = "hand flat, thumb relaxed along the fingers " - configuration.glove_data = [] - # corresponding data for the hand - configuration.hand_data = [[0.0, 0.0, -60.0]] - - self.configurations.append(configuration.copy()) - - ### - # Eigth configuration - configuration.description = "hand flat, thumb opened " - configuration.glove_data = [] - # corresponding data for the hand - configuration.hand_data = [[0.0, 75.0, -60.0]] - - self.configurations.append(configuration.copy()) - - def evaluation_function(self, matrix): - """ - The error is computed by adding the square error for each configuration. - """ - error = 0 - - # rebuild the matrix as a matrix, not a vector (the implementation of the simplex in scipy works on vectors - matrix_tmp = [] - for i in range(len(self.thumb_glove)): - line = [] - for j in range(len(self.thumb_hand)): - line.append(matrix[i*len(self.thumb_hand)+j]) - matrix_tmp.append(line) - - matrix = matrix_tmp - computed_vector_hand = [] - - for config in self.configurations: - # get the hand vectors from the glove data - for vec_glove in config.glove_data: - computed_vector_hand.append(self.multiply(vec_glove, matrix)) - - # compute the square error - for vec_comp, vec_hand in zip(computed_vector_hand, config.hand_data): - for computed_hand_data in vec_comp: - for hand_data in vec_hand: - error += (computed_hand_data-hand_data)*(computed_hand_data-hand_data) - - if self.verbose == 1: - print("error: "+str(error)) - - if error < self.minerror: - self.minerror = error - - if error > self.maxerror: - self.maxerror = error - - return error - - def multiply(self, vector_glove, matrix): - """ - multiply the vector by the matrix. Returns a vector. - """ - vector_hand = [] - index_col = 0 - print(vector_glove) - print(matrix) - for glove_data in vector_glove: - data = 0 - for line in matrix: - data += (glove_data*line[index_col]) - vector_hand.append(data) - index_col += 1 - - return vector_hand - - def callback(self, xk): - """ - Function called at each iteration - """ - self.simplex_iteration_index += 1 - if self.simplex_iteration_index % 50 == 0: - print("-------------------------") - print("iteration number: " + str(self.simplex_iteration_index)) - # print xk - - def minimize(self): - start = [] - for i in range(0, len(self.thumb_hand)): - line = [] - for j in range(0, len(self.thumb_glove)): - line.append(0.1) - start.append(line) - - xopt = fmin(self.evaluation_function, start, callback=self.callback, maxiter=5000) - - # rebuild the result as a matrix, not a vector (the implementation of the simplex in scipy works on vectors - output = [] - for i in range(len(self.thumb_glove)): - line = [] - for j in range(len(self.thumb_hand)): - line.append(xopt[i*len(self.thumb_hand)+j]) - output.append(line) - - print("min error: " + str(self.minerror)) - print("max error: " + str(self.maxerror)) - - return output - - def write_full_mapping(self, output_path="../../../param/GloveToHandMappings"): - """ - Writes the mapping matrix to a file: - the glove values are the lines, the hand values are the columns - """ - - # initialise the matrix with 0s - mapping_matrix = [] - for i in range(0, len(self.glove_name_index_map)): - line = [] - for j in range(0, len(self.hand_name_index_map)): - line.append(0.0) - mapping_matrix.append(line) - - # TH3 is always 0, RF4 as well - # fill the matrix with the known values + th1 (all except the thumb) - mapping_matrix[self.glove_name_index_map["G_IndexDIJ"]][self.hand_name_index_map["FF0"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_IndexPIJ"]][self.hand_name_index_map["FF0"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_IndexMPJ"]][self.hand_name_index_map["FF3"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_MiddleIndexAb"]][self.hand_name_index_map["FF4"]] = -1.0 - - mapping_matrix[self.glove_name_index_map["G_MiddleDIJ"]][self.hand_name_index_map["MF0"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_MiddlePIJ"]][self.hand_name_index_map["MF0"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_MiddleMPJ"]][self.hand_name_index_map["MF3"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_RingMiddleAb"]][self.hand_name_index_map["MF4"]] = -1.0 - - mapping_matrix[self.glove_name_index_map["G_RingDIJ"]][self.hand_name_index_map["RF0"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_RingPIJ"]][self.hand_name_index_map["RF0"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_RingMPJ"]][self.hand_name_index_map["RF3"]] = 1.0 - - mapping_matrix[self.glove_name_index_map["G_PinkieDIJ"]][self.hand_name_index_map["LF0"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_PinkiePIJ"]][self.hand_name_index_map["LF0"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_PinkieMPJ"]][self.hand_name_index_map["LF3"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_PinkieRingAb"]][self.hand_name_index_map["LF4"]] = -1.0 - mapping_matrix[self.glove_name_index_map["G_PalmArch"]][self.hand_name_index_map["LF5"]] = 1.0 - - mapping_matrix[self.glove_name_index_map["G_WristPitch"]][self.hand_name_index_map["WR1"]] = 1.0 - mapping_matrix[self.glove_name_index_map["G_WristYaw"]][self.hand_name_index_map["WR2"]] = 1.0 - - mapping_matrix[self.glove_name_index_map["G_ThumbIJ"]][self.hand_name_index_map["TH1"]] = 1.0 - - # mapping_matrix[self.glove_name_index_map["G_ThumbAb"]][self.hand_name_index_map["TH4"]] = 1.0 - # mapping_matrix[self.glove_name_index_map["G_ThumbRotate"]][self.hand_name_index_map["TH5"]] = 1.0 - # mapping_matrix[self.glove_name_index_map["G_ThumbMPJ"]][self.hand_name_index_map["TH2"]] = 1.0 - - # compute the best mapping for the thumb except th1 - thumb_mapping = self.minimize() - - # fill the matrix with the thumb values computed with the simplex - for glove_name in list(self.thumb_glove.keys()): - for hand_name in list(self.thumb_hand.keys()): - # indexes in the computed thumb mapping matrix - tmp_index_glove = self.thumb_glove[glove_name] - tmp_index_hand = self.thumb_hand[hand_name] - - # retrieve the mapping value for those sensors - mapping_value = thumb_mapping[tmp_index_glove][tmp_index_hand] - - # indexes for the whole mapping matrix - final_index_glove = self.glove_name_index_map[glove_name] - final_index_hand = self.hand_name_index_map[hand_name] - - # update matrix - mapping_matrix[final_index_glove][final_index_hand] = mapping_value - # write the matrix to a file - file = open(output_path, "w") - for line in mapping_matrix: - for col in line: - file.write(" "+str(col)) - file.write("\n") - - file.close() - - def record(self, config): - for i in range(0, 2): - input(str(i) + ": " + config.description) - vector_data = [0] * len(self.thumb_glove) - for sensor_name in list(self.thumb_glove.keys()): - data = self.cyberglove.read_calibrated_average_value(sensor_name) - vector_data[self.thumb_glove[sensor_name]] = data - config.glove_data.append(vector_data) - - if(self.verbose == 1): - print("read values:") - print(config.glove_data) - - -############## -# MAIN # -############## -def main(): - cyber_mapper = MappingMinimizer() - - for config in cyber_mapper.configurations: - cyber_mapper.record(config) - - cyber_mapper.write_full_mapping() - - return 0 - - -# start the script -if __name__ == "__main__": - main() diff --git a/cyberglove/src/cyberglove_node.cpp b/cyberglove/src/cyberglove_node.cpp deleted file mode 100644 index 76254057..00000000 --- a/cyberglove/src/cyberglove_node.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/* -* @file cyberglove_node.cpp -* @author Ugo Cupcic -* @date Thu Apr 22 10:21:50 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief The cyberglove node publishes data collected from a -* Cyberglove. -* -* -*/ - -#include -#include -#include "cyberglove/cyberglove_publisher.h" -#include "cyberglove/cyberglove_service.h" -#include "cyberglove/Start.h" -#include - -/* -* Start the cyberglove publisher. -* -* @param argc -* @param argv -* -* @return -1 if error (e.g. no glove found) -*/ -int main(int argc, char** argv) -{ - ros::init(argc, argv, "cyberglove_publisher"); - boost::shared_ptr cyberglove_pub(new cyberglove::CyberglovePublisher()); - - cyberglove::CybergloveService service(cyberglove_pub); - - ros::spin(); - - return 0; -} - - -/* For the emacs weenies in the crowd. -Local Variables: - c-basic-offset: 2 -End: -*/ diff --git a/cyberglove/src/cyberglove_publisher.cpp b/cyberglove/src/cyberglove_publisher.cpp deleted file mode 100644 index 5f4c338e..00000000 --- a/cyberglove/src/cyberglove_publisher.cpp +++ /dev/null @@ -1,253 +0,0 @@ -/* -* @file shadowhand_publisher.cpp -* @author Ugo Cupcic -* @date Thu Mar 25 15:36:41 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief The goal of this ROS publisher is to publish raw and calibrated -* joint positions from the cyberglove at a regular time interval. We're -* oversampling to get a better accuracy on our data. -* -* -*/ - -#include -#include -#include -#include -#include "cyberglove/cyberglove_publisher.h" - -namespace cyberglove -{ - - CyberglovePublisher::CyberglovePublisher() - : n_tilde("~"), publish_counter_max(0), publish_counter_index(0), - path_to_glove("/dev/ttyS0"), publishing(true) - { - std::string path_to_calibration; - n_tilde.param("path_to_calibration", path_to_calibration, std::string("/etc/robot/calibration.d/cyberglove.cal")); - ROS_INFO("Calibration file loaded for the Cyberglove: %s", path_to_calibration.c_str()); - - initialize_calibration(path_to_calibration); - - // set sampling frequency - double sampling_freq; - n_tilde.param("sampling_frequency", sampling_freq, 100.0); - - // set publish_counter: the number of data we'll average - // before publishing. - double publish_freq; - n_tilde.param("publish_frequency", publish_freq, 20.0); - publish_counter_max = static_cast(sampling_freq / publish_freq); - - ROS_INFO_STREAM("Sampling at " << sampling_freq << "Hz ; Publishing at " - << publish_freq << "Hz ; Publish counter: "<< publish_counter_max); - - // Get the cyberglove version '2' or '3' - n_tilde.param("cyberglove_version", cyberglove_version_, std::string("2")); - ROS_INFO("Cyberglove version: %s", cyberglove_version_.c_str()); - - // Get the cyberglove streaming protocol '8bit' or '16bit' - n_tilde.param("streaming_protocol", streaming_protocol_, std::string("8bit")); - ROS_INFO("Streaming protocol: %s", streaming_protocol_.c_str()); - - // set path to glove - n_tilde.param("path_to_glove", path_to_glove, std::string("/dev/ttyS0")); - ROS_INFO("Opening glove on port: %s", path_to_glove.c_str()); - - // initialize the connection with the cyberglove and binds the callback function - serial_glove = boost::shared_ptr(new CybergloveSerial(path_to_glove, cyberglove_version_, - streaming_protocol_, boost::bind(&CyberglovePublisher::glove_callback, this, _1, _2))); - - int res = -1; - if (cyberglove_version_ == "2") - { - cyberglove_freq::CybergloveFreq frequency; - - switch (static_cast(sampling_freq)) - { - case 100: - res = serial_glove->set_frequency(frequency.hundred_hz); - break; - case 45: - res = serial_glove->set_frequency(frequency.fourtyfive_hz); - break; - case 10: - res = serial_glove->set_frequency(frequency.ten_hz); - break; - case 1: - res = serial_glove->set_frequency(frequency.one_hz); - break; - default: - res = serial_glove->set_frequency(frequency.hundred_hz); - break; - } - - // We want the glove to transmit the status (light on/off) - res = serial_glove->set_transmit_info(true); - } - // Should the glove filter the data? (it leads to less smooth movements, but quieter behaviour on the motors) - bool filtering; - n_tilde.param("filter", filtering, false); - std::string filt_msg(filtering?"ON":"OFF"); - ROS_INFO("Filtering: %s", filt_msg.c_str()); - res = serial_glove->set_filtering(filtering); - - // publishes calibrated JointState messages - std::string prefix; - std::string searched_param; - n_tilde.searchParam("cyberglove_prefix", searched_param); - n_tilde.param(searched_param, prefix, std::string()); - std::string full_topic = prefix + "/calibrated/joint_states"; - cyberglove_pub = n_tilde.advertise(full_topic, 2); - - // publishes raw JointState messages - n_tilde.searchParam("cyberglove_prefix", searched_param); - n_tilde.param(searched_param, prefix, std::string()); - full_topic = prefix + "/raw/joint_states"; - cyberglove_raw_pub = n_tilde.advertise(full_topic, 2); - - // initialises joint names (the order is important) - jointstate_msg.name.push_back("G_ThumbRotate"); - jointstate_msg.name.push_back("G_ThumbMPJ"); - jointstate_msg.name.push_back("G_ThumbIJ"); - jointstate_msg.name.push_back("G_ThumbAb"); - jointstate_msg.name.push_back("G_IndexMPJ"); - jointstate_msg.name.push_back("G_IndexPIJ"); - jointstate_msg.name.push_back("G_IndexDIJ"); - jointstate_msg.name.push_back("G_MiddleMPJ"); - jointstate_msg.name.push_back("G_MiddlePIJ"); - jointstate_msg.name.push_back("G_MiddleDIJ"); - jointstate_msg.name.push_back("G_MiddleIndexAb"); - jointstate_msg.name.push_back("G_RingMPJ"); - jointstate_msg.name.push_back("G_RingPIJ"); - jointstate_msg.name.push_back("G_RingDIJ"); - jointstate_msg.name.push_back("G_RingMiddleAb"); - jointstate_msg.name.push_back("G_PinkieMPJ"); - jointstate_msg.name.push_back("G_PinkiePIJ"); - jointstate_msg.name.push_back("G_PinkieDIJ"); - jointstate_msg.name.push_back("G_PinkieRingAb"); - jointstate_msg.name.push_back("G_PalmArch"); - jointstate_msg.name.push_back("G_WristPitch"); - jointstate_msg.name.push_back("G_WristYaw"); - - jointstate_raw_msg.name = jointstate_msg.name; - - // start reading the data. - res = serial_glove->start_stream(); - } - - CyberglovePublisher::~CyberglovePublisher() - { - } - - void CyberglovePublisher::initialize_calibration(std::string path_to_calibration) - { - calibration_parser = xml_calibration_parser::XmlCalibrationParser(path_to_calibration); - } - - bool CyberglovePublisher::isPublishing() - { - if (publishing) - { - return true; - } - else - { - return false; - } - } - - void CyberglovePublisher::setPublishing(bool value) - { - publishing = value; - } - - ///////////////////////////////// - // CALLBACK METHOD // - ///////////////////////////////// - void CyberglovePublisher::glove_callback(std::vector glove_pos, bool light_on) - { - // if the light is off, we don't publish any data. - if (!light_on) - { - publishing = false; - ROS_DEBUG("The glove button is off, no data will be read / sent"); - ros::spinOnce(); - return; - } - publishing = true; - - // appends the current position to the vector of position - glove_positions.push_back(glove_pos); - - publish_counter_index += 1; - - // if we've enough samples, publish the data: - if (publish_counter_index == publish_counter_max) - { - // reset the messages - jointstate_msg.position.clear(); - jointstate_msg.velocity.clear(); - jointstate_raw_msg.position.clear(); - jointstate_raw_msg.velocity.clear(); - jointstate_raw_msg.header.stamp = ros::Time::now(); - - // fill the joint_state msg with the averaged glove data - for (unsigned int index_joint = 0; index_joint < CybergloveSerial::glove_size; ++index_joint) - { - // compute the average over the samples for the current joint - float averaged_value = 0.0f; - for (unsigned int index_sample = 0; index_sample < publish_counter_max; ++index_sample) - { - averaged_value += glove_positions[index_sample][index_joint]; - } - averaged_value /= publish_counter_max; - - jointstate_raw_msg.position.push_back(averaged_value); - add_jointstate(averaged_value, jointstate_msg.name[index_joint]); - } - - // publish the msgs - cyberglove_pub.publish(jointstate_msg); - cyberglove_raw_pub.publish(jointstate_raw_msg); - - publish_counter_index = 0; - glove_positions.clear(); - } - ros::spinOnce(); - } - - void CyberglovePublisher::add_jointstate(float position, std::string joint_name) - { - // get the calibration value - float calibration_value = calibration_parser.get_calibration_value(position, joint_name); - // publish the glove position - jointstate_msg.position.push_back(calibration_value); - // set velocity to 0. - // @TODO : send the correct velocity ? - jointstate_msg.velocity.push_back(0.0); - } -} // namespace cyberglove - - - -/* For the emacs weenies in the crowd. -Local Variables: - c-basic-offset: 2 -End: -*/ diff --git a/cyberglove/src/cyberglove_service.cpp b/cyberglove/src/cyberglove_service.cpp deleted file mode 100644 index 93b13699..00000000 --- a/cyberglove/src/cyberglove_service.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* -* @file cyberglove_service.cpp -* @author Ugo Cupcic -* @date Tue Mar 29 15:04:02 2011 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief Adding services to the cyberglove, to be able to reload the -* calibration mostly. -* -* -*/ - -#include -#include -#include - -#include "cyberglove/cyberglove_publisher.h" -#include "cyberglove/cyberglove_service.h" - -namespace cyberglove -{ - -CybergloveService::CybergloveService(boost::shared_ptr publish) - : node("~"), pub(publish) -{ - service_start = node.advertiseService("start", &CybergloveService::start, this); - service_calibration = node.advertiseService("calibration", &CybergloveService::calibration, this); - ROS_INFO("Listening for service"); -} - -bool CybergloveService::start(cyberglove::Start::Request &req, cyberglove::Start::Response &res) -{ - if (req.start) - { - ROS_INFO("Glove is now publishing"); - this->pub->setPublishing(true); - } - else - { - ROS_INFO("Glove has stopped publishing"); - this->pub->setPublishing(false); - } - return true; -} -bool CybergloveService::calibration(cyberglove::Calibration::Request &req, cyberglove::Calibration::Response &res) -{ - this->pub->setPublishing(false); - this->pub->initialize_calibration(req.path); - this->pub->setPublishing(true); - return true; -} -} // namespace cyberglove diff --git a/cyberglove/src/serial_glove.cpp b/cyberglove/src/serial_glove.cpp deleted file mode 100644 index 74ab2fc7..00000000 --- a/cyberglove/src/serial_glove.cpp +++ /dev/null @@ -1,397 +0,0 @@ -/* -* @file serial_glove.cpp -* @author Ugo Cupcic -* @date Thu May 5 15:30:17 2011 -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief Communicate via the serial port with the Cyberglove. -* -*/ - -#include "cyberglove/serial_glove.h" - -#include -#include -#include -#include -#include - -#define OUTPUT_CHARS 0 - -namespace cyberglove_freq -{ - const std::string CybergloveFreq::fastest = "t 1152 0\r"; // fastest speed, just for testing - const std::string CybergloveFreq::hundred_hz = "t 1152 1\r"; // 100Hz - const std::string CybergloveFreq::fourtyfive_hz = "t 2560 1\r"; // 45Hz - const std::string CybergloveFreq::ten_hz = "t 11520 1\r"; // 10Hz - const std::string CybergloveFreq::one_hz = "t 57600 2\r"; // 1Hz -} - -namespace cyberglove -{ - const uint16_t CybergloveSerial::glove_size = 22; - const uint16_t CybergloveSerial::timestamp_size = 14; - - CybergloveSerial::CybergloveSerial(std::string serial_port, std::string cyberglove_version, - std::string streaming_protocol, boost::function, bool)> callback) : - nb_msgs_received(0), glove_pos_index(0), timestamp_bytes_(0), byte_index_(0), current_value(0), sensor_value_(0), - light_on(true), button_on(true), no_errors(true), - cyberglove_version_(cyberglove_version), reception_state_(INITIAL), streaming_protocol_(streaming_protocol) - { - // initialize the vector of positions with 0s - for (int i = 0; i < glove_size; ++i) - { - glove_positions.push_back(0); - } - - // open the serial port - cereal_port = boost::shared_ptr(new cereal::CerealPort()); - cereal_port->open(serial_port.c_str()); - - // set the callback function - callback_function = callback; - } - - CybergloveSerial::~CybergloveSerial() - { - cereal_port->stopStream(); - // stop the cyberglove transmission - cereal_port->write("^c", 2); - } - - int CybergloveSerial::set_filtering(bool value) - { - char aux[30]; - aux[0] = 'F'; - if (value ) // Filtering will be on - { - aux[1] = 0x01; - cereal_port->write(aux, 2); - std::cout << " - Data filtered" << std::endl; - } - else // Filtering off - { - aux[1] = 0x00; - cereal_port->write(aux, 2); - std::cout << " - Data not filtered" << std::endl; - } - cereal_port->flush(); - - // wait for the command to be applied - sleep(1); - - return 0; - } - - int CybergloveSerial::set_transmit_info(bool value) - { - if (value ) // transmit info will be on - { - cereal_port->write("u 1\r", 4); - std::cout << " - Additional info transmitted" << std::endl; - } - else // transmit info off - { - cereal_port->write("u 0\r", 4); - std::cout << " - Additional info not transmitted" << std::endl; - } - cereal_port->flush(); - - // wait for the command to be applied - sleep(1); - - return 0; - } - - int CybergloveSerial::set_frequency(std::string frequency) - { - cereal_port->write(frequency.c_str(), frequency.size()); - cereal_port->flush(); - - // wait for the command to be applied - sleep(1); - return 0; - } - - int CybergloveSerial::start_stream() - { - std::cout << "starting stream" << std::endl; - - cereal_port->startReadStream(boost::bind(&CybergloveSerial::stream_callback, this, _1, _2)); - - if ((cyberglove_version_ == "3") && (streaming_protocol_ == "16bit")) - { - // enable USB streaming - cereal_port->write("1eu", 3); - cereal_port->flush(); - // start streaming by writing 1S to the serial port - cereal_port->write("1S", 2); - cereal_port->flush(); - } - else - { - // start streaming by writing S to the serial port - cereal_port->write("S", 1); - cereal_port->flush(); - } - - return 0; - } - - void CybergloveSerial::stream_callback(char* world, int length) - { - // read each received char. - for (int i = 0; i < length; ++i) - { - current_value = (unsigned int)(unsigned char)world[i]; - if (OUTPUT_CHARS) - { - std::cout << "Receiving state: " << reception_state_ << ", Value: " << std::setw(3) << current_value << - ", Character: " << world[i] <<"\n"; - } - - if ((cyberglove_version_ == "3") && (streaming_protocol_ == "16bit")) - { - char aux[30]; -// sprintf(aux, "0x%X", current_value); -// std::cout << aux << std::endl; - switch (reception_state_) - { - case reception_16bit::SYNCHRONIZATION_1: - switch (current_value ) - { - // the data set starts after 0xd 0xa 0x0, it starts with the time + sample index in the format - // HH:MM:SS:ss:n'S' where ss is a number from 1 to 30 indicating the index of the sample (if the sampling - // frequency is 30 Hz) - // the n is an index referring to the multiplier index (0-2 if the multiplier is 3) - // This is followed by the sensors values (2 bytes per sensor) - case 0x0D: - reception_state_ = reception_16bit::SYNCHRONIZATION_2; - break; - } - break; - case reception_16bit::SYNCHRONIZATION_2: - switch (current_value ) - { - case 0x0A: - reception_state_ = reception_16bit::SYNCHRONIZATION_3; - break; - default: - reception_state_ = reception_16bit::SYNCHRONIZATION_1; - break; - } - break; - case reception_16bit::SYNCHRONIZATION_3: - switch (current_value ) - { - case 0x00: - timestamp_bytes_ = 0; - reception_state_ = reception_16bit::TIMESTAMP; - break; - default: - reception_state_ = reception_16bit::SYNCHRONIZATION_1; - break; - } - break; - case reception_16bit::TIMESTAMP: - timestamp_bytes_++; - // special case observed: sometimes after D A 0 sequence we get n'S' instead of directly the time - // another case observed is e1S, so checking for any S coming before time - if ((timestamp_bytes_ < timestamp_size) && (current_value == 'S')) - { - timestamp_bytes_ = 0; - } - if (timestamp_bytes_ == timestamp_size) - { - if (current_value == 'S') - { - ++nb_msgs_received; - // reset the index to 0 - glove_pos_index = 0; - byte_index_ = 0; - // reset no_errors to true for the new message - no_errors = true; - reception_state_ = reception_16bit::RECEIVING_FRAME; - } - else - { - std::cout << "Sync error. Not an S: Reset frame" << std::endl; - reception_state_ = reception_16bit::SYNCHRONIZATION_1; - } - } - break; - case reception_16bit::RECEIVING_FRAME: - if (byte_index_) - { - sensor_value_ += current_value; - // the values sent by the glove are in the range [1;4094] (12 bit ADC) - // -> we convert them to float in the range [0;1] -// char aux[30]; -// sprintf(aux, "%u", sensor_value_); -// std::cout << aux << std::endl; - if (sensor_value_ > 0x0FFF) - { - snprintf(aux, sizeof(aux), "%u", sensor_value_); - - std::cout << "bad sensor value: " << aux << " Reset frame" << std::endl; - reception_state_ = reception_16bit::SYNCHRONIZATION_1; - break; - } - - glove_positions[glove_pos_index] = ((static_cast(sensor_value_)) - 1.0f) / - static_cast(0x0FFF - 1); - ++glove_pos_index; - byte_index_ = 0; - } - else - { - sensor_value_ = current_value << 8; - byte_index_ = 1; - } - // this is a joint data from the glove - // the value in the message should never be 0. - if ((byte_index_ == 0) && (sensor_value_ == 0)) - { - no_errors = false; - std::cout << "error detected" << std::endl; - } - - if (glove_pos_index == glove_size) - { - if (no_errors) - callback_function(glove_positions, true); - reception_state_ = reception_16bit::SYNCHRONIZATION_1; - } - break; - } - } - else - { - switch (reception_state_) - { - case INITIAL: - switch (current_value ) - { - case 'S': - // the line starts with S, followed by the sensors values - ++nb_msgs_received; - // reset the index to 0 - glove_pos_index = 0; - // reset no_errors to true for the new message - no_errors = true; - reception_state_ = RECEIVING_FRAME; - break; - } - break; - case RECEIVING_FRAME: - // this is a glove sensor value, a status byte or a "message end" - switch (glove_pos_index ) - { - case glove_size: - if (cyberglove_version_ == "1") - { - // Cyberglove I doesn't provide information on the LED light state - // so we will consider it's always on - light_on = true; - } - else if (cyberglove_version_ == "2") - { - // the last char of the msg is the status byte - - // the status bit 1 corresponds to the button - if (current_value & 2) - button_on = true; - else - button_on = false; - // the status bit 2 corresponds to the light - if (current_value & 4) - light_on = true; - else - light_on = false; - } - else - { - // the last char of the line should be 0 - // if it is 0, then the full message has been received, - // and we call the callback function. - if (current_value == 0 && no_errors) - callback_function(glove_positions, light_on); - if (current_value != 0) - std::cout << "Last char is not 0: " << current_value << std::endl; - - reception_state_ = INITIAL; - } - break; - - case glove_size + 1: // Contains info we don't currently know how to interpret. - case glove_size + 2: // Contains info we don't currently know how to interpret. - break; - - case glove_size + 3: - if (cyberglove_version_ == "1") - { - // the last char of the line should be 'S' (83) but this is an assumption not based on documentation - // most of the time we get 83, but other numbers have been observed occasionally - // if it is 83, then the full message has been received, - // and we call the callback function. - if (current_value == 83 && no_errors) - callback_function(glove_positions, light_on); - if (current_value != 83) - std::cout << "Last char is not 0: " << current_value << std::endl; - } - else if (cyberglove_version_ == "2") - { - // the last char of the line should be 0 - // if it is 0, then the full message has been received, - // and we call the callback function. - if (current_value == 0 && no_errors) - callback_function(glove_positions, light_on); - if (current_value != 0) - std::cout << "Last char is not 0: " << current_value << std::endl; - } - reception_state_ = INITIAL; - break; - - default: - // this is a joint data from the glove - // the value in the message should never be 0. - if (current_value == 0) - { - no_errors = false; - } - // the values sent by the glove are in the range [1;254] - // -> we convert them to float in the range [0;1] - glove_positions[glove_pos_index] = ((static_cast(current_value)) - 1.0f) / 254.0f; - break; - } - ++glove_pos_index; - break; - } - } - } - } - - int CybergloveSerial::get_nb_msgs_received() - { - return nb_msgs_received; - } -} // namespace cyberglove - -/* For the emacs weenies in the crowd. -Local Variables: - c-basic-offset: 2 -End: -*/ diff --git a/cyberglove/src/xml_calibration_parser.cpp b/cyberglove/src/xml_calibration_parser.cpp deleted file mode 100644 index 802af92b..00000000 --- a/cyberglove/src/xml_calibration_parser.cpp +++ /dev/null @@ -1,309 +0,0 @@ -/* -* @file xml_calibration_parser.cpp -* @author Ugo Cupcic -* @date Tue Apr 27 11:30:41 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief This is a simple xml parser, used to parse the calibration -* file for the cyberglove. -* A Calibration file must have this format: -* -* -* -* -* -* -* -* The calibration will be used by the glove node to stream coherent -* angles (and not an uncalibrated sensor value between 0 and 1). -* -* -*/ - -#include -#include "cyberglove/xml_calibration_parser.h" -#include -#include -#include - -namespace xml_calibration_parser -{ - - const float XmlCalibrationParser::lookup_precision = 1000.0f; - const float XmlCalibrationParser::lookup_offset = 1.0f; - - /* - * The constructor: parses the given file and stores the calibration - * in the vector std::vector jointsCalibrations. - * - * @param path_to_calibration the path to the xml calibration - * file. Please note that it is best to use ros parameters to set - * the path in your code calling this constructor. - */ - XmlCalibrationParser::XmlCalibrationParser(std::string path_to_calibration) - { - TiXmlDocument doc(path_to_calibration.c_str()); - bool loadOkay = doc.LoadFile(); - if (loadOkay) - { - ROS_DEBUG("loading calibration %s", path_to_calibration.c_str()); - parse_calibration_file(doc.RootElement()); - build_calibration_table(); - } - else - { - ROS_ERROR("Failed to load file \"%s\"", path_to_calibration.c_str()); - } - } - - /* - * Parses the calibration file and retreive the full calibration for - * the cyberglove. - * - * @param pParent The parent node (Cyberglove_calibration) - * containing all the xml tree. - */ - void XmlCalibrationParser::parse_calibration_file(TiXmlNode* pParent) - { - if (!pParent ) return; - - TiXmlElement* child = pParent->FirstChildElement("Joint"); - - // no Joint elements => error - if (!child ) - { - ROS_ERROR("The calibration file seems to be broken: there's no Joint elements."); - return; - } - - bool has_sibling = true; - while (has_sibling) - { - // a Joint element was found - XmlCalibrationParser::JointCalibration joint_calib; - joint_calib.name = child->Attribute("name"); - - joint_calib.calibrations = parse_joint_attributes(child); - - jointsCalibrations.push_back(joint_calib); - - // get the next Joint element - child = child->NextSiblingElement("Joint"); - // no more Joint elements => stop - if (!child) - { - has_sibling = false; - } - } - } - - /* - * Parses the Joint element of the calibration file. - * - * @param pParent a Joint element - * - * @return the calibration values for this Joint. - */ - std::vector - XmlCalibrationParser::parse_joint_attributes(TiXmlNode* pParent) - { - std::vector calibrations; - - TiXmlElement* child = pParent->FirstChildElement("calib"); - - // no Joint elements => error - if (!child ) - { - ROS_ERROR("The calibration file seems to be broken: there's no calibration elements."); - return calibrations; - } - bool has_sibling = true; - - while (has_sibling) - { - // a Joint element was found - XmlCalibrationParser::Calibration calib; - float fval; - - // get the raw-value - if (child->QueryFloatAttribute("raw_value", &fval) == TIXML_SUCCESS ) - calib.raw_value = fval; - else - ROS_ERROR("The calibration file seems to be broken: there's no raw_value attribute."); - - // get the calibrated-value - if (child->QueryFloatAttribute("calibrated_value", &fval) == TIXML_SUCCESS ) - calib.calibrated_value = fval; - else - ROS_ERROR("The calibration file seems to be broken: there's no calibrated_value attribute."); - - // add the calibration to the vector - calibrations.push_back(calib); - - // get the next Joint element - child = child->NextSiblingElement("calib"); - // no more Joint elements => stop - if (!child) - has_sibling = false; - } - - return calibrations; - } - - - /* - * Transform the calibration values to a lookup table for fast - * processing of the calibration process. - * NB: the lookup table ranges from 0 to +lookup_offset - * with a precision of 1/lookup_precision. - * - */ - int XmlCalibrationParser::build_calibration_table() - { - for (unsigned int index_calib = 0; index_calib < jointsCalibrations.size(); ++index_calib) - { - std::string name = jointsCalibrations[index_calib].name; - std::cout << name << std::endl; - - std::vector calib = jointsCalibrations[index_calib].calibrations; - - std::vector lookup_table(static_cast(lookup_offset)*static_cast(lookup_precision)); - - if (calib.size() < 2 ) - ROS_ERROR("Not enough points were defined to set up the calibration."); - - // order the calibration vector by ascending values of raw_value - // ROS_ERROR("TODO: calibration vector not ordered yet"); - - std::cout << "lookup table : "; - - // setup the lookup table - for (unsigned int index_lookup=0; index_lookup < lookup_table.size(); ++index_lookup) - { - float value = compute_lookup_value(index_lookup, calib); - std::cout << index_lookup << ":" << value << " "; - lookup_table[index_lookup] = value; - } - - std::cout << std::endl; - - // add the values to the map - joints_calibrations_map[name] = lookup_table; - // joints_calibrations_map.insert(std::pair >(name, lookup_table)); - } - - return 0; - } - - /* - * return the value to store in the lookup table for a given index, - * using the calibration information. - * - * @param index the index for which we compute the value - * - * @param calib the vector containing the calibration informations - * (a list of raw_value <=> calibrated_value) - * - * @return the value to be stored in the lookup table - */ - float XmlCalibrationParser::compute_lookup_value(int index, std::vector calib) - { - float raw_pos = return_raw_position_from_index(index); - - if (calib.size() == 2) - return linear_interpolate( raw_pos, - calib[0].raw_value, - calib[0].calibrated_value, - calib[1].raw_value, - calib[1].calibrated_value); - - for (unsigned int index_calib = 0; index_calib < calib.size() - 1; ++index_calib) - { - if (calib[index_calib].raw_value > raw_pos) - { - return linear_interpolate( raw_pos, - calib[index_calib].raw_value, - calib[index_calib].calibrated_value, - calib[index_calib+1].raw_value, - calib[index_calib+1].calibrated_value); - } - } - - // bigger than last calibrated value => extrapolate the value from - // last 2 values - // Comment translated from french: does it work the formula if we are outside the points? Yes - return linear_interpolate( raw_pos, - calib[calib.size()-1].raw_value, - calib[calib.size()-1].calibrated_value, - calib[calib.size()].raw_value, - calib[calib.size()].calibrated_value); - } - - float XmlCalibrationParser::get_calibration_value(float position, std::string joint_name) - { - mapType::iterator iter = joints_calibrations_map.find(joint_name); - - if (iter != joints_calibrations_map.end()) - { - // reads from the lookup table - int index = return_index_from_raw_position(position); - std::cout << index << std::endl; - return iter->second[index]; - } - else - { - ROS_ERROR("%s is not calibrated", joint_name.c_str()); - return 1.0f; - } - } - - float XmlCalibrationParser::linear_interpolate(float x, float x0, float y0, float x1, float y1) - { - // Comment translated from french: "does it work decreasing? Yes" - float y = 0.0f; - if (x1 - x0 == 0.0f ) - { - ROS_WARN("Bad calibration: raw_calib[1] = raw_calib[0]"); - return 0.0f; - } - - y = y0 + (x-x0)* ((y1-y0)/(x1-x0)); - return y; - } - - - int XmlCalibrationParser::return_index_from_raw_position(float raw_position) - { - if (raw_position < 0.0f) - return 0; - if (raw_position > 1.0f) - return lookup_precision; - return round(raw_position * lookup_precision); - }; - - int XmlCalibrationParser::round(float number) - { - // we only have positive numbers - return static_cast(floor(number + 0.5)); - } - - - std::vector XmlCalibrationParser::getJointsCalibrations() - { - return jointsCalibrations; - } -} // namespace xml_calibration_parser diff --git a/cyberglove/srv/Calibration.srv b/cyberglove/srv/Calibration.srv deleted file mode 100644 index e8606c78..00000000 --- a/cyberglove/srv/Calibration.srv +++ /dev/null @@ -1,3 +0,0 @@ -string path ---- -bool state diff --git a/cyberglove/srv/Start.srv b/cyberglove/srv/Start.srv deleted file mode 100644 index 777019da..00000000 --- a/cyberglove/srv/Start.srv +++ /dev/null @@ -1,3 +0,0 @@ -bool start ---- -bool state diff --git a/cyberglove/test/cyberglove_test.cal b/cyberglove/test/cyberglove_test.cal deleted file mode 100644 index 45a5cded..00000000 --- a/cyberglove/test/cyberglove_test.cal +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/cyberglove/test/test_calibration.cpp b/cyberglove/test/test_calibration.cpp deleted file mode 100644 index e3fe1205..00000000 --- a/cyberglove/test/test_calibration.cpp +++ /dev/null @@ -1,158 +0,0 @@ -/* -* @file test_calibration.cpp -* @author Ugo Cupcic -* @date Tue Mar 29 15:03:12 2011 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* -* @brief Testing the calibration mapping for the glove. -* -* -*/ - - -#include -#include -#include -#include -#include -#include -#include - -#define TEST_EXPRESSION(a) EXPECT_EQ((a), meval::EvaluateMathExpression(#a)) - -float epsilon = 0.01f; - -xml_calibration_parser::XmlCalibrationParser calib_parser; - -TEST(LookupTable, testSimple) -{ - float valtmp; - valtmp = calib_parser.get_calibration_value(0.05f, "test1"); - EXPECT_TRUE(fabs(valtmp - 50.0f) < epsilon) - << "Expected value : 50 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.1f, "test1"); - EXPECT_TRUE(fabs(valtmp - 100.0f) < epsilon) - << "Expected value : 100 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.0f, "test1"); - EXPECT_TRUE(fabs(valtmp - 0.0f) < epsilon) - << "Expected value : 0 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.2f, "test1"); - EXPECT_TRUE(fabs(valtmp - 200.0f) < epsilon) - << "Expected value : 200 " - << "Received value : "<< valtmp; -} - -TEST(LookupTable, integrity) -{ - std::vector myCalib = - calib_parser.getJointsCalibrations(); - - EXPECT_EQ(4, myCalib.size()); - for (unsigned int i = 0; i < myCalib.size(); ++i) - { - EXPECT_EQ(2, myCalib[i].calibrations.size()); - } -} - -TEST(LookupTable, testCalibNotStartingAtZero) -{ - float valtmp; - valtmp = calib_parser.get_calibration_value(0.05f, "test2"); - EXPECT_TRUE(fabs(valtmp - 35.0f) < epsilon) - << "Expected value : 35 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.1f, "test2"); - EXPECT_TRUE(fabs(valtmp - 60.0f) < epsilon) - << "Expected value : 60 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.0f, "test2"); - EXPECT_TRUE(fabs(valtmp - 10.0f) < epsilon) - << "Expected value : 10 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.2f, "test2"); - EXPECT_TRUE(fabs(valtmp - 110.0f) < epsilon) - << "Expected value : 110 " - << "Received value : "<< valtmp; -} - -TEST(LookupTable, testRawNotStartingAtZero) -{ - float valtmp; - valtmp = calib_parser.get_calibration_value(0.15f, "test3"); - EXPECT_TRUE(fabs(valtmp - 35.0f) < epsilon) - << "Expected value : 35 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.2f, "test3"); - EXPECT_TRUE(fabs(valtmp - 60.0f) < epsilon) - << "Expected value : 60 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.1f, "test3"); - EXPECT_TRUE(fabs(valtmp - 10.0f) < epsilon) - << "Expected value : 10 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.3f, "test3"); - EXPECT_TRUE(fabs(valtmp - 110.0f) < epsilon) - << "Expected value : 110 " - << "Received value : "<< valtmp; -} - -TEST(LookupTable, tableNotOrdered) -{ - float valtmp; - valtmp = calib_parser.get_calibration_value(0.05f, "test4"); - EXPECT_TRUE(fabs(valtmp - 35.0f) < epsilon) - << "Expected value : 35 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.1f, "test4"); - EXPECT_TRUE(fabs(valtmp - 10.0f) < epsilon) - << "Expected value : 10 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.0f, "test4"); - EXPECT_TRUE(fabs(valtmp - 60.0f) < epsilon) - << "Expected value : 60 " - << "Received value : "<< valtmp; - - valtmp = calib_parser.get_calibration_value(0.2f, "test4"); - EXPECT_TRUE(fabs(valtmp + 40.0f) < epsilon) - << "Expected value : -40 " - << "Received value : "<< valtmp; -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) -{ - std::string path_to_calibration = ros::package::getPath("cyberglove") + "/test/cyberglove_test.cal"; - calib_parser = xml_calibration_parser::XmlCalibrationParser(path_to_calibration); - - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/cyberglove/test/test_calibration.test b/cyberglove/test/test_calibration.test deleted file mode 100644 index c27e0c93..00000000 --- a/cyberglove/test/test_calibration.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/cyberglove_trajectory/CMakeLists.txt b/cyberglove_trajectory/CMakeLists.txt deleted file mode 100644 index 0a8da3a4..00000000 --- a/cyberglove_trajectory/CMakeLists.txt +++ /dev/null @@ -1,178 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(cyberglove_trajectory) - -## 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 - actionlib - control_msgs - cyberglove - roscpp - roslint - rospy - sr_remappers - trajectory_msgs - sr_utilities -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - -roslint_cpp() -roslint_python() - -## 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 -# trajectory_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 cyberglove_trajectory -# CATKIN_DEPENDS cyberglove roscpp sr_remappers trajectory_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( - include - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(cyberglove_trajectory -# src/${PROJECT_NAME}/cyberglove_trajectory.cpp -# ) - -## Declare a cpp executable -add_executable(cyberglove_trajectory - src/cyberglove_trajectory_publisher.cpp - src/cyberglove_trajectory_node.cpp -) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(cyberglove_trajectory - ${catkin_EXPORTED_TARGETS} -) - -## Specify libraries to link a library or executable target against -target_link_libraries(cyberglove_trajectory - ${catkin_LIBRARIES} - ${Boost_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 cyberglove_trajectory cyberglove_trajectory_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_cyberglove_trajectory.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) diff --git a/cyberglove_trajectory/README.md b/cyberglove_trajectory/README.md deleted file mode 100644 index 978ead83..00000000 --- a/cyberglove_trajectory/README.md +++ /dev/null @@ -1,13 +0,0 @@ -# cyberglove_trajectory - -This package contains source code for interacting with the Cyberglove and sending trajectory control data to the hand. - -## Mocking - -In order to mock the cyberglove and send data to the hand without actual hardware, run: - -```sh -rosrun cyberglove_trajectory cyberglove_mock.py -``` - -This will send a signal to the hand making it alternate between pack and open poses every 3 seconds. diff --git a/cyberglove_trajectory/include/cyberglove_trajectory/cyberglove_trajectory_publisher.h b/cyberglove_trajectory/include/cyberglove_trajectory/cyberglove_trajectory_publisher.h deleted file mode 100644 index fcca481a..00000000 --- a/cyberglove_trajectory/include/cyberglove_trajectory/cyberglove_trajectory_publisher.h +++ /dev/null @@ -1,151 +0,0 @@ -/* -* @file cyberglove_publisher.h -* @author Ugo Cupcic , Contact -* @date Thu Apr 22 10:25:55 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief The goal of this ROS publisher is to publish raw and calibrated -* joint positions from the cyberglove at a regular time interval. We're -* oversampling to get a better accuracy on our data. -* To publish those data, just call the publish() -* function. -* -* -*/ - -#ifndef CYBERGLOVE_TRAJECTORY_CYBERGLOVE_TRAJECTORY_PUBLISHER_H_ -#define CYBERGLOVE_TRAJECTORY_CYBERGLOVE_TRAJECTORY_PUBLISHER_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "cyberglove/serial_glove.h" -#include -#include -#include -#include -#include "sr_remappers/calibration_parser.h" -#include - -namespace cyberglove -{ -class CybergloveTrajectoryPublisher -{ -public: - /// Constructor - CybergloveTrajectoryPublisher(); - - /// Destructor - ~CybergloveTrajectoryPublisher(); - - ros::Publisher cyberglove_pub; - - typedef threadsafe::Map > CalibrationMap; - - - /* - * Reads the calibration from the parameter server. - * - * - * @return a calibration map - */ - CalibrationMap read_joint_calibration(); - - bool isPublishing(); - void setPublishing(bool value); -private: - ///////////////// - // CALLBACKS // - ///////////////// - - ros::NodeHandle node, n_tilde; - unsigned int publish_counter_max, publish_counter_index; - - /// the actual connection with the cyberglove is done here. - boost::shared_ptr serial_glove; - - /* - * The callback function: called each time a full message - * is received. This function is bound to the serial_glove - * object using boost::bind. - * - * @param glove_pos A vector containing the current raw joints positions. - * @param light_on true if the light is on, false otherwise. - */ - void glove_callback(std::vector glove_pos, bool light_on); - bool reload_calibration(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); - - std::string path_to_glove; - bool publishing; - - /// The map used to calibrate each joint. - boost::shared_ptr calibration_map; - /// A temporary calibration for a given joint. - boost::shared_ptr calibration_tmp; - /// the calibration parser containing the mapping matrix - boost::scoped_ptr map_calibration_parser; - - ros::Publisher cyberglove_raw_pub; - ros::Publisher cyberglove_cal_pub; - sensor_msgs::JointState raw_jointstate_msg; - sensor_msgs::JointState cal_jointstate_msg; - - std::vector > glove_positions; - - ros::ServiceServer reload_calibration_service; - - - void applyJointMapping(const std::vector& glove_postions, std::vector& hand_positions); - void processJointZeros(const std::vector& postions_with_J0, std::vector& postions_without_J0); - - /* - * process the joint_states callback for the finger abductions: processes the message from the cyberglove node, remap it to the Dextrous hand J4s - * It overwrites whatever was written for the J4s by the calibration parser get_remapped_vector - * - * @param glove_postions The positions that come from the glove sensors (usually calibrated) - * @param hand_positions the vector where the result is written (only J4s are written) - */ - void getAbductionJoints(const std::vector& glove_postions, std::vector& hand_positions); - - static const std::vector joint_name_vector_; - static const std::vector joint_mapping_vector_; - static const std::vector glove_sensors_vector_; - - boost::scoped_ptr > action_client_; - control_msgs::FollowJointTrajectoryGoal trajectory_goal_; - - std::string cyberglove_version_; - std::string streaming_protocol_; - - ros::Duration trajectory_tx_delay_; - ros::Duration trajectory_delay_; -}; // end class CybergloveTrajectoryPublisher - -} // namespace cyberglove - -#endif // CYBERGLOVE_TRAJECTORY_CYBERGLOVE_TRAJECTORY_PUBLISHER_H_ - -/* For the emacs weenies in the crowd. -Local Variables: - c-basic-offset: 2 -End: -*/ diff --git a/cyberglove_trajectory/launch/cyberglove.launch b/cyberglove_trajectory/launch/cyberglove.launch deleted file mode 100644 index 31f0f8e3..00000000 --- a/cyberglove_trajectory/launch/cyberglove.launch +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/cyberglove_trajectory/package.xml b/cyberglove_trajectory/package.xml deleted file mode 100644 index 2b47a627..00000000 --- a/cyberglove_trajectory/package.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - cyberglove_trajectory - 0.0.0 - A package that gets joint positions from the cyberglove via serial port, and calibrates them, remaps them to Shadow hand joints, and publishes them as joint trajectories in a single node - - Shadow Robot's software team - GPL - - Toni Oliver - - - catkin - cyberglove - roscpp - roslint - rospy - sr_remappers - trajectory_msgs - actionlib - control_msgs - sr_utilities - - cyberglove - roscpp - rospy - sr_remappers - trajectory_msgs - actionlib - control_msgs - sr_utilities - - diff --git a/cyberglove_trajectory/setup.py b/cyberglove_trajectory/setup.py deleted file mode 100755 index 196f3ae7..00000000 --- a/cyberglove_trajectory/setup.py +++ /dev/null @@ -1,14 +0,0 @@ -# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD -from __future__ import absolute_import - -from distutils.core import setup - -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['cyberglove_trajectory'], - package_dir={'': 'src'}, -) - -setup(**setup_args) diff --git a/cyberglove_trajectory/src/cyberglove_trajectory/__init__.py b/cyberglove_trajectory/src/cyberglove_trajectory/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/cyberglove_trajectory/src/cyberglove_trajectory/cyberglove_mock.py b/cyberglove_trajectory/src/cyberglove_trajectory/cyberglove_mock.py deleted file mode 100755 index a781b90c..00000000 --- a/cyberglove_trajectory/src/cyberglove_trajectory/cyberglove_mock.py +++ /dev/null @@ -1,102 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright 2019, 2022 Shadow Robot Company Ltd. -# -# This program is free software: you can redistribute it and/or modify it -# under the terms of the GNU General Public License as published by the Free -# Software Foundation version 2 of the License. -# -# This program is distributed in the hope that it will be useful, but WITHOUT -# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -# more details. -# -# You should have received a copy of the GNU General Public License along -# with this program. If not, see . - -# pylint: disable-all -from __future__ import absolute_import - -import actionlib -import rospy -from control_msgs.msg import (FollowJointTrajectoryAction, - FollowJointTrajectoryGoal) -from trajectory_msgs.msg import JointTrajectoryPoint - - -class CybergloveMock(object): - def __init__(self, hand_prefix='rh', wrist_control=True, hand_type='hand_e'): - self._hand_prefix = hand_prefix - self._wrist_control = wrist_control - self.hand_type = hand_type - self._set_traj_server('/{}_trajectory_controller/follow_joint_trajectory'.format(self._hand_prefix)) - self.joint_names = [] - - self._set_up_joint_list() - - def _set_traj_server(self, traj_server_ns): - self._hand_traj_ns = traj_server_ns - self._hand_traj_client = actionlib.SimpleActionClient(self._hand_traj_ns, - FollowJointTrajectoryAction) - - def _set_up_joint_list(self): - if 'hand_e' == self.hand_type: - self.joint_names = ["_FFJ1", "_FFJ2", "_FFJ3", "_FFJ4", "_MFJ1", "_MFJ2", - "_MFJ3", "_MFJ4", "_RFJ1", "_RFJ2", "_RFJ3", "_RFJ4", - "_LFJ1", "_LFJ2", "_LFJ3", "_LFJ4", "_LFJ5", "_THJ1", - "_THJ2", "_THJ3", "_THJ4", "_THJ5"] - if self._wrist_control: - self.joint_names += ["_WRJ1", "_WRJ2"] - else: - raise ValueError("Unsupported hand type!") - - self.joint_names = [self._hand_prefix + name for name in self.joint_names] - - def open_hand(self): - num_of_joints = len(self.joint_names) - CONST_OPEN_HAND_JOINT_VALUES = [0] * num_of_joints - self._send_goal_to_hand(CONST_OPEN_HAND_JOINT_VALUES) - - def pack_hand(self): - if 'hand_e' == self.hand_type: - CONST_PACK_HAND_JOINT_VALUES = [1.571, 1.571, 1.571, 0.0, - 1.571, 1.571, 1.571, 0.0, - 1.571, 1.571, 1.571, 0.0, - 1.571, 1.571, 1.571, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0] - if self._wrist_control: - CONST_PACK_HAND_JOINT_VALUES += [0.14, 0.0] - else: - raise ValueError("Unsupported hand type!") - - self._send_goal_to_hand(CONST_PACK_HAND_JOINT_VALUES) - - def _send_goal_to_hand(self, hand_positions): - if not self._hand_traj_client.wait_for_server(timeout=rospy.Duration(3)): - rospy.logwarn("Failed to connected to actionlib server '{}'.".format(self._hand_traj_ns)) - return - goal = FollowJointTrajectoryGoal() - goal.trajectory.header.stamp = rospy.Time.now() - goal.trajectory.joint_names = self.joint_names - joint_traj_point = JointTrajectoryPoint() - - joint_traj_point.positions = hand_positions - - joint_traj_point.time_from_start = rospy.Duration(0.1) - goal.trajectory.points.append(joint_traj_point) - self._hand_traj_client.send_goal(goal) - - def run(self): - rospy.loginfo("Alternating between pack and open every 3 seconds...") - while not rospy.is_shutdown(): - self.pack_hand() - rospy.sleep(3) - self.open_hand() - rospy.sleep(3) - - -if __name__ == "__main__": - rospy.init_node('cyberglove_mock_node') - - cyberglove_mock = CybergloveMock() - cyberglove_mock.run() diff --git a/cyberglove_trajectory/src/cyberglove_trajectory_node.cpp b/cyberglove_trajectory/src/cyberglove_trajectory_node.cpp deleted file mode 100644 index cd34d36d..00000000 --- a/cyberglove_trajectory/src/cyberglove_trajectory_node.cpp +++ /dev/null @@ -1,62 +0,0 @@ -/* -* @file cyberglove_node.cpp -* @author Ugo Cupcic -* @date Thu Apr 22 10:21:50 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief The cyberglove node publishes data collected from a -* Cyberglove. -* -* -*/ - -#include -#include -#include "cyberglove_trajectory/cyberglove_trajectory_publisher.h" -#include - -///////////////////////////////// -// MAIN // -///////////////////////////////// - - -/* -* Start the cyberglove trajectory publisher. -* -* @param argc -* @param argv -* -* @return -1 if error (e.g. no glove found) -*/ -int main(int argc, char** argv) -{ - ros::init(argc, argv, "cyberglove_trajectory_node"); - - boost::shared_ptr cyberglove_pub( - new cyberglove::CybergloveTrajectoryPublisher()); - - ros::spin(); - - return 0; -} - - -/* For the emacs weenies in the crowd. -Local Variables: - c-basic-offset: 2 -End: -*/ diff --git a/cyberglove_trajectory/src/cyberglove_trajectory_publisher.cpp b/cyberglove_trajectory/src/cyberglove_trajectory_publisher.cpp deleted file mode 100644 index 7ef06f4f..00000000 --- a/cyberglove_trajectory/src/cyberglove_trajectory_publisher.cpp +++ /dev/null @@ -1,486 +0,0 @@ -/* -* @file shadowhand_publisher.cpp -* @author Ugo Cupcic , Toni Oliver -* @date 12/11/2014 -* -* -* Copyright 2014 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief gets joint positions from the cyberglove via serial port, and calibrates them, -* remaps them to Shadow hand joints, and publishes them as joint trajectories in a single node. -* -*/ - -#include -#include -#include -#include -#include "cyberglove_trajectory/cyberglove_trajectory_publisher.h" -#include -#include -#include -#include - -namespace cyberglove -{ - -const std::vector CybergloveTrajectoryPublisher::joint_name_vector_ = boost::assign::list_of - ("THJ1") - ("THJ2") - ("THJ3") - ("THJ4") - ("THJ5") - ("FFJ1") - ("FFJ2") - ("FFJ3") - ("FFJ4") - ("MFJ1") - ("MFJ2") - ("MFJ3") - ("MFJ4") - ("RFJ1") - ("RFJ2") - ("RFJ3") - ("RFJ4") - ("LFJ1") - ("LFJ2") - ("LFJ3") - ("LFJ4") - ("LFJ5") - ("WRJ1") - ("WRJ2"); - -const std::vector CybergloveTrajectoryPublisher::joint_mapping_vector_ = boost::assign::list_of - ("THJ1") - ("THJ2") - ("THJ3") - ("THJ4") - ("THJ5") - ("FFJ0") - ("FFJ3") - ("FFJ4") - ("MFJ0") - ("MFJ3") - ("MFJ4") - ("RFJ0") - ("RFJ3") - ("RFJ4") - ("LFJ0") - ("LFJ3") - ("LFJ4") - ("LFJ5") - ("WRJ1") - ("WRJ2"); - - - - - -// initialises joint names (the order is important) -const std::vector CybergloveTrajectoryPublisher::glove_sensors_vector_ = boost::assign::list_of - ("G_ThumbRotate") - ("G_ThumbMPJ") - ("G_ThumbIJ") - ("G_ThumbAb") - ("G_IndexMPJ") - ("G_IndexPIJ") - ("G_IndexDIJ") - ("G_MiddleMPJ") - ("G_MiddlePIJ") - ("G_MiddleDIJ") - ("G_MiddleIndexAb") - ("G_RingMPJ") - ("G_RingPIJ") - ("G_RingDIJ") - ("G_RingMiddleAb") - ("G_PinkieMPJ") - ("G_PinkiePIJ") - ("G_PinkieDIJ") - ("G_PinkieRingAb") - ("G_PalmArch") - ("G_WristPitch") - ("G_WristYaw"); - ///////////////////////////////// - // CONSTRUCTOR/DESTRUCTOR // - ///////////////////////////////// - - CybergloveTrajectoryPublisher::CybergloveTrajectoryPublisher() - : n_tilde("~"), publish_counter_max(0), publish_counter_index(0), - path_to_glove("/dev/ttyS0"), publishing(true) - { - std::string param; - std::string path; - n_tilde.searchParam("cyberglove_mapping_path", param); - n_tilde.param(param, path, std::string()); - map_calibration_parser.reset(new CalibrationParser(path)); - ROS_INFO("Mapping file loaded for the Cyberglove: %s", path.c_str()); - - calibration_map.reset(new CalibrationMap(read_joint_calibration())); - - std::string searched_param; - std::string hand_prefix; - searched_param = "hand_prefix"; - n_tilde.param(searched_param, hand_prefix, std::string()); - std::string action_server_name = "trajectory_controller/follow_joint_trajectory"; - action_client_.reset(new actionlib::SimpleActionClient( - hand_prefix + '_' + action_server_name, true)); - - for (size_t i = 0; i < joint_name_vector_.size(); i++) - { - trajectory_goal_.trajectory.joint_names.push_back(hand_prefix + '_' + joint_name_vector_[i]); - } - - cyberglove_raw_pub = n_tilde.advertise("raw/joint_states", 2); - cyberglove_cal_pub = n_tilde.advertise("calibrated/joint_states", 2); - - reload_calibration_service = n_tilde.advertiseService( - "reload_calibration", boost::bind(&CybergloveTrajectoryPublisher::reload_calibration, this, _1, _2)); - - for (std::string name : glove_sensors_vector_) - { - raw_jointstate_msg.name.push_back(name); - cal_jointstate_msg.name.push_back(name); - } - - // set sampling frequency - double sampling_freq; - n_tilde.param("sampling_frequency", sampling_freq, 100.0); - - // set publish_counter: the number of data we'll average - // before publishing. - double publish_freq; - n_tilde.param("publish_frequency", publish_freq, 20.0); - publish_counter_max = static_cast(sampling_freq / publish_freq); - - ROS_INFO_STREAM("Sampling at " << sampling_freq << "Hz ; Publishing at " - << publish_freq << "Hz ; Publish counter: "<< publish_counter_max); - - // Get the cyberglove version '2' or '3' - n_tilde.param("cyberglove_version", cyberglove_version_, std::string("2")); - ROS_INFO("Cyberglove version: %s", cyberglove_version_.c_str()); - - // Get the cyberglove streaming protocol '8bit' or '16bit' - n_tilde.param("streaming_protocol", streaming_protocol_, std::string("8bit")); - ROS_INFO("Streaming protocol: %s", streaming_protocol_.c_str()); - - // set path to glove - n_tilde.param("path_to_glove", path_to_glove, std::string("/dev/ttyS0")); - ROS_INFO("Opening glove on port: %s", path_to_glove.c_str()); - - // set trajectory tx delay: the delay it takes to get to the trajectory controller. - // it is used to set the timestamp of the trajectory goal. 10ms default - // (the trajectory point might be discarded if the trajectory arrives later) - double delay; - n_tilde.param("trajectory_tx_delay", delay, 0.01); - trajectory_tx_delay_ = ros::Duration(delay); - - // set trajectory delay: the delay from the trajectory beginning to the trajectory point. - // it is used to set the time_from start of the single trajectory point. 2ms default - // (it can be very small, but not zero, or the point will be discarded as past). - n_tilde.param("trajectory_delay", delay, 0.002); - trajectory_delay_ = ros::Duration(delay); - - // initialize the connection with the cyberglove and binds the callback function - serial_glove = boost::shared_ptr(new CybergloveSerial(path_to_glove, cyberglove_version_, - streaming_protocol_, boost::bind(&CybergloveTrajectoryPublisher::glove_callback, this, _1, _2))); - - int res = -1; - if (cyberglove_version_ == "2") - { - cyberglove_freq::CybergloveFreq frequency; - - switch (static_cast(sampling_freq)) - { - case 100: - res = serial_glove->set_frequency(frequency.hundred_hz); - break; - case 45: - res = serial_glove->set_frequency(frequency.fourtyfive_hz); - break; - case 10: - res = serial_glove->set_frequency(frequency.ten_hz); - break; - case 1: - res = serial_glove->set_frequency(frequency.one_hz); - break; - default: - res = serial_glove->set_frequency(frequency.hundred_hz); - break; - } - - // We want the glove to transmit the status (light on/off) - res = serial_glove->set_transmit_info(true); - } - - // Should the glove filter the data? (it leads to less smooth movements, but quieter behaviour on the motors) - bool filtering; - n_tilde.param("filter", filtering, false); - std::string filt_msg(filtering?"ON":"OFF"); - ROS_INFO("Filtering: %s", filt_msg.c_str()); - res = serial_glove->set_filtering(filtering); - - // start reading the data. - res = serial_glove->start_stream(); - } - - CybergloveTrajectoryPublisher::~CybergloveTrajectoryPublisher() - { - } - - bool CybergloveTrajectoryPublisher::isPublishing() - { - if (publishing) - { - return true; - } - else - { - return false; - } - } - - void CybergloveTrajectoryPublisher::setPublishing(bool value) - { - publishing = value; - } - - bool CybergloveTrajectoryPublisher::reload_calibration(std_srvs::Empty::Request& request, - std_srvs::Empty::Response& response) - { - calibration_map.reset(new CalibrationMap(read_joint_calibration())); - return true; - } - - ///////////////////////////////// - // CALLBACK METHOD // - ///////////////////////////////// - void CybergloveTrajectoryPublisher::glove_callback(std::vector glove_pos, bool light_on) - { - // if the light is off, we don't publish any data. - if (!light_on ) - { - publishing = false; - ROS_DEBUG("The glove button is off, no data will be read / sent"); - ros::spinOnce(); - return; - } - publishing = true; - - // appends the current position to the vector of position - glove_positions.push_back(glove_pos); - - publish_counter_index += 1; - - // if we've enough samples, publish the data: - if (publish_counter_index == publish_counter_max ) - { - std::vector glove_calibrated_positions, hand_positions, hand_positions_no_J0; - - raw_jointstate_msg.position.clear(); - raw_jointstate_msg.header.stamp = ros::Time::now(); - cal_jointstate_msg.position.clear(); - cal_jointstate_msg.header.stamp = ros::Time::now(); - - - // fill the joint_state msg with the averaged glove data - for (unsigned int index_joint = 0; index_joint < CybergloveSerial::glove_size; ++index_joint) - { - // compute the average over the samples for the current joint - float averaged_value = 0.0f; - for (unsigned int index_sample = 0; index_sample < publish_counter_max; ++index_sample) - { - averaged_value += glove_positions[index_sample][index_joint]; - } - averaged_value /= publish_counter_max; - - calibration_tmp = calibration_map->find(glove_sensors_vector_[index_joint]); - double calibration_value = calibration_tmp->compute(static_cast (averaged_value)); - - raw_jointstate_msg.position.push_back(averaged_value); - cal_jointstate_msg.position.push_back(calibration_value); - glove_calibrated_positions.push_back(calibration_value); - } - cyberglove_raw_pub.publish(raw_jointstate_msg); - cyberglove_cal_pub.publish(cal_jointstate_msg); - - - publish_counter_index = 0; - glove_positions.clear(); - - applyJointMapping(glove_calibrated_positions, hand_positions); - processJointZeros(hand_positions, hand_positions_no_J0); - - // Build and send the goal - - trajectory_goal_.trajectory.points.clear(); - // WARNING if this node runs on a different machine from the trajectory controller, both machines will need to be - // synchronized. chrony (sudo apt-get install crony) has been used successfully to achieve that - // The extra 10ms will allow time for the trajectory to get to the trajectory controller - trajectory_goal_.trajectory.header.stamp = ros::Time::now() + trajectory_tx_delay_; - - trajectory_msgs::JointTrajectoryPoint trajectory_point = trajectory_msgs::JointTrajectoryPoint(); - trajectory_point.positions = hand_positions_no_J0; - // We set the time from start to 10 ms, to allow some time for the hand to get there - trajectory_point.time_from_start = trajectory_delay_; - - for (size_t i=0; i < trajectory_point.positions.size(); i++) - { - if (isnan(trajectory_point.positions[i])) - return; - } - - trajectory_goal_.trajectory.points.push_back(trajectory_point); - - action_client_->sendGoal(trajectory_goal_); - } - - ros::spinOnce(); - } - - - void CybergloveTrajectoryPublisher::applyJointMapping(const std::vector& glove_postions, - std::vector& hand_positions ) - { - // Do conversion - std::vector vect = map_calibration_parser->get_remapped_vector(glove_postions); - - // Process J4's - getAbductionJoints(glove_postions, vect); - - hand_positions = vect; - } - - void CybergloveTrajectoryPublisher::processJointZeros(const std::vector& postions_with_J0, - std::vector& postions_without_J0 ) - { - for (unsigned int i = 0; i < postions_with_J0.size(); ++i ) - { - if (joint_mapping_vector_[i][joint_mapping_vector_[i].size()-1] == '0') - { - postions_without_J0.push_back(postions_with_J0[i] / 2); - postions_without_J0.push_back(postions_with_J0[i] / 2); - } - else - { - postions_without_J0.push_back(postions_with_J0[i]); - } - } - } - - void CybergloveTrajectoryPublisher::getAbductionJoints(const std::vector& glove_postions, - std::vector& hand_positions) - { - double middleIndexAb = glove_postions[10]; - double ringMiddleAb = glove_postions[14]; - double pinkieRingAb = glove_postions[18]; - - // if the abduction sensors are less than 0, it is an artifact of the calibration (we don't want to consider - // anything smaller than 0 for these sensors) - if (middleIndexAb < 0.0) - middleIndexAb = 0.0; - if (ringMiddleAb < 0.0) - ringMiddleAb = 0.0; - if (pinkieRingAb < 0.0) - pinkieRingAb = 0.0; - // Add the 3 abduction angles to have an idea of where the centre lies - double ab_total = middleIndexAb + ringMiddleAb + pinkieRingAb; - - // When trying to understand this code bear in mind that the abduction sign convention - // in the shadow hand is the opposite for ff and mf than for rf and lf. - if (ab_total/2 < middleIndexAb) // If the centre lies between ff and mf - { - // FFJ4 - hand_positions[7] = -ab_total/2; - // MFJ4 - hand_positions[10] = middleIndexAb - ab_total/2; - // RFJ4 - hand_positions[13] = -(ringMiddleAb + hand_positions[10]); - // LFJ4 - hand_positions[16] = -pinkieRingAb + hand_positions[13]; - } - else if (ab_total/2 < middleIndexAb + ringMiddleAb) // If the centre lies between mf and rf - { - // MFJ4 - hand_positions[10] = -(ab_total/2 - middleIndexAb); - // FFJ4 - hand_positions[7] = -middleIndexAb + hand_positions[10]; - // RFJ4 - hand_positions[13] = -(ringMiddleAb + hand_positions[10]); - // LFJ4 - hand_positions[16] = -pinkieRingAb + hand_positions[13]; - } - else // If the centre lies between rf and lf - { - // LFJ4 - hand_positions[16] = -ab_total/2; - // RFJ4 - hand_positions[13] = pinkieRingAb + hand_positions[16]; - // MFJ4 - hand_positions[10] = -(ringMiddleAb + hand_positions[13]); - // FFJ4 - hand_positions[7] = -middleIndexAb + hand_positions[10]; - } - } - -CybergloveTrajectoryPublisher::CalibrationMap CybergloveTrajectoryPublisher::read_joint_calibration() -{ - CalibrationMap joint_calibration; - - XmlRpc::XmlRpcValue calib; - n_tilde.getParam("cyberglove_calibration", calib); - ROS_ASSERT(calib.getType() == XmlRpc::XmlRpcValue::TypeArray); - // iterate on all the joints - for (int32_t index_cal = 0; index_cal < calib.size(); ++index_cal) - { - // check the calibration is well formatted: - // first joint name, then calibration table - ROS_ASSERT(calib[index_cal][0].getType() == XmlRpc::XmlRpcValue::TypeString); - ROS_ASSERT(calib[index_cal][1].getType() == XmlRpc::XmlRpcValue::TypeArray); - - std::string joint_name = static_cast (calib[index_cal][0]); - std::vector calib_table_tmp; - - // now iterates on the calibration table for the current joint - for (int32_t index_table = 0; index_table < calib[index_cal][1].size(); ++index_table) - { - ROS_ASSERT(calib[index_cal][1][index_table].getType() == XmlRpc::XmlRpcValue::TypeArray); - // only 2 values per calibration point: raw and calibrated (doubles) - ROS_ASSERT(calib[index_cal][1][index_table].size() == 2); - ROS_ASSERT(calib[index_cal][1][index_table][0].getType() == XmlRpc::XmlRpcValue::TypeDouble); - ROS_ASSERT(calib[index_cal][1][index_table][1].getType() == XmlRpc::XmlRpcValue::TypeDouble); - - - joint_calibration::Point point_tmp; - point_tmp.raw_value = static_cast (calib[index_cal][1][index_table][0]); - point_tmp.calibrated_value = sr_math_utils::to_rad(static_cast (calib[index_cal][1][index_table][1])); - calib_table_tmp.push_back(point_tmp); - } - - joint_calibration.insert(joint_name, boost::shared_ptr( - new shadow_robot::JointCalibration(calib_table_tmp))); - } - - return joint_calibration; -} // end read_joint_calibration - -} // namespace cyberglove - - - -/* For the emacs weenies in the crowd. -Local Variables: - c-basic-offset: 2 -End: -*/ diff --git a/repository.rosinstall b/repository.rosinstall index 3e403d4b..bb97d1d1 100644 --- a/repository.rosinstall +++ b/repository.rosinstall @@ -1,8 +1,3 @@ -- git: - uri: https://github.com/ozymandium/cereal-port.git - version: master - local-name: cereal_port - - git: uri: https://github.com/ros-drivers/leap_motion.git version: hydro @@ -18,11 +13,6 @@ version: noetic-devel local-name: sr_core -- git: - uri: https://github.com/shadow-robot/sr_cyberglove_config.git - version: master - local-name: sr_cyberglove_config - # TODO: Remove when leap_motion released in noeatic rosdistro - git: uri: https://github.com/ros-drivers/leap_motion.git diff --git a/sr_remappers/CMakeLists.txt b/sr_remappers/CMakeLists.txt deleted file mode 100644 index 73de00d6..00000000 --- a/sr_remappers/CMakeLists.txt +++ /dev/null @@ -1,139 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(sr_remappers) - -## 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 - roslint - rospy - std_msgs - sensor_msgs - sr_robot_msgs - sr_cyberglove_config - cyberglove -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED system filesystem date_time thread) - -roslint_cpp() -roslint_python() - -## 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 ## -################################################ - -## 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 sr_remappers -CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs sr_robot_msgs sr_cyberglove_config cyberglove -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(SYSTEM ${Boost_INCLUDE_DIR}) -include_directories(include) -include_directories(${catkin_INCLUDE_DIRS}) - -## Declare a cpp executable -add_library(${PROJECT_NAME} - src/shadowhand_to_cyberglove_remapper.cpp - src/calibration_parser.cpp -) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) - - - -## Declare a cpp executable -add_executable(cyberglove_remapper - src/shadowhand_to_cyberglove_remapper.cpp - src/shadowhand_to_cyberglove_remapper_node.cpp - src/calibration_parser.cpp -) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(cyberglove_remapper - ${catkin_EXPORTED_TARGETS} -) - -## Specify libraries to link a library or executable target against -target_link_libraries(cyberglove_remapper - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html -install(TARGETS sr_remappers - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/sr_remappers/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/sr_remappers/README.md b/sr_remappers/README.md deleted file mode 100644 index 3ff7f804..00000000 --- a/sr_remappers/README.md +++ /dev/null @@ -1,27 +0,0 @@ -**sr_remappers** is used to remap data coming from different hardware to the Shadow Hardware. It also provides apps to remap data coming from the Shadow Hardware to different hardware. - -Generic principle for the Remapper ----------------------------------- - -The generic principle for those remappers is as follow: - -* subscribe to a given topic, get the vectors of incoming data. -* multiply these vectors as they come by a mapping matrix => remap one (or more) input data to one (or more) output data. -* publish the generated vector to a new topic. - -Cyberglove Remapper -------------------- - -This is a remapper used to remap coming from a cyberglove node to a shadowhand node. It allows the user to control the Shadow Robot Dextrous Hand with a Cyberglove from Immersion. The cyberglove node can be found in the shadow_robot stack as well. There's a tool in sr_control_gui to generate an optimal mapping matrix for a given user in a few steps. - -You can specify different parameters in the launch file remapper_glove.launch: - -* cyberglove_prefix: set the prefix from which the data are coming. -* sendupdate_prefix: set the prefix to which the remapped data will be published. -* cyberglove_mapping_path: the path to the mapping matrix. - -Code API --------- - -* The CalibrationParser class is taking care of parsing the calibration matrices and multiplying the input vector to compute the remapped vectors. -* shadowhand_to_cyberglove_remapper::ShadowhandToCybergloveRemapper is where the subscribe / publish are done for the Cyberglove. diff --git a/sr_remappers/include/sr_remappers/calibration_parser.h b/sr_remappers/include/sr_remappers/calibration_parser.h deleted file mode 100644 index bdc16412..00000000 --- a/sr_remappers/include/sr_remappers/calibration_parser.h +++ /dev/null @@ -1,90 +0,0 @@ -/* -* @file calibration_parser.h -* @author Ugo Cupcic , Contact -* @date Thu May 13 09:44:52 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief This is where the calibration matrix is read from a file, stored and where the actual mapping take place. -* -* -*/ - -#ifndef SR_REMAPPERS_CALIBRATION_PARSER_H_ -#define SR_REMAPPERS_CALIBRATION_PARSER_H_ - -#include -#include -#include -#include - -/* -* This is where the calibration matrix is read from a file, stored and where the actual mapping take place. -*/ -class CalibrationParser -{ -public: - /* - * Default constructor, using the default path. Initialize a - * calibration matrix loaded from the default calibration file. - * - */ - CalibrationParser(); - - /* - * Constructor initializing a calibration matrix loaded from the - * given calibration file. - * - * @param path the path to the calibration file - */ - explicit CalibrationParser(std::string path); - ~CalibrationParser() {} - - /* - * multiplies the vector by the mapping matrix - * - * @param vector vector to be mapped - * - * @return mapped vector - */ - std::vector get_remapped_vector(std::vector); - -private: - static const char* default_path; - - /* - * Open the given file and parses it into a matrix - * - * @param path path of the calibration file - * - * @return -1 if file not found, 0 if everything ok. - */ - int init(std::string path); - - std::vector< std::vector > calibration_matrix; - - inline double convertToDouble(std::string const& s) - { - std::istringstream i(s); - double x; - if (!(i >> x)) - ROS_ERROR("Bad calibration file: %s", s.c_str()); - return x; - } -}; // end class - - -#endif // SR_REMAPPERS_CALIBRATION_PARSER_H_ diff --git a/sr_remappers/include/sr_remappers/shadowhand_to_cyberglove_remapper.h b/sr_remappers/include/sr_remappers/shadowhand_to_cyberglove_remapper.h deleted file mode 100644 index bde00223..00000000 --- a/sr_remappers/include/sr_remappers/shadowhand_to_cyberglove_remapper.h +++ /dev/null @@ -1,99 +0,0 @@ -/* -* @file shadowhand_to_cyberglove_remapper.h -* @author Ugo Cupcic , Contact -* @date Thu May 13 09:44:52 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief This program remaps the force information contained in -* /joint_states coming from the hand to the /cybergraspforces topic -* used to control the cybergrasp. -* -* -*/ - -#ifndef SR_REMAPPERS_SHADOWHAND_TO_CYBERGLOVE_REMAPPER_H_ -#define SR_REMAPPERS_SHADOWHAND_TO_CYBERGLOVE_REMAPPER_H_ - -// messages -#include -#include "sr_remappers/calibration_parser.h" -#include -#include - -namespace shadowhand_to_cyberglove_remapper -{ - -/* -* This program remaps the force information contained in -* /joint_states coming from the hand to the /cybergraspforces topic -* used to control the cybergrasp. -*/ -class ShadowhandToCybergloveRemapper -{ - public: - /* - * Init the publisher / subscriber, the joint names, read the calibratin matrix - */ - ShadowhandToCybergloveRemapper(); - ~ShadowhandToCybergloveRemapper() {} - private: - /* - * Number of joints in the hand - */ - static const unsigned int number_hand_joints; - - /* - * Init the vector containing the joints names - * - */ - void init_names(); - /// ROS node handles - ros::NodeHandle node, n_tilde; - /// Vector containing all the joints names for the shadowhand. - std::vector joints_names; - /// subscriber to the jointstates topic from the cyberglove - ros::Subscriber cyberglove_jointstates_sub; - /// publish to the shadowhand sendupdate topic - ros::Publisher shadowhand_pub; - /// the calibration parser containing the mapping matrix - CalibrationParser* calibration_parser; - - ///////////////// - // CALLBACKS // - ///////////////// - - /* - * process the joint_states callback: receives the message from the cyberglove node, remap it to the Dextrous hand and - * publish this message on a given topic - * - * @param msg the joint_states message - */ - void jointstatesCallback(const sensor_msgs::JointStateConstPtr& msg); - - /* - * process the joint_states callback for the finger abductions: processes the message from the cyberglove node, remap it to the Dextrous hand J4s - * It overwrites whatever was written for the J4s by the calibration parser get_remapped_vector - * - * @param msg the joint_states message - * @param vect the vector where the result is written (only J4s are written) - */ - void getAbductionJoints(const sensor_msgs::JointStateConstPtr& msg, std::vector& vect); -}; // end class - -} // namespace shadowhand_to_cyberglove_remapper - -#endif // SR_REMAPPERS_SHADOWHAND_TO_CYBERGLOVE_REMAPPER_H_ diff --git a/sr_remappers/include/sr_remappers/shadowhand_to_cybergrasp_remapper.h b/sr_remappers/include/sr_remappers/shadowhand_to_cybergrasp_remapper.h deleted file mode 100644 index c9b9ca7a..00000000 --- a/sr_remappers/include/sr_remappers/shadowhand_to_cybergrasp_remapper.h +++ /dev/null @@ -1,82 +0,0 @@ -/* -* @file shadowhand_to_cybergrasp_remapper.h -* @author Ugo Cupcic , Contact -* @date Thu May 13 09:44:52 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief This program remapps the force information contained in -* /joint_states coming from the hand to the /cybergraspforces topic -* used to control the cybergrasp. -* -* -*/ - -#ifndef SR_REMAPPERS_SHADOWHAND_TO_CYBERGRASP_REMAPPER_H_ -#define SR_REMAPPERS_SHADOWHAND_TO_CYBERGRASP_REMAPPER_H_ - -// messages -#include -#include - -#include "sr_remappers/calibration_parser.h" - - -namespace shadowhand_to_cybergrasp_remapper -{ - -/* -* @brief This program remaps the force information contained in -* /joint_states coming from the hand to the /cybergraspforces topic -* used to control the cybergrasp. -* -*/ -class ShadowhandToCybergraspRemapper -{ - public: - /* - * Init the publisher / subscriber, the joint names, read the calibratin matrix - */ - ShadowhandToCybergraspRemapper(); - ~ShadowhandToCybergraspRemapper(); - - private: - /// ROS node handles - ros::NodeHandle node, n_tilde; - - /// subscribe to the shadowhand sendupdate topic - ros::Subscriber shadowhand_jointstates_sub; - /// publish to the cybergrasp /cybergraspforces topic - ros::Publisher shadowhand_cybergrasp_pub; - /// the calibration parser containing the mapping matrix - CalibrationParser* calibration_parser; - - ///////////////// - // CALLBACKS // - ///////////////// - - /* - * process the joint_states callback: receives the message from the shadowhand node, remap it to the Cybergrasp and - * publish this message on a given topic - * - * @param msg the joint_states message - */ - void jointstatesCallback(const sensor_msgs::JointStateConstPtr& msg); -}; // end class - -} // namespace shadowhand_to_cybergrasp_remapper - -#endif // SR_REMAPPERS_SHADOWHAND_TO_CYBERGRASP_REMAPPER_H_ diff --git a/sr_remappers/launch/remapper_glove.launch b/sr_remappers/launch/remapper_glove.launch deleted file mode 100644 index 31cf6999..00000000 --- a/sr_remappers/launch/remapper_glove.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - diff --git a/sr_remappers/package.xml b/sr_remappers/package.xml deleted file mode 100644 index e2c83f24..00000000 --- a/sr_remappers/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - sr_remappers - 0.0.0 - Remaps data from and to the Shadow Robot hardware - - - Shadow Robot's software team - - - GPL - - catkin - - roscpp - roslint - rospy - std_msgs - sensor_msgs - sr_robot_msgs - cyberglove - sr_cyberglove_config - - roscpp - rospy - std_msgs - sensor_msgs - sr_robot_msgs - cyberglove - sr_cyberglove_config - diff --git a/sr_remappers/param/cyb.py b/sr_remappers/param/cyb.py deleted file mode 100644 index 6f52b077..00000000 --- a/sr_remappers/param/cyb.py +++ /dev/null @@ -1,40 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright 2011, 2022 Shadow Robot Company Ltd. -# -# This program is free software: you can redistribute it and/or modify it -# under the terms of the GNU General Public License as published by the Free -# Software Foundation version 2 of the License. -# -# This program is distributed in the hope that it will be useful, but WITHOUT -# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -# more details. -# -# You should have received a copy of the GNU General Public License along -# with this program. If not, see . -# - -with open("cyberglovetoshadowhand.map", 'r', encoding="utf8") as file_to_read: - matrix = [] - - next(file_to_read) - for line in file_to_read.readlines(): - line = line.strip("\n") - line = line.split(" ") - matrix.append(line) - -matrix_t = [] - -for i in range(0, len(matrix[0])): - matrix_t.append(list(range(0, len(matrix)))) - -for index_row, line in enumerate(matrix): - for index_column, _ in enumerate(line): - matrix_t[index_column][index_row] = matrix[index_row][index_column] - -with open("cyberglovetoshadowhand_transposed.map", 'w', encoding="utf8") as file_to_write: - for line in matrix_t: - for col in line: - file_to_write.write(col + " ") - file_to_write.write("\n") diff --git a/sr_remappers/param/cyberglovetoshadowhand.map b/sr_remappers/param/cyberglovetoshadowhand.map deleted file mode 100644 index 45bd26af..00000000 --- a/sr_remappers/param/cyberglovetoshadowhand.map +++ /dev/null @@ -1,21 +0,0 @@ -# Glove --> ThumbIJ ThumbMPJ ThumbAb ThumbRotate IndexDIJ IndexPIJ IndexMPJ MiddleIndexAb MiddleDIJ MiddlePIJ MiddleMPJ RingMiddleAb RingDIJ RingPIJ RingMPJ PinkieDIJ PinkiePIJ PinkieMPJ PinkieRingAb PalmArch WristPitch WristYaw -TH1 0.937 0.193 -0.486 -0.050 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.023 0.000 0.000 -TH2 -0.077 1.061 0.166 0.234 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.059 0.000 0.000 -TH3 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -TH4 -0.105 0.096 0.836 -0.096 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.146 0.000 0.000 -TH5 -0.041 -0.008 -0.023 0.941 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.135 0.000 0.000 -FF0 0.000 0.000 0.000 0.000 1.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -FF3 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -FF4 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -MF0 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -MF3 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -MF4 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -RF0 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -RF3 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -RF4 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -LF0 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 1.000 0.000 0.000 0.000 0.000 0.000 -LF3 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 -LF4 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -1.000 0.000 0.000 0.000 -LF5 0.018 0.091 -0.032 0.032 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.118 0.000 0.000 -WR1 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 -WR2 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 diff --git a/sr_remappers/param/cyberglovetoshadowhand_transposed.map b/sr_remappers/param/cyberglovetoshadowhand_transposed.map deleted file mode 100644 index 2db17406..00000000 --- a/sr_remappers/param/cyberglovetoshadowhand_transposed.map +++ /dev/null @@ -1,22 +0,0 @@ -0.937 -0.077 0.000 -0.105 -0.041 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.018 0.000 0.000 -0.193 1.061 0.000 0.096 -0.008 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.091 0.000 0.000 --0.486 0.166 0.000 0.836 -0.023 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.032 0.000 0.000 --0.050 0.234 0.000 -0.096 0.941 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.032 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 -1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 0.000 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 -1.000 0.000 0.000 0.000 -0.023 0.059 0.000 0.146 0.135 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.118 0.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 0.000 -0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 0.000 1.000 diff --git a/sr_remappers/param/shadowhandtocybergrasp.cal b/sr_remappers/param/shadowhandtocybergrasp.cal deleted file mode 100644 index d8f30206..00000000 --- a/sr_remappers/param/shadowhandtocybergrasp.cal +++ /dev/null @@ -1,31 +0,0 @@ -#TH FF MF RF LF (cybergrasp) -#FF (hand) -0.0 0.0 0.0 0.0 0.0 -0.0 1.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -#MF -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 1.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -#RF -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 1.0 0.0 -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -#LF -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 1.0 -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -#TH -1.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 -#WR -0.0 0.0 0.0 0.0 0.0 -0.0 0.0 0.0 0.0 0.0 diff --git a/sr_remappers/param/test.cal b/sr_remappers/param/test.cal deleted file mode 100644 index 9b301bdc..00000000 --- a/sr_remappers/param/test.cal +++ /dev/null @@ -1,6 +0,0 @@ -0 0 0 0 1 -0 0 0 0 0 -0 0 0 1 0 -0 0 1 0 0 -0 1 0 0 0 -1 0 0 0 0 diff --git a/sr_remappers/param/tmp.py b/sr_remappers/param/tmp.py deleted file mode 100644 index 1c13a9a1..00000000 --- a/sr_remappers/param/tmp.py +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python3 -# -# Copyright 2011, 2022 Shadow Robot Company Ltd. -# -# This program is free software: you can redistribute it and/or modify it -# under the terms of the GNU General Public License as published by the Free -# Software Foundation version 2 of the License. -# -# This program is distributed in the hope that it will be useful, but WITHOUT -# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -# more details. -# -# You should have received a copy of the GNU General Public License along -# with this program. If not, see . -# - -# pylint: disable-all - -lapin = ['FFJ1', 'FFJ2', 'FFJ3', 'FFJ4', 'MFJ1', 'MFJ2', 'MFJ3', 'MFJ4', 'RFJ1', 'RFJ2', 'RFJ3', 'RFJ4', 'LFJ1', 'LFJ2', - 'LFJ3', 'LFJ4', 'LFJ5', 'THJ1', 'THJ2', 'THJ3', 'THJ4', 'THJ5', 'WRJ1', 'WRJ2'] - -for lap in lapin: - if lap == 'THJ1': - print("1.0 ", end=' ') - else: - print("0.0 ", end=' ') -print("") - -for lap in lapin: - if lap == 'FFJ2': - print("1.0 ", end=' ') - else: - print("0.0 ", end=' ') -print("") - -for lap in lapin: - if lap == 'MFJ2': - print("1.0 ", end=' ') - else: - print("0.0 ", end=' ') -print("") - -for lap in lapin: - if lap == 'RFJ2': - print("1.0 ", end=' ') - else: - print("0.0 ", end=' ') -print("") - -for lap in lapin: - if lap == 'LFJ2': - print("1.0 ", end=' ') - else: - print("0.0 ", end=' ') diff --git a/sr_remappers/src/calibration_parser.cpp b/sr_remappers/src/calibration_parser.cpp deleted file mode 100644 index 4a435aed..00000000 --- a/sr_remappers/src/calibration_parser.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/* -* @file calibration_parser.cpp -* @author Ugo Cupcic , Contact -* @date Thu May 13 11:41:56 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief This is where the calibration matrix is read from a file, stored and where the actual mapping take place. -* -* -*/ - -#include - -#include -#include -#include -#include "sr_remappers/calibration_parser.h" -#include -#include -#include - -const char* CalibrationParser::default_path = "/etc/robot/mappings/default_mapping"; - -CalibrationParser::CalibrationParser() -{ - ROS_WARN("No calibration path was specified, using default path"); - init(default_path); -} - -CalibrationParser::CalibrationParser(std::string path) -{ - init(path); -} - -int CalibrationParser::init(std::string path) -{ - // reserve enough lines - calibration_matrix.reserve(25); - - std::ifstream calibration_file; - calibration_file.open(path.c_str()); - - // can't find the file - if (!calibration_file.is_open() ) - { - ROS_ERROR("Couldn't open the file %s", path.c_str()); - return -1; - } - - // we read the file and put all the data in this matrix - std::vector > tmp_matrix; - - std::string line; - while (!calibration_file.eof()) - { - getline(calibration_file, line); - - // remove leading and trailing whitespaces - line = boost::algorithm::trim_copy(line); - - // ignore empty line - if (line.size() == 0 ) - continue; - - // ignore comments - if (line[0] == '#' ) - continue; - - std::vector splitted_string; - boost::split(splitted_string, line, boost::is_any_of("\t ")); - - std::vector double_line(splitted_string.size()); - for (unsigned int index_col = 0; index_col < splitted_string.size(); ++index_col ) - double_line[index_col] = convertToDouble(splitted_string[index_col]); - - calibration_matrix.push_back(double_line); - } - calibration_file.close(); - - std::stringstream ss; - ss << "mapping matrix, from glove to hand" << std::endl; - for (unsigned int line = 0; line < calibration_matrix.size(); ++line ) - { - for (unsigned int col = 0; col < calibration_matrix[0].size(); ++col ) - { - ss << calibration_matrix[line][col] << " "; - } - ss << std::endl; - } - - ROS_DEBUG("%s", ss.str().c_str()); - return 0; -} - -std::vector CalibrationParser::get_remapped_vector(std::vector input_vector) -{ - // check the size of the matrix - if (input_vector.size() != calibration_matrix.size() ) - { - ROS_ERROR_STREAM("The size of the given vector doesn't correspond to the mapping: received " - << input_vector.size() - << ", wanted " - << calibration_matrix.size()); - return std::vector(calibration_matrix[0].size()); - } - - std::vector result(calibration_matrix[0].size()); - double tmp_value; - - for (unsigned int col = 0; col < calibration_matrix[0].size(); ++col ) - { - tmp_value = 0.0; - for (unsigned int index_vec = 0; index_vec < calibration_matrix.size(); ++index_vec ) - { - tmp_value += (input_vector[index_vec] * calibration_matrix[index_vec][col]); - } - result[col] = tmp_value; - } - - return result; -} diff --git a/sr_remappers/src/shadowhand_to_cyberglove_remapper.cpp b/sr_remappers/src/shadowhand_to_cyberglove_remapper.cpp deleted file mode 100644 index 8a85c3b5..00000000 --- a/sr_remappers/src/shadowhand_to_cyberglove_remapper.cpp +++ /dev/null @@ -1,175 +0,0 @@ -/* -* @file shadowhand_to_cyberglove_remapper.cpp -* @author Ugo Cupcic , Contact -* @date Thu May 13 09:44:52 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief This program remapps the force information contained in -* /joint_states coming from the hand to the /cybergraspforces topic -* used to control the cybergrasp. -* -* -*/ - -#include -#include -#include -#include "sr_remappers/shadowhand_to_cyberglove_remapper.h" -#include -#include - -namespace shadowhand_to_cyberglove_remapper -{ - -const unsigned int ShadowhandToCybergloveRemapper::number_hand_joints = 20; - -ShadowhandToCybergloveRemapper::ShadowhandToCybergloveRemapper() : - n_tilde("~") -{ - joints_names.resize(number_hand_joints); - ShadowhandToCybergloveRemapper::init_names(); - - std::string param; - std::string path; - n_tilde.searchParam("cyberglove_mapping_path", param); - n_tilde.param(param, path, std::string()); - calibration_parser = new CalibrationParser(path); - ROS_INFO("Mapping file loaded for the Cyberglove: %s", path.c_str()); - - std::string prefix; - std::string searched_param; - n_tilde.searchParam("cyberglove_prefix", searched_param); - n_tilde.param(searched_param, prefix, std::string()); - - std::string full_topic = prefix + "/calibrated/joint_states"; - - cyberglove_jointstates_sub = node.subscribe(full_topic, 10, &ShadowhandToCybergloveRemapper::jointstatesCallback, - this); - - n_tilde.searchParam("sendupdate_prefix", searched_param); - n_tilde.param(searched_param, prefix, std::string()); - full_topic = prefix + "sendupdate"; - - shadowhand_pub = node.advertise (full_topic, 5); -} - -void ShadowhandToCybergloveRemapper::init_names() -{ - joints_names[0] = "THJ1"; - joints_names[1] = "THJ2"; - joints_names[2] = "THJ3"; - joints_names[3] = "THJ4"; - joints_names[4] = "THJ5"; - joints_names[5] = "FFJ0"; - joints_names[6] = "FFJ3"; - joints_names[7] = "FFJ4"; - joints_names[8] = "MFJ0"; - joints_names[9] = "MFJ3"; - joints_names[10] = "MFJ4"; - joints_names[11] = "RFJ0"; - joints_names[12] = "RFJ3"; - joints_names[13] = "RFJ4"; - joints_names[14] = "LFJ0"; - joints_names[15] = "LFJ3"; - joints_names[16] = "LFJ4"; - joints_names[17] = "LFJ5"; - joints_names[18] = "WRJ1"; - joints_names[19] = "WRJ2"; -} - -void ShadowhandToCybergloveRemapper::jointstatesCallback(const sensor_msgs::JointStateConstPtr& msg) -{ - sr_robot_msgs::joint joint; - sr_robot_msgs::sendupdate pub; - - // Do conversion - std::vector vect = calibration_parser->get_remapped_vector(msg->position); - - // Process J4's - getAbductionJoints(msg, vect); - - // Generate sendupdate message - pub.sendupdate_length = number_hand_joints; - - std::vector table(number_hand_joints); - for (unsigned int i = 0; i < number_hand_joints; ++i ) - { - joint.joint_name = joints_names[i]; - joint.joint_target = vect[i]; - table[i] = joint; - } - pub.sendupdate_length = number_hand_joints; - pub.sendupdate_list = table; - shadowhand_pub.publish(pub); -} - -void ShadowhandToCybergloveRemapper::getAbductionJoints(const sensor_msgs::JointStateConstPtr& msg, - std::vector& vect) -{ - double middleIndexAb = msg->position[10]; - double ringMiddleAb = msg->position[14]; - double pinkieRingAb = msg->position[18]; - - // if the abduction sensors are less than 0, it is an artifact of the calibration (we don't want to consider anything - // smaller than 0 for these sensors) - if (middleIndexAb < 0.0) - middleIndexAb = 0.0; - if (ringMiddleAb < 0.0) - ringMiddleAb = 0.0; - if (pinkieRingAb < 0.0) - pinkieRingAb = 0.0; - - // Add the 3 abduction angles to have an idea of where the centre lies - double ab_total = middleIndexAb + ringMiddleAb + pinkieRingAb; - - // When trying to understand this code bear in mind that the abduction sign convention - // in the shadow hand is the opposite for ff and mf than for rf and lf. - if (ab_total/2 < middleIndexAb) // If the centre lies between ff and mf - { - // FFJ4 - vect[7] = -ab_total/2; - // MFJ4 - vect[10] = middleIndexAb - ab_total/2; - // RFJ4 - vect[13] = -(ringMiddleAb + vect[10]); - // LFJ4 - vect[16] = -pinkieRingAb + vect[13]; - } - else if (ab_total/2 < middleIndexAb + ringMiddleAb) // If the centre lies between mf and rf - { - // MFJ4 - vect[10] = -(ab_total/2 - middleIndexAb); - // FFJ4 - vect[7] = -middleIndexAb + vect[10]; - // RFJ4 - vect[13] = -(ringMiddleAb + vect[10]); - // LFJ4 - vect[16] = -pinkieRingAb + vect[13]; - } - else // If the centre lies between rf and lf - { - // LFJ4 - vect[16] = -ab_total/2; - // RFJ4 - vect[13] = pinkieRingAb + vect[16]; - // MFJ4 - vect[10] = -(ringMiddleAb + vect[13]); - // FFJ4 - vect[7] = -middleIndexAb + vect[10]; - } -} -} // namespace shadowhand_to_cyberglove_remapper diff --git a/sr_remappers/src/shadowhand_to_cyberglove_remapper_node.cpp b/sr_remappers/src/shadowhand_to_cyberglove_remapper_node.cpp deleted file mode 100644 index 85e9d692..00000000 --- a/sr_remappers/src/shadowhand_to_cyberglove_remapper_node.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/* -* @file shadowhand_to_cyberglove_remapper_node.cpp -* @author Ugo Cupcic , Contact -* @date Thu May 13 10:39:44 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief Launch a ros node to remap data coming from the Cyberglove to the Dextrous Hand. -* -* -*/ - -#include - -#include "sr_remappers/shadowhand_to_cyberglove_remapper.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "cyberglove_remapper"); - - shadowhand_to_cyberglove_remapper::ShadowhandToCybergloveRemapper remapper; - ros::spin(); - - return 0; -} diff --git a/sr_remappers/src/shadowhand_to_cybergrasp_remapper.cpp b/sr_remappers/src/shadowhand_to_cybergrasp_remapper.cpp deleted file mode 100644 index 113da0e3..00000000 --- a/sr_remappers/src/shadowhand_to_cybergrasp_remapper.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* -* @file shadowhand_to_cybergrasp_remapper.cpp -* @author Ugo Cupcic , Contact -* @date Thu May 13 09:44:52 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief This program remapps the force information contained in -* /joint_states coming from the hand to the /cybergraspforces topic -* used to control the cybergrasp. -* -* -*/ - -#include -#include -#include -#include -#include "sr_remappers/shadowhand_to_cybergrasp_remapper.h" - -namespace shadowhand_to_cybergrasp_remapper -{ - -ShadowhandToCybergraspRemapper::ShadowhandToCybergraspRemapper() - : n_tilde("~"), publish_rate(0.0) -{ - std::string searched_param; - - // load the calibration - std::string path; - n_tilde.searchParam("cybergrasp_calibration_path", searched_param); - n_tilde.param(searched_param, path, std::string()); - - calibration_parser = new CalibrationParser(path); - - // publish to cybergraspforces topic - std::string prefix; - n_tilde.searchParam("cybergrasp_prefix", searched_param); - n_tilde.param(searched_param, prefix, std::string()); - std::string full_topic = prefix + "cybergraspforces"; - - shadowhand_cybergrasp_pub = node.advertise(full_topic, 20); - - // subscribe to joint_states topic - n_tilde.searchParam("joint_states_prefix", searched_param); - n_tilde.param(searched_param, prefix, std::string()); - - full_topic = prefix + "joint_states"; - - shadowhand_jointstates_sub = node.subscribe(full_topic, 10, &ShadowhandToCybergraspRemapper::jointstatesCallback, - this); -} - -ShadowhandToCybergraspRemapper::~ShadowhandToCybergraspRemapper() -{ - if (calibration_parser ) - delete calibration_parser; -} - -/* - * process the joint_states callback: get the force data from the - * finger tips and stream it to the cybergrasp - * - * @param msg the joint_states message - */ -void ShadowhandToCybergraspRemapper::jointstatesCallback(const sensor_msgs::JointStateConstPtr& msg) -{ - // read msg and remap the vector to the cybergrasp - cybergrasp::cybergraspforces cybergrasp_msg; - - double average = 0.0; - - for (unsigned int i=0; i < msg->effort.size(); ++i) - { - average = msg->effort[i]*10.0; - } - - average /= msg->effort.size(); - - average /= msg->effort.size(); - average *= 1000.0; - - for (unsigned int i=0; i < 5; ++i) - { - cybergrasp_msg.forces[i] = 0.01; - } - - // return the max of the read values on the sensor - cybergrasp_msg.forces[1] = (average - 0.016)*2.0; - // send vector to cybergrasp - shadowhand_cybergrasp_pub.publish(cybergrasp_msg); - - // sleep(2); -} -} // namespace shadowhand_to_cybergrasp_remapper diff --git a/sr_remappers/src/shadowhand_to_cybergrasp_remapper_node.cpp b/sr_remappers/src/shadowhand_to_cybergrasp_remapper_node.cpp deleted file mode 100644 index e782a2ec..00000000 --- a/sr_remappers/src/shadowhand_to_cybergrasp_remapper_node.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/* -* @file shadowhand_to_cybergrasp_remapper_node.cpp -* @author Ugo Cupcic , Contact -* @date Thu May 13 10:39:44 2010 -* -* -* Copyright 2011 Shadow Robot Company Ltd. -* -* This program is free software: you can redistribute it and/or modify it -* under the terms of the GNU General Public License as published by the Free -* Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, but WITHOUT -* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for -* more details. -* -* You should have received a copy of the GNU General Public License along -* with this program. If not, see . -* -* @brief Launch a ros node to remap data coming from the Dextrous Hand to the Cybergrasp. -* -* -*/ - -#include - -#include "sr_remappers/shadowhand_to_cybergrasp_remapper.h" - - - ///////////////////////////////// - // MAIN // - ///////////////////////////////// - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "shadowhand_listener"); - - shadowhand_to_cybergrasp_remapper::ShadowhandToCybergraspRemapper remapper; - ros::spin(); - - return 0; -}