diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 581e61d..3703df4 120000 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1 +1 @@ -/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file +/opt/ros/indigo/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/shear_sensor/CMakeLists.txt b/src/shear_sensor/CMakeLists.txt new file mode 100644 index 0000000..da3be23 --- /dev/null +++ b/src/shear_sensor/CMakeLists.txt @@ -0,0 +1,198 @@ +cmake_minimum_required(VERSION 2.8.3) +project(shear_sensor) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + message_generation +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a 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 + # cap.msg +#) + +## Generate services in the 'srv' folder +add_service_files( + FILES + SensorOffset.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 +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES shear_sensor +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/shear_sensor.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/shear_sensor_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_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_shear_sensor.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/src/shear_sensor/README.md b/src/shear_sensor/README.md deleted file mode 100644 index bf866aa..0000000 --- a/src/shear_sensor/README.md +++ /dev/null @@ -1 +0,0 @@ -The ROS package for the sensor messages and services diff --git a/src/shear_sensor/msg/cap.msg b/src/shear_sensor/msg/cap.msg new file mode 100644 index 0000000..1ac04d3 --- /dev/null +++ b/src/shear_sensor/msg/cap.msg @@ -0,0 +1,2 @@ +float64 capacitance +float64 mean diff --git a/src/shear_sensor/package.xml b/src/shear_sensor/package.xml new file mode 100644 index 0000000..6131e6f --- /dev/null +++ b/src/shear_sensor/package.xml @@ -0,0 +1,68 @@ + + + shear_sensor + 0.0.0 + The shear_sensor package + + + + + rrlab + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + message_generation + + + + + + message_runtime + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/src/shear_sensor/src/filter_cap.py b/src/shear_sensor/src/filter_cap.py new file mode 100755 index 0000000..ab8db72 --- /dev/null +++ b/src/shear_sensor/src/filter_cap.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +import rospy +import matplotlib.pyplot as plt +from std_msgs.msg import String +from std_msgs.msg import Float64 +import builtins +import collections +from scipy.signal import savgol_filter +import numpy as np + +# create the list of capacitance +cap_list=collections.deque(maxlen=20) +cap_list_mean=collections.deque(maxlen=200) + +#plt.axis([0, 10, 0, 1]) +#plt.ion() + +# float capacitance +capacitance = 100.00 +mean_cap = 100.00 + +# set up publisher +pub_cap = rospy.Publisher('capacitance_val', Float64) +pub_mean = rospy.Publisher('mean_cap', Float64) + +#count = 1 +def callback(data): + # global count + # Create the list to filter and calculate mean + cap_list.append(data.data/100000) + if len(cap_list) < 20 : + capacitance = cap_list[-1] + else : + # Savitzky Golay filter for continuous stream of data + y = savgol_filter(cap_list, 15, 2) + # p= savgol_filter(cap_list, 19, 2) + cap_list_mean.append(y[-1]) + capacitance = y[-1] + pub_cap.publish(Float64(capacitance)) + if len(cap_list_mean) == 200 : + mean_cap = np.mean(cap_list_mean) + pub_mean.publish(Float64(mean_cap)) + #count = count + 1 + #plt.plot(count,capacitance,'r--',count, mean_cap,'bs') + #plt.pause(0.05) + rospy.loginfo("capacitance value %s", capacitance) + +def listener(): + + # In ROS, nodes are uniquely named. If two nodes with the same + # node are launched, the previous one is kicked off. The + # anonymous=True flag means that rospy will choose a unique + # name for our 'listener' node so that multiple listeners can + # run simultaneously. + rospy.init_node('filter_cap', anonymous=True) + rospy.Subscriber("chatter", Float64, callback) + # spin() simply keeps python from exiting until this node is stopped + rospy.spin() + +if __name__ == '__main__': + listener() diff --git a/src/shear_sensor/src/shear_sensor_test.py b/src/shear_sensor/src/shear_sensor_test.py new file mode 100755 index 0000000..84418fe --- /dev/null +++ b/src/shear_sensor/src/shear_sensor_test.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python + +# Example code for a script using the ReFlex Takktile hand +# Note: you must connect a hand by running "roslaunch reflex reflex.launch" before you can run this script + + +from math import pi, cos + +import rospy +import builtins +from std_srvs.srv import Empty +from std_msgs.msg import Float64 + +from reflex_msgs.msg import Command +from reflex_msgs.msg import PoseCommand +from reflex_msgs.msg import VelocityCommand +from reflex_msgs.msg import ForceCommand +from reflex_msgs.msg import Hand +from reflex_msgs.msg import FingerPressure +from reflex_msgs.srv import SetTactileThreshold, SetTactileThresholdRequest + + +hand_state = Hand() +cap =1046 +list_cap=collections.deque(maxlen=100) + +def main(): + ################################################################################################################## + rospy.init_node('ExampleHandNode') + + # sensor init +# initial() + # Services can automatically call hand calibration + calibrate_fingers = rospy.ServiceProxy('/reflex_takktile/calibrate_fingers', Empty) + calibrate_tactile = rospy.ServiceProxy('/reflex_takktile/calibrate_tactile', Empty) + + # Services can set tactile thresholds and enable tactile stops + enable_tactile_stops = rospy.ServiceProxy('/reflex_takktile/enable_tactile_stops', Empty) + disable_tactile_stops = rospy.ServiceProxy('/reflex_takktile/disable_tactile_stops', Empty) + set_tactile_threshold = rospy.ServiceProxy('/reflex_takktile/set_tactile_threshold', SetTactileThreshold) + + # This collection of publishers can be used to command the hand + command_pub = rospy.Publisher('/reflex_takktile/command', Command, queue_size=1) + pos_pub = rospy.Publisher('/reflex_takktile/command_position', PoseCommand, queue_size=1) + vel_pub = rospy.Publisher('/reflex_takktile/command_velocity', VelocityCommand, queue_size=1) + force_pub = rospy.Publisher('/reflex_takktile/command_motor_force', ForceCommand, queue_size=1) + + # Constantly capture the current hand state + rospy.Subscriber('/reflex_takktile/hand_state', Hand, hand_state_cb) + + ################################################################################################################## +# # Calibrate the fingers (make sure hand is opened in zero position) +# raw_input("== When ready to calibrate the hand, press [Enter]\n") +# calibrate_fingers() +# raw_input("... [Enter]\n") +# + ################################################################################################################## + # Demonstration of position control + rospy.sleep(2.0) + pos_pub.publish(PoseCommand(f1=1.0, f2=1.0, f3=3.0, preshape=0.0)) + rospy.sleep(5.0) + + while not rospy.is_shutdown(): +#check() +#def check(): + # posi_pub = rospy.Publisher('/reflex_takktile/command_position', PoseCommand, queue_size=1) + # + # #Position control part + # rospy.Subscriber('chatter', Float64, callback) + # rospy.loginfo(rospy.get_caller_id() + "in the loop %s", cap) + # if cap > 10000.0: + # pos_pub.publish(PoseCommand(f1=1.0, f2=1.0, f3=2.0, preshape=1.5)) + # rospy.sleep(1.0) + # else: + # pos_pub.publish(PoseCommand(f1=1.0, f2=1.0, f3=3.0, preshape=0.0)) + # rospy.sleep(1.0) + + #force control + rospy.Subscriber('chatter', Float64, callback) + rospy.loginfo(rospy.get_caller_id() + "in the loop %s", cap) + if cap - 100000 > 100000 and cap - 100000 < 200000: + force_pub.publish(ForceCommand(f3=50.0)) + rospy.sleep(5.0) + force_pub.publish(ForceCommand(f3=0.0)) + rospy.sleep(5.0) + elif cap - 100000 > 400000 and cap - 10000 < 1000000: + force_pub.publish(ForceCommand(f3=75.0)) + rospy.sleep(5.0) + force_pub.publish(ForceCommand(f3=0.0)) + rospy.sleep(5.0) + elif cap - 100000 > 1000000: + force_pub.publish(ForceCommand(f3=100.0)) + rospy.sleep(5.0) + force_pub.publish(ForceCommand(f3=0.0)) + rospy.sleep(5.0) + elif cap - 100000 <= 0: + pos_pub.publish(PoseCommand(f1=1.0, f2=1.0, f3=2.0, preshape=0.0)) + rospy.sleep(3.0) + pos_pub.publish(PoseCommand(f1=1.0, f2=1.0, f3=3.0, preshape=0.0)) + rospy.sleep(3.0) + + + +# raw_input("== When ready to wiggle fingers with position control, hit [Enter]\n") +# for i in range(100): +# setpoint = (-cos(i / 15.0) + 1) * 1.75 +# pos_pub.publish(PoseCommand(f1=setpoint, f2=setpoint, f3=setpoint, preshape=0.0)) +# rospy.sleep(0.1) +# raw_input("... [Enter]\n") +# pos_pub.publish(PoseCommand()) + + # rospy.spin() + ################################################################################################################## +# # Demonstration of preshape joint +# raw_input("== When ready to test preshape joint, hit [Enter]\n") +# pos_pub.publish(PoseCommand(preshape=1.57)) +# rospy.sleep(2.0) +# pos_pub.publish(PoseCommand()) +# rospy.sleep(2.0) +# raw_input("... [Enter]\n") +# +# ################################################################################################################## +# # Demonstration of velocity control - variable closing speed +# raw_input("== When ready to open and close fingers with velocity control, hit [Enter]\n") +# vel_pub.publish(VelocityCommand(f1=3.0, f2=2.0, f3=1.0, preshape=0.0)) +# rospy.sleep(4.0) +# vel_pub.publish(VelocityCommand(f1=-1.0, f2=-2.0, f3=-3.0, preshape=0.0)) +# rospy.sleep(4.0) +# raw_input("... [Enter]\n") +# pos_pub.publish(PoseCommand()) +# +# ################################################################################################################## +# # Demonstration of blended control - asymptotic approach to goal - uses hand_state +# raw_input("== When ready to approach target positions with blended control, hit [Enter]\n") +# pose = PoseCommand(f1=3.5, f2=2.25, f3=1.0, preshape=0.0) +# velocity = VelocityCommand() +# for i in range(1, 5): +# velocity.f1 = round(pose.f1 - hand_state.motor[0].joint_angle, 1) + 0.25 +# velocity.f2 = round(pose.f2 - hand_state.motor[1].joint_angle, 1) + 0.25 +# velocity.f3 = round(pose.f3 - hand_state.motor[2].joint_angle, 1) + 0.25 +# command_pub.publish(Command(pose, velocity)) +# rospy.sleep(0.4) +# raw_input("... [Enter]\n") +# pos_pub.publish(PoseCommand()) +# + ################################################################################################################## + # # # Demonstration of force control - square wave + # raw_input("== When ready to feel variable force control, hit [Enter]\n") + # raw_input("== Putting your arm or hand in the hand will allow you to feel the effect [Enter]\n") + # pos_pub.publish(PoseCommand(f1=1.5, f2=1.5, f3=1.5, preshape=0.0)) + # rospy.sleep(2.0) + # print("Tightening finger 1") + # force_pub.publish(ForceCommand(f1=300.0)) + # rospy.sleep(5.0) + # print("Partially loosen finger 1") + # force_pub.publish(ForceCommand(f1=100.0)) + # rospy.sleep(1.0) + # print("Tightening finger 2") + # force_pub.publish(ForceCommand(f2=300.0)) + # rospy.sleep(5.0) + ## print("Partially loosen finger 2") + # force_pub.publish(ForceCommand(f2=100.0)) + # rospy.sleep(1.0) + # print("Tightening finger 3") + # force_pub.publish(ForceCommand(f3=300.0)) + # rospy.sleep(5.0) + # print("Partially loosen finger 3") + # force_pub.publish(ForceCommand(f3=100.0)) + # rospy.sleep(1.0) + # vel_pub.publish(VelocityCommand(f1=-5.0, f2=-5.0, f3=-5.0, preshape=0.0)) + # rospy.sleep(2.0) +# raw_input("... [Enter]\n") + + ################################################################################################################## + + # Demonstration of tactile feedback and setting sensor thresholds + # raw_input("== When ready to calibrate tactile sensors and close until contact, hit [Enter]\n") +# calibrate_tactile() + # enable_tactile_stops() + # f1 = FingerPressure([10, 10, 10, 10, 10, 10, 10, 10, 1000]) + # f2 = FingerPressure([15, 15, 15, 15, 15, 15, 15, 15, 1000]) + # f3 = FingerPressure([20, 20, 20, 20, 20, 20, 20, 20, 1000]) + # threshold = SetTactileThresholdRequest([f1, f2, f3]) + # set_tactile_threshold(threshold) + # vel_pub.publish(VelocityCommand(f1=1.0, f2=1.0, f3=1.0, preshape=0.0)) + # rospy.sleep(3.0) + # raw_input("... [Enter]\n") + # pos_pub.publish(PoseCommand()) + # disable_tactile_stops() + +#def initial(): +# rospy.Subscriber('chatter', Float64, calibratecap) +# +#def calibratecap(data): +# rospy.loginfo(rospy.get_caller_id() + "intialize", data.data) +# normalcap=data.data +# +def hand_state_cb(data): + global hand_state + hand_state = data + +def callback(data): + #rospy.loginfo(rospy.get_caller_id() + "capcitance read %s", data.data) + global cap + cap = data.data + +if __name__ == '__main__': + main() diff --git a/src/shear_sensor/srv/SensorOffset.srv b/src/shear_sensor/srv/SensorOffset.srv new file mode 100644 index 0000000..6ec6a21 --- /dev/null +++ b/src/shear_sensor/srv/SensorOffset.srv @@ -0,0 +1,3 @@ +float64 cap +--- +float64 offset