From 371446c92bd7579e08894cc58c10c52b15f6ffb7 Mon Sep 17 00:00:00 2001 From: Geoff Viola Date: Tue, 24 May 2016 11:31:19 -0600 Subject: [PATCH 1/9] added new phidgets_high_speed_encoder package --- phidgets_high_speed_encoder/CMakeLists.txt | 37 +++ .../msg/encoder_params.msg | 5 + phidgets_high_speed_encoder/package.xml | 26 ++ .../src/high_speed_encoder.cpp | 242 ++++++++++++++++++ 4 files changed, 310 insertions(+) create mode 100644 phidgets_high_speed_encoder/CMakeLists.txt create mode 100644 phidgets_high_speed_encoder/msg/encoder_params.msg create mode 100644 phidgets_high_speed_encoder/package.xml create mode 100644 phidgets_high_speed_encoder/src/high_speed_encoder.cpp diff --git a/phidgets_high_speed_encoder/CMakeLists.txt b/phidgets_high_speed_encoder/CMakeLists.txt new file mode 100644 index 0000000..bb698f2 --- /dev/null +++ b/phidgets_high_speed_encoder/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.8.3) +project(phidgets_high_speed_encoder) + +find_package(catkin REQUIRED COMPONENTS phidgets_api roscpp std_msgs message_generation) + +add_message_files( + FILES + encoder_params.msg +) + +generate_messages( + DEPENDENCIES + std_msgs +) + +catkin_package( + CATKIN_DEPENDS phidgets_api roscpp std_msgs message_runtime +) + +message("catkin_INCLUDE_DIRS=" ${catkin_INCLUDE_DIRS}) +include_directories(${catkin_INCLUDE_DIRS}) + +add_executable(high_speed_encoder src/high_speed_encoder.cpp) +target_link_libraries(high_speed_encoder ${catkin_LIBRARIES}) +add_dependencies(high_speed_encoder ${catkin_EXPORTED_TARGETS}) +add_dependencies(high_speed_encoder phidgets_high_speed_encoder_generate_messages) + +install(TARGETS high_speed_encoder + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) + diff --git a/phidgets_high_speed_encoder/msg/encoder_params.msg b/phidgets_high_speed_encoder/msg/encoder_params.msg new file mode 100644 index 0000000..1f7919c --- /dev/null +++ b/phidgets_high_speed_encoder/msg/encoder_params.msg @@ -0,0 +1,5 @@ +Header header +int32 index +int32 count +int32 count_change +int32 time diff --git a/phidgets_high_speed_encoder/package.xml b/phidgets_high_speed_encoder/package.xml new file mode 100644 index 0000000..2b14cdd --- /dev/null +++ b/phidgets_high_speed_encoder/package.xml @@ -0,0 +1,26 @@ + + + phidgets_high_speed_encoder + 0.2.2 + Driver for the Phidgets high speed encoder devices + + Geoff Viola + + BSD + + Geoff Viola + + catkin + phidgets_api + roscpp + std_msgs + message_generation + phidgets_api + roscpp + std_msgs + message_generation + message_runtime + + + + diff --git a/phidgets_high_speed_encoder/src/high_speed_encoder.cpp b/phidgets_high_speed_encoder/src/high_speed_encoder.cpp new file mode 100644 index 0000000..1100a6d --- /dev/null +++ b/phidgets_high_speed_encoder/src/high_speed_encoder.cpp @@ -0,0 +1,242 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * ROS driver for Phidgets high speed encoder + * Copyright (c) 2010, Bob Mottram + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include +#include +#include +#include "phidgets_high_speed_encoder/encoder_params.h" + +// Handle +CPhidgetEncoderHandle phid; + +// encoder state publisher +ros::Publisher encoder_pub; + +bool initialised = false; + +int AttachHandler(CPhidgetHandle phid, void *userptr) +{ + int serial_number; + const char *name; + + CPhidget_getDeviceName (phid, &name); + CPhidget_getSerialNumber(phid, &serial_number); + ROS_INFO("%s Serial number %d attached!", + name, serial_number); + + return 0; +} + +int DetachHandler(CPhidgetHandle phid, void *userptr) +{ + int serial_number; + const char *name; + + CPhidget_getDeviceName (phid, &name); + CPhidget_getSerialNumber(phid, &serial_number); + ROS_INFO("%s Serial number %d detached!", + name, serial_number); + + return 0; +} + +int ErrorHandler(CPhidgetHandle phid, void *userptr, + int ErrorCode, const char *Description) +{ + ROS_INFO("Error handled. %d - %s", ErrorCode, Description); + return 0; +} + +int InputChangeHandler(CPhidgetEncoderHandle ENC, + void *usrptr, int Index, int State) +{ + return 0; +} + +int PositionChangeHandler(CPhidgetEncoderHandle ENC, + void *usrptr, int Index, + int Time, int RelativePosition) +{ + int Position; + CPhidgetEncoder_getPosition(ENC, Index, &Position); + + phidgets_high_speed_encoder::encoder_params e; + e.index = Index; + e.count = Position; + e.count_change = RelativePosition; + e.time = Time; + if (initialised) encoder_pub.publish(e); + ROS_INFO("Encoder %d Count %d", Index, Position); + return 0; +} + +int display_properties(CPhidgetEncoderHandle phid) +{ + int serial_number, version, num_encoders, num_inputs; + const char* ptr; + + CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr); + CPhidget_getSerialNumber((CPhidgetHandle)phid, + &serial_number); + CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version); + + CPhidgetEncoder_getInputCount(phid, &num_inputs); + CPhidgetEncoder_getEncoderCount(phid, &num_encoders); + + ROS_INFO("%s", ptr); + ROS_INFO("Serial Number: %d", serial_number); + ROS_INFO("Version: %d", version); + ROS_INFO("Number of encoders %d", num_encoders); + + return 0; +} + +bool attach( + CPhidgetEncoderHandle &phid, + int serial_number) +{ + // create the object + CPhidgetEncoder_create(&phid); + + // Set the handlers to be run when the device is + // plugged in or opened from software, unplugged or + // closed from software, or generates an error. + CPhidget_set_OnAttach_Handler((CPhidgetHandle)phid, + AttachHandler, NULL); + CPhidget_set_OnDetach_Handler((CPhidgetHandle)phid, + DetachHandler, NULL); + CPhidget_set_OnError_Handler((CPhidgetHandle)phid, + ErrorHandler, NULL); + + // Registers a callback that will run if an input changes. + // Requires the handle for the Phidget, the function + // that will be called, and an arbitrary pointer that + // will be supplied to the callback function (may be NULL). + CPhidgetEncoder_set_OnInputChange_Handler(phid, + InputChangeHandler, + NULL); + + // Registers a callback that will run if the + // encoder changes. + // Requires the handle for the Encoder, the function + // that will be called, and an arbitrary pointer that + // will be supplied to the callback function (may be NULL). + CPhidgetEncoder_set_OnPositionChange_Handler (phid, + PositionChangeHandler, + NULL); + + //open the device for connections + CPhidget_open((CPhidgetHandle)phid, serial_number); + + // get the program to wait for an encoder device + // to be attached + if (serial_number == -1) { + ROS_INFO("Waiting for High Speed Encoder Phidget " \ + "to be attached...."); + } + else { + ROS_INFO("Waiting for High Speed Encoder Phidget " \ + "%d to be attached....", serial_number); + } + int result; + if((result = + CPhidget_waitForAttachment((CPhidgetHandle)phid, + 10000))) + { + const char *err; + CPhidget_getErrorDescription(result, &err); + ROS_ERROR("Problem waiting for attachment: %s", + err); + return false; + } + else return true; +} + +/*! + * \brief disconnect the encoder + */ +void disconnect(CPhidgetEncoderHandle &phid) +{ + ROS_INFO("Closing..."); + CPhidget_close((CPhidgetHandle)phid); + CPhidget_delete((CPhidgetHandle)phid); +} + +int main(int argc, char* argv[]) +{ + ros::init(argc, argv, "phidgets_high_speed_encoder"); + ros::NodeHandle n; + ros::NodeHandle nh("~"); + int serial_number = -1; + nh.getParam("serial", serial_number); + std::string name = "encoder"; + nh.getParam("name", name); + if (serial_number==-1) { + nh.getParam("serial_number", serial_number); + } + std::string topic_path = "phidgets/"; + nh.getParam("topic_path", topic_path); + + int frequency = 30; + nh.getParam("frequency", frequency); + + if (attach(phid, serial_number)) { + display_properties(phid); + + const int buffer_length = 100; + std::string topic_name = topic_path + name; + if (serial_number > -1) { + char ser[10]; + sprintf(ser,"%d", serial_number); + topic_name += "/"; + topic_name += ser; + } + encoder_pub = + n.advertise(topic_name, + buffer_length); + ros::Rate loop_rate(frequency); + + initialised = true; + + while (ros::ok()) { + ros::spinOnce(); + loop_rate.sleep(); + } + + disconnect(phid); + } + return 0; +} + From 09fcc082e185b3773e19e9dce1bd86a7ae067b80 Mon Sep 17 00:00:00 2001 From: Geoff Viola Date: Tue, 24 May 2016 11:54:52 -0600 Subject: [PATCH 2/9] deleted unnecessary print CMakeLists --- phidgets_high_speed_encoder/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/phidgets_high_speed_encoder/CMakeLists.txt b/phidgets_high_speed_encoder/CMakeLists.txt index bb698f2..a0b8386 100644 --- a/phidgets_high_speed_encoder/CMakeLists.txt +++ b/phidgets_high_speed_encoder/CMakeLists.txt @@ -17,7 +17,6 @@ catkin_package( CATKIN_DEPENDS phidgets_api roscpp std_msgs message_runtime ) -message("catkin_INCLUDE_DIRS=" ${catkin_INCLUDE_DIRS}) include_directories(${catkin_INCLUDE_DIRS}) add_executable(high_speed_encoder src/high_speed_encoder.cpp) From 4e38b30eb9f25b10db6ff4d0db246de4855699d6 Mon Sep 17 00:00:00 2001 From: Geoff Viola Date: Tue, 24 May 2016 15:15:43 -0600 Subject: [PATCH 3/9] cleaned up message generation stuff and bogus installation --- phidgets_high_speed_encoder/CMakeLists.txt | 4 ---- phidgets_high_speed_encoder/package.xml | 1 - 2 files changed, 5 deletions(-) diff --git a/phidgets_high_speed_encoder/CMakeLists.txt b/phidgets_high_speed_encoder/CMakeLists.txt index a0b8386..469ddcb 100644 --- a/phidgets_high_speed_encoder/CMakeLists.txt +++ b/phidgets_high_speed_encoder/CMakeLists.txt @@ -30,7 +30,3 @@ install(TARGETS high_speed_encoder RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) - diff --git a/phidgets_high_speed_encoder/package.xml b/phidgets_high_speed_encoder/package.xml index 2b14cdd..6fed7c6 100644 --- a/phidgets_high_speed_encoder/package.xml +++ b/phidgets_high_speed_encoder/package.xml @@ -18,7 +18,6 @@ phidgets_api roscpp std_msgs - message_generation message_runtime From c8b6c3c11ab0d39f8dd3869fa87af664ca3722a6 Mon Sep 17 00:00:00 2001 From: Geoff Viola Date: Wed, 25 May 2016 10:46:31 -0600 Subject: [PATCH 4/9] removed frequency customization --- phidgets_high_speed_encoder/src/high_speed_encoder.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/phidgets_high_speed_encoder/src/high_speed_encoder.cpp b/phidgets_high_speed_encoder/src/high_speed_encoder.cpp index 1100a6d..3fe2478 100644 --- a/phidgets_high_speed_encoder/src/high_speed_encoder.cpp +++ b/phidgets_high_speed_encoder/src/high_speed_encoder.cpp @@ -209,9 +209,6 @@ int main(int argc, char* argv[]) std::string topic_path = "phidgets/"; nh.getParam("topic_path", topic_path); - int frequency = 30; - nh.getParam("frequency", frequency); - if (attach(phid, serial_number)) { display_properties(phid); @@ -226,14 +223,10 @@ int main(int argc, char* argv[]) encoder_pub = n.advertise(topic_name, buffer_length); - ros::Rate loop_rate(frequency); initialised = true; - while (ros::ok()) { - ros::spinOnce(); - loop_rate.sleep(); - } + ros::spin(); disconnect(phid); } From 05a4aa2b62d5e2bf94b5ab888796b3fe77c45d65 Mon Sep 17 00:00:00 2001 From: Geoff Viola Date: Wed, 25 May 2016 14:10:36 -0600 Subject: [PATCH 5/9] renamed node with phidgets_ --- phidgets_high_speed_encoder/CMakeLists.txt | 10 +++++----- ...eed_encoder.cpp => phidgets_high_speed_encoder.cpp} | 0 2 files changed, 5 insertions(+), 5 deletions(-) rename phidgets_high_speed_encoder/src/{high_speed_encoder.cpp => phidgets_high_speed_encoder.cpp} (100%) diff --git a/phidgets_high_speed_encoder/CMakeLists.txt b/phidgets_high_speed_encoder/CMakeLists.txt index 469ddcb..5c7efad 100644 --- a/phidgets_high_speed_encoder/CMakeLists.txt +++ b/phidgets_high_speed_encoder/CMakeLists.txt @@ -19,12 +19,12 @@ catkin_package( include_directories(${catkin_INCLUDE_DIRS}) -add_executable(high_speed_encoder src/high_speed_encoder.cpp) -target_link_libraries(high_speed_encoder ${catkin_LIBRARIES}) -add_dependencies(high_speed_encoder ${catkin_EXPORTED_TARGETS}) -add_dependencies(high_speed_encoder phidgets_high_speed_encoder_generate_messages) +add_executable(phidgets_high_speed_encoder src/phidgets_high_speed_encoder.cpp) +target_link_libraries(phidgets_high_speed_encoder ${catkin_LIBRARIES}) +add_dependencies(phidgets_high_speed_encoder ${catkin_EXPORTED_TARGETS}) +add_dependencies(phidgets_high_speed_encoder phidgets_high_speed_encoder_generate_messages) -install(TARGETS high_speed_encoder +install(TARGETS phidgets_high_speed_encoder ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/phidgets_high_speed_encoder/src/high_speed_encoder.cpp b/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp similarity index 100% rename from phidgets_high_speed_encoder/src/high_speed_encoder.cpp rename to phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp From 400e92abfc45c613f14cacfe2d9540aab3b375b2 Mon Sep 17 00:00:00 2001 From: Geoff Viola Date: Thu, 26 May 2016 17:17:11 -0600 Subject: [PATCH 6/9] populate the header --- .../src/phidgets_high_speed_encoder.cpp | 27 +++++++++++-------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp b/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp index 3fe2478..7edc4c4 100644 --- a/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp +++ b/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp @@ -38,13 +38,11 @@ #include #include "phidgets_high_speed_encoder/encoder_params.h" -// Handle -CPhidgetEncoderHandle phid; - -// encoder state publisher -ros::Publisher encoder_pub; - -bool initialised = false; +static CPhidgetEncoderHandle phid; +static ros::Publisher encoder_pub; +static bool initialised = false; +static std::string frame_id; +static bool inverted = false; int AttachHandler(CPhidgetHandle phid, void *userptr) { @@ -89,16 +87,20 @@ int PositionChangeHandler(CPhidgetEncoderHandle ENC, void *usrptr, int Index, int Time, int RelativePosition) { + static uint32_t sequence_number = 0U; int Position; CPhidgetEncoder_getPosition(ENC, Index, &Position); phidgets_high_speed_encoder::encoder_params e; + e.header.stamp = ros::Time::now(); + e.header.frame_id = frame_id; + e.header.seq = sequence_number++; e.index = Index; - e.count = Position; - e.count_change = RelativePosition; + e.count = (inverted ? -Position : Position); + e.count_change = (inverted ? -RelativePosition : RelativePosition); e.time = Time; if (initialised) encoder_pub.publish(e); - ROS_INFO("Encoder %d Count %d", Index, Position); + ROS_DEBUG("Encoder %d Count %d", Index, Position); return 0; } @@ -205,9 +207,12 @@ int main(int argc, char* argv[]) nh.getParam("name", name); if (serial_number==-1) { nh.getParam("serial_number", serial_number); - } + } ROS_INFO("%s, %d", frame_id.c_str(), (inverted ? 1ff : 0) ); + std::string topic_path = "phidgets/"; nh.getParam("topic_path", topic_path); + nh.getParam("frame_id", frame_id); + nh.getParam("inverted", inverted); if (attach(phid, serial_number)) { display_properties(phid); From cb616b5c1641cbe0933bb23f6356d0f79ebac817 Mon Sep 17 00:00:00 2001 From: Geoff Viola Date: Fri, 27 May 2016 11:01:47 -0600 Subject: [PATCH 7/9] reformatted and added print at startup --- .../src/phidgets_high_speed_encoder.cpp | 304 +++++++++--------- 1 file changed, 157 insertions(+), 147 deletions(-) diff --git a/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp b/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp index 7edc4c4..6e316ba 100644 --- a/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp +++ b/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp @@ -46,144 +46,146 @@ static bool inverted = false; int AttachHandler(CPhidgetHandle phid, void *userptr) { - int serial_number; - const char *name; + int serial_number; + const char *name; - CPhidget_getDeviceName (phid, &name); - CPhidget_getSerialNumber(phid, &serial_number); - ROS_INFO("%s Serial number %d attached!", - name, serial_number); + CPhidget_getDeviceName(phid, &name); + CPhidget_getSerialNumber(phid, &serial_number); + ROS_INFO("%s Serial number %d attached!", + name, serial_number); - return 0; + return 0; } int DetachHandler(CPhidgetHandle phid, void *userptr) { - int serial_number; - const char *name; + int serial_number; + const char *name; - CPhidget_getDeviceName (phid, &name); - CPhidget_getSerialNumber(phid, &serial_number); - ROS_INFO("%s Serial number %d detached!", - name, serial_number); + CPhidget_getDeviceName(phid, &name); + CPhidget_getSerialNumber(phid, &serial_number); + ROS_INFO("%s Serial number %d detached!", + name, serial_number); - return 0; + return 0; } int ErrorHandler(CPhidgetHandle phid, void *userptr, - int ErrorCode, const char *Description) + int ErrorCode, const char *Description) { - ROS_INFO("Error handled. %d - %s", ErrorCode, Description); - return 0; + ROS_INFO("Error handled. %d - %s", ErrorCode, Description); + return 0; } int InputChangeHandler(CPhidgetEncoderHandle ENC, - void *usrptr, int Index, int State) + void *usrptr, int Index, int State) { - return 0; + return 0; } int PositionChangeHandler(CPhidgetEncoderHandle ENC, - void *usrptr, int Index, - int Time, int RelativePosition) + void *usrptr, int Index, + int Time, int RelativePosition) { - static uint32_t sequence_number = 0U; - int Position; - CPhidgetEncoder_getPosition(ENC, Index, &Position); - - phidgets_high_speed_encoder::encoder_params e; - e.header.stamp = ros::Time::now(); - e.header.frame_id = frame_id; - e.header.seq = sequence_number++; - e.index = Index; - e.count = (inverted ? -Position : Position); - e.count_change = (inverted ? -RelativePosition : RelativePosition); - e.time = Time; - if (initialised) encoder_pub.publish(e); - ROS_DEBUG("Encoder %d Count %d", Index, Position); - return 0; + static uint32_t sequence_number = 0U; + int Position; + CPhidgetEncoder_getPosition(ENC, Index, &Position); + + phidgets_high_speed_encoder::encoder_params e; + e.header.stamp = ros::Time::now(); + e.header.frame_id = frame_id; + e.header.seq = sequence_number++; + e.index = Index; + e.count = (inverted ? -Position : Position); + e.count_change = (inverted ? -RelativePosition : RelativePosition); + e.time = Time; + if (initialised) encoder_pub.publish(e); + ROS_DEBUG("Encoder %d Count %d", Index, Position); + return 0; } int display_properties(CPhidgetEncoderHandle phid) { - int serial_number, version, num_encoders, num_inputs; - const char* ptr; + int serial_number, version, num_encoders, num_inputs; + const char *ptr; - CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr); - CPhidget_getSerialNumber((CPhidgetHandle)phid, - &serial_number); - CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version); + CPhidget_getDeviceType((CPhidgetHandle) phid, &ptr); + CPhidget_getSerialNumber((CPhidgetHandle) phid, + &serial_number); + CPhidget_getDeviceVersion((CPhidgetHandle) phid, &version); - CPhidgetEncoder_getInputCount(phid, &num_inputs); - CPhidgetEncoder_getEncoderCount(phid, &num_encoders); + CPhidgetEncoder_getInputCount(phid, &num_inputs); + CPhidgetEncoder_getEncoderCount(phid, &num_encoders); - ROS_INFO("%s", ptr); - ROS_INFO("Serial Number: %d", serial_number); - ROS_INFO("Version: %d", version); - ROS_INFO("Number of encoders %d", num_encoders); + ROS_INFO("%s", ptr); + ROS_INFO("Serial Number: %d", serial_number); + ROS_INFO("Version: %d", version); + ROS_INFO("Number of encoders %d", num_encoders); - return 0; + return 0; } bool attach( - CPhidgetEncoderHandle &phid, - int serial_number) + CPhidgetEncoderHandle &phid, + int serial_number) { - // create the object - CPhidgetEncoder_create(&phid); - - // Set the handlers to be run when the device is - // plugged in or opened from software, unplugged or - // closed from software, or generates an error. - CPhidget_set_OnAttach_Handler((CPhidgetHandle)phid, - AttachHandler, NULL); - CPhidget_set_OnDetach_Handler((CPhidgetHandle)phid, - DetachHandler, NULL); - CPhidget_set_OnError_Handler((CPhidgetHandle)phid, - ErrorHandler, NULL); - - // Registers a callback that will run if an input changes. - // Requires the handle for the Phidget, the function - // that will be called, and an arbitrary pointer that - // will be supplied to the callback function (may be NULL). - CPhidgetEncoder_set_OnInputChange_Handler(phid, - InputChangeHandler, - NULL); - - // Registers a callback that will run if the - // encoder changes. - // Requires the handle for the Encoder, the function - // that will be called, and an arbitrary pointer that - // will be supplied to the callback function (may be NULL). - CPhidgetEncoder_set_OnPositionChange_Handler (phid, - PositionChangeHandler, - NULL); - - //open the device for connections - CPhidget_open((CPhidgetHandle)phid, serial_number); - - // get the program to wait for an encoder device - // to be attached - if (serial_number == -1) { - ROS_INFO("Waiting for High Speed Encoder Phidget " \ - "to be attached...."); - } - else { - ROS_INFO("Waiting for High Speed Encoder Phidget " \ - "%d to be attached....", serial_number); - } - int result; - if((result = - CPhidget_waitForAttachment((CPhidgetHandle)phid, - 10000))) - { - const char *err; - CPhidget_getErrorDescription(result, &err); - ROS_ERROR("Problem waiting for attachment: %s", - err); - return false; - } - else return true; + // create the object + CPhidgetEncoder_create(&phid); + + // Set the handlers to be run when the device is + // plugged in or opened from software, unplugged or + // closed from software, or generates an error. + CPhidget_set_OnAttach_Handler((CPhidgetHandle) phid, + AttachHandler, NULL); + CPhidget_set_OnDetach_Handler((CPhidgetHandle) phid, + DetachHandler, NULL); + CPhidget_set_OnError_Handler((CPhidgetHandle) phid, + ErrorHandler, NULL); + + // Registers a callback that will run if an input changes. + // Requires the handle for the Phidget, the function + // that will be called, and an arbitrary pointer that + // will be supplied to the callback function (may be NULL). + CPhidgetEncoder_set_OnInputChange_Handler(phid, + InputChangeHandler, + NULL); + + // Registers a callback that will run if the + // encoder changes. + // Requires the handle for the Encoder, the function + // that will be called, and an arbitrary pointer that + // will be supplied to the callback function (may be NULL). + CPhidgetEncoder_set_OnPositionChange_Handler(phid, + PositionChangeHandler, + NULL); + + //open the device for connections + CPhidget_open((CPhidgetHandle) phid, serial_number); + + // get the program to wait for an encoder device + // to be attached + if (serial_number == -1) + { + ROS_INFO("Waiting for High Speed Encoder Phidget " \ + "to be attached...."); + } + else + { + ROS_INFO("Waiting for High Speed Encoder Phidget " \ + "%d to be attached....", serial_number); + } + int result; + if ((result = + CPhidget_waitForAttachment((CPhidgetHandle) phid, + 10000))) + { + const char *err; + CPhidget_getErrorDescription(result, &err); + ROS_ERROR("Problem waiting for attachment: %s", + err); + return false; + } + else return true; } /*! @@ -191,50 +193,58 @@ bool attach( */ void disconnect(CPhidgetEncoderHandle &phid) { - ROS_INFO("Closing..."); - CPhidget_close((CPhidgetHandle)phid); - CPhidget_delete((CPhidgetHandle)phid); + ROS_INFO("Closing..."); + CPhidget_close((CPhidgetHandle) phid); + CPhidget_delete((CPhidgetHandle) phid); } -int main(int argc, char* argv[]) +int main(int argc, char *argv[]) { - ros::init(argc, argv, "phidgets_high_speed_encoder"); - ros::NodeHandle n; - ros::NodeHandle nh("~"); - int serial_number = -1; - nh.getParam("serial", serial_number); - std::string name = "encoder"; - nh.getParam("name", name); - if (serial_number==-1) { - nh.getParam("serial_number", serial_number); - } ROS_INFO("%s, %d", frame_id.c_str(), (inverted ? 1ff : 0) ); - - std::string topic_path = "phidgets/"; - nh.getParam("topic_path", topic_path); - nh.getParam("frame_id", frame_id); - nh.getParam("inverted", inverted); - - if (attach(phid, serial_number)) { - display_properties(phid); - - const int buffer_length = 100; - std::string topic_name = topic_path + name; - if (serial_number > -1) { - char ser[10]; - sprintf(ser,"%d", serial_number); - topic_name += "/"; - topic_name += ser; - } - encoder_pub = - n.advertise(topic_name, - buffer_length); - - initialised = true; - - ros::spin(); - - disconnect(phid); + ros::init(argc, argv, "phidgets_high_speed_encoder"); + ros::NodeHandle n; + ros::NodeHandle nh("~"); + int serial_number = -1; + nh.getParam("serial", serial_number); + std::string name = "encoder"; + nh.getParam("name", name); + if (serial_number == -1) + { + nh.getParam("serial_number", serial_number); + } + ROS_INFO("frame_id = %s", frame_id.c_str()); + if (inverted) + { + ROS_INFO("values are inverted"); + } + + std::string topic_path = "phidgets/"; + nh.getParam("topic_path", topic_path); + nh.getParam("frame_id", frame_id); + nh.getParam("inverted", inverted); + + if (attach(phid, serial_number)) + { + display_properties(phid); + + const int buffer_length = 100; + std::string topic_name = topic_path + name; + if (serial_number > -1) + { + char ser[10]; + sprintf(ser, "%d", serial_number); + topic_name += "/"; + topic_name += ser; } - return 0; + encoder_pub = + n.advertise(topic_name, + buffer_length); + + initialised = true; + + ros::spin(); + + disconnect(phid); + } + return 0; } From 898198cc6ee7c79fee7ac28ca6e18443594d1484 Mon Sep 17 00:00:00 2001 From: Geoff Viola Date: Wed, 8 Jun 2016 12:08:43 -0600 Subject: [PATCH 8/9] using ROS installed phidgets API --- phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp b/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp index 6e316ba..3b4ce3a 100644 --- a/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp +++ b/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp @@ -34,7 +34,7 @@ #include #include #include -#include +#include #include #include "phidgets_high_speed_encoder/encoder_params.h" From 917b4ec3cb4cf1d0ad80d13fc749a3baa5746413 Mon Sep 17 00:00:00 2001 From: Geoff Viola Date: Wed, 23 Nov 2016 17:52:24 -0700 Subject: [PATCH 9/9] initial attempts at addressing code review --- phidgets_high_speed_encoder/CMakeLists.txt | 2 +- .../msg/{encoder_params.msg => EncoderParams.msg} | 2 -- .../src/phidgets_high_speed_encoder.cpp | 14 +++++--------- 3 files changed, 6 insertions(+), 12 deletions(-) rename phidgets_high_speed_encoder/msg/{encoder_params.msg => EncoderParams.msg} (55%) diff --git a/phidgets_high_speed_encoder/CMakeLists.txt b/phidgets_high_speed_encoder/CMakeLists.txt index 5c7efad..1be235a 100644 --- a/phidgets_high_speed_encoder/CMakeLists.txt +++ b/phidgets_high_speed_encoder/CMakeLists.txt @@ -5,7 +5,7 @@ find_package(catkin REQUIRED COMPONENTS phidgets_api roscpp std_msgs message_gen add_message_files( FILES - encoder_params.msg + EncoderParams.msg ) generate_messages( diff --git a/phidgets_high_speed_encoder/msg/encoder_params.msg b/phidgets_high_speed_encoder/msg/EncoderParams.msg similarity index 55% rename from phidgets_high_speed_encoder/msg/encoder_params.msg rename to phidgets_high_speed_encoder/msg/EncoderParams.msg index 1f7919c..0af5b83 100644 --- a/phidgets_high_speed_encoder/msg/encoder_params.msg +++ b/phidgets_high_speed_encoder/msg/EncoderParams.msg @@ -1,5 +1,3 @@ Header header int32 index int32 count -int32 count_change -int32 time diff --git a/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp b/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp index 3b4ce3a..a03fe90 100644 --- a/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp +++ b/phidgets_high_speed_encoder/src/phidgets_high_speed_encoder.cpp @@ -36,7 +36,7 @@ #include #include #include -#include "phidgets_high_speed_encoder/encoder_params.h" +#include "phidgets_high_speed_encoder/EncoderParams.h" static CPhidgetEncoderHandle phid; static ros::Publisher encoder_pub; @@ -85,20 +85,17 @@ int InputChangeHandler(CPhidgetEncoderHandle ENC, int PositionChangeHandler(CPhidgetEncoderHandle ENC, void *usrptr, int Index, - int Time, int RelativePosition) + int Time, int) { static uint32_t sequence_number = 0U; int Position; CPhidgetEncoder_getPosition(ENC, Index, &Position); - phidgets_high_speed_encoder::encoder_params e; + phidgets_high_speed_encoder::EncoderParams e; e.header.stamp = ros::Time::now(); e.header.frame_id = frame_id; - e.header.seq = sequence_number++; e.index = Index; e.count = (inverted ? -Position : Position); - e.count_change = (inverted ? -RelativePosition : RelativePosition); - e.time = Time; if (initialised) encoder_pub.publish(e); ROS_DEBUG("Encoder %d Count %d", Index, Position); return 0; @@ -211,7 +208,6 @@ int main(int argc, char *argv[]) { nh.getParam("serial_number", serial_number); } - ROS_INFO("frame_id = %s", frame_id.c_str()); if (inverted) { ROS_INFO("values are inverted"); @@ -220,6 +216,7 @@ int main(int argc, char *argv[]) std::string topic_path = "phidgets/"; nh.getParam("topic_path", topic_path); nh.getParam("frame_id", frame_id); + ROS_INFO("frame_id = %s", frame_id.c_str()); nh.getParam("inverted", inverted); if (attach(phid, serial_number)) @@ -236,7 +233,7 @@ int main(int argc, char *argv[]) topic_name += ser; } encoder_pub = - n.advertise(topic_name, + n.advertise(topic_name, buffer_length); initialised = true; @@ -247,4 +244,3 @@ int main(int argc, char *argv[]) } return 0; } -