def planning_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
if handle == planning_getready:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.GETREADY)
elif handle == planning_grasp:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.GRASP)
elif handle == planning_ungrasp:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.UNGRASP)
elif handle == planning_turning:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.TURN)
elif handle == planning_finish:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.FINISH)
elif handle == planning_preview:
self.valve_status.color = "SOLID"
self.call_execute(self.valve_status.PREVIEW)
else:
rospy.roswarn("Planning > Unknown Plan Clicked!")
def talker(self):
# Class Instances
pressure = FluidPressure()
pressure.variance = 0
temp = Temperature()
temp.variance = 0
depth = Float64()
while not rospy.is_shutdown():
if self.sensor.read():
# Pressure reading (Pascal)
pressure.header.stamp = rospy.get_rostime()
pressure.fluid_pressure = self.sensor.pressure(ms5837.UNITS_Pa)
# Temperature reading (Celsius)
temp.header.stamp = rospy.get_rostime()
temp.temperature = self.sensor.temperature(
ms5837.UNITS_Centigrade)
# Depth reading (Meters)
depth.data = self.sensor.depth(ATM_PRESSURE_PA, LOCAL_GRAVITY)
# Publishing
self.pub_pressure.publish(pressure)
self.pub_temp.publish(temp)
self.pub_depth.publish(depth)
else:
rospy.roswarn('Failed to read.')
rospy.Rate(10).sleep() # 10 hz
def planning_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
if handle == planning_getready:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.GETREADY)
elif handle == planning_grasp:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.GRASP)
elif handle == planning_ungrasp:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.UNGRASP)
elif handle == planning_turning:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.TURN)
elif handle == planning_finish:
self.valve_status.color = "SOLID"
self.call_planner(self.valve_status.FINISH)
elif handle == planning_preview:
self.valve_status.color = "SOLID"
self.call_execute(self.valve_status.PREVIEW)
else:
rospy.roswarn("Planning > Unknown Plan Clicked!")
def lock_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
if handle == lock_master:
if self.valve_status.lock_state == self.valve_status.UNLOCKED:
self.valve_status.lock_state = self.valve_status.LOCKED
self.populate_valve_menu()
elif self.valve_status.lock_state == self.valve_status.LOCKED:
self.valve_status.lock_state = self.valve_status.UNLOCKED
self.populate_valve_menu()
else:
rospy.roswarn("Direction > Unknown Direction Clicked!")
def execute_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
if handle == execute:
print "Executing"
self.valve_status.color = "SOLID"
self.call_execute(self.valve_status.EXECUTE)
elif handle == execute_stored:
print "Executing Stored"
self.valve_status.color = "SOLID"
self.call_execute(self.valve_status.EXECUTE_STORED)
else:
rospy.roswarn("Unknown Execution Clicked!")
def execute_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
if handle == execute:
print "Executing"
self.valve_status.color = "SOLID"
self.call_execute(self.valve_status.EXECUTE)
elif handle == execute_stored:
print "Executing Stored"
self.valve_status.color = "SOLID"
self.call_execute(self.valve_status.EXECUTE_STORED)
else:
rospy.roswarn("Unknown Execution Clicked!")
def lock_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
if handle == lock_master:
if self.valve_status.lock_state == self.valve_status.UNLOCKED:
self.valve_status.lock_state = self.valve_status.LOCKED
self.populate_valve_menu()
elif self.valve_status.lock_state == self.valve_status.LOCKED:
self.valve_status.lock_state = self.valve_status.UNLOCKED
self.populate_valve_menu()
else:
rospy.roswarn("Direction > Unknown Direction Clicked!")
def direction_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == direction_cw:
self.valve_status.turning_direction = self.valve_status.CW
elif handle == direction_ccw:
self.valve_status.turning_direction = self.valve_status.CCW
else:
rospy.roswarn("Direction > Unknown Direction Clicked!")
self.populate_valve_menu()
def direction_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == direction_cw:
self.valve_status.turning_direction = self.valve_status.CW
elif handle == direction_ccw:
self.valve_status.turning_direction = self.valve_status.CCW
else:
rospy.roswarn("Direction > Unknown Direction Clicked!")
self.populate_valve_menu()
def radius_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == radius_increase:
self.valve_status.radius += 0.01
elif handle == radius_decrease:
self.valve_status.radius += (-0.01)
elif handle == radius_default:
self.valve_status.radius = self.valve_status.default_radius
else:
rospy.roswarn("Radius > Unknown Radius Clicked!")
def radius_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == radius_increase:
self.valve_status.radius += 0.01
elif handle == radius_decrease:
self.valve_status.radius += (-0.01)
elif handle == radius_default:
self.valve_status.radius = self.valve_status.default_radius
else:
rospy.roswarn("Radius > Unknown Radius Clicked!")
def pose_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == pose_reset_default:
self.valve_status.pose_stamped = deepcopy(self.valve_status.default_pose_stamped)
elif handle == pose_reset_session_defaul:
self.valve_status.pose_stamped = deepcopy(self.valve_status.session_pose_stamped)
elif handle == pose_set_default:
self.valve_status.session_pose_stamped = deepcopy(self.valve_status.pose_stamped)
else:
rospy.roswarn("Pose > Unknown Pose Clicked!")
def type_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == type_round:
self.valve_status.valve_type = self.valve_status.ROUND
elif handle == type_left:
self.valve_status.valve_type = self.valve_status.LEFTLEVER
elif handle == type_right:
self.valve_status.valve_type = self.valve_status.RIGHTLEVER
else:
rospy.roswarn("Type > Unknown Type Clicked!")
self.populate_valve_menu()
def hand_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == hand_left:
self.valve_status.hands = self.valve_status.LEFT
elif handle == hand_right:
self.valve_status.hands = self.valve_status.RIGHT
elif handle == hand_both:
self.valve_status.hands = self.valve_status.BOTH
else:
rospy.roswarn("Hand > Unknown Hand Clicked!")
self.populate_valve_menu()
def hand_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == hand_left:
self.valve_status.hands = self.valve_status.LEFT
elif handle == hand_right:
self.valve_status.hands = self.valve_status.RIGHT
elif handle == hand_both:
self.valve_status.hands = self.valve_status.BOTH
else:
rospy.roswarn("Hand > Unknown Hand Clicked!")
self.populate_valve_menu()
def type_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState( handle )
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == type_round:
self.valve_status.valve_type = self.valve_status.ROUND
elif handle == type_left:
self.valve_status.valve_type = self.valve_status.LEFTLEVER
elif handle == type_right:
self.valve_status.valve_type = self.valve_status.RIGHTLEVER
else:
rospy.roswarn("Type > Unknown Type Clicked!")
self.populate_valve_menu()
def getobjectidfromtype(type_name):
lc_type_name = string.lower(type_name)
if type_name == 'bool':
dv = ua.ObjectIds.Boolean
elif type_name == 'byte':
dv = ua.ObjectIds.Byte
elif type_name == 'int':
dv = ua.ObjectIds.Int16
elif type_name == 'int8':
dv = ua.ObjectIds.SByte
elif type_name == 'uint8':
dv = ua.ObjectIds.Byte
elif type_name == 'int16':
dv = ua.ObjectIds.Int16
rospy.roswarn("Int16??")
elif type_name == 'uint16':
dv = ua.ObjectIds.UInt16
elif type_name == 'int32':
dv = ua.ObjectIds.Int32
elif type_name == 'uint32':
dv = ua.ObjectIds.UInt32
elif type_name == 'int64':
dv = ua.ObjectIds.Int64
elif type_name == 'uint64':
dv = ua.ObjectIds.UInt64
elif type_name == 'float' or type_name == 'float32' or type_name == 'float64':
dv = ua.ObjectIds.Float
elif type_name == 'double':
dv = ua.ObjectIds.Double
elif type_name == 'string' or type_name == 'str':
dv = ua.ObjectIds.String
elif type_name == 'array':
dv = ua.ObjectIds.Enumeration
elif lc_type_name == 'time':
dv = ua.ObjectIds.Time
elif lc_type_name == 'duration':
dv = ua.ObjectIds.UInt32 # We probably need a custom type for this, the built-in duration is msecs, ROS uses nsecs
else:
rospy.logerr("Can't create type with name " + type_name)
return None
return dv
def pose_cb(self, feedback):
handle = feedback.menu_entry_id
state = self.menu_handler.getCheckState(handle)
self.valve_status.color = "CLEAR"
self.flushTrajMenu()
if handle == pose_reset_default:
self.valve_status.pose_stamped = deepcopy(
self.valve_status.default_pose_stamped)
elif handle == pose_reset_session_defaul:
self.valve_status.pose_stamped = deepcopy(
self.valve_status.session_pose_stamped)
elif handle == pose_set_default:
self.valve_status.session_pose_stamped = deepcopy(
self.valve_status.pose_stamped)
else:
rospy.roswarn("Pose > Unknown Pose Clicked!")
def talker(self):
pressure_msg = FluidPressure()
pressure_msg.variance = 0
temp_msg = Temperature()
temp_msg.variance = 0
while not rospy.is_shutdown():
if self.ms5837.read():
pressure_msg.header.stamp = rospy.get_rostime()
pressure_msg.fluid_pressure = self.ms5837.pressure(
ms5837.UNITS_Pa)
temp_msg.header.stamp = rospy.get_rostime()
temp_msg.temperature = self.ms5837.temperature()
self.pub_pressure.publish(pressure_msg)
self.pub_temp.publish(temp_msg)
else:
rospy.roswarn('Failed to read pressure sensor!')
rospy.Rate(10).sleep()
def getobjectidfromtype(type_name):
if type_name == 'bool':
dv = ua.ObjectIds.Boolean
elif type_name == 'byte':
dv = ua.ObjectIds.Byte
elif type_name == 'int':
dv = ua.ObjectIds.Int16
elif type_name == 'int8':
dv = ua.ObjectIds.SByte
elif type_name == 'uint8':
dv = ua.ObjectIds.Byte
elif type_name == 'int16':
dv = ua.ObjectIds.Int16
rospy.roswarn("Int16??")
elif type_name == 'uint16':
dv = ua.ObjectIds.UInt16
elif type_name == 'int32':
dv = ua.ObjectIds.Int32
elif type_name == 'uint32':
dv = ua.ObjectIds.UInt32
elif type_name == 'int64':
dv = ua.ObjectIds.Int64
elif type_name == 'uint64':
dv = ua.ObjectIds.UInt64
elif type_name == 'float' or type_name == 'float32' or type_name == 'float64':
dv = ua.ObjectIds.Float
elif type_name == 'double':
dv = ua.ObjectIds.Double
elif type_name == 'string' or type_name == 'str':
dv = ua.ObjectIds.String
elif type_name == 'array':
dv = ua.ObjectIds.Enumeration
elif type_name == 'Time' or type_name == 'time':
dv = ua.ObjectIds.Time
else:
rospy.logerr("Can't create type with name " + type_name)
return None
return dv
instant_temp.header.stamp = rospy.get_rostime()
instant_temp.temperature = sensor_data.temperature()
instant_temperature.publish(instant_temp)
if pressure_data.read():
pressure_message.header.stamp = rospy.get_rostime()
pressure_message.fluid_pressure = pressure_data.pressure(
ms5837.UNITS_atm)
temp_message.header.stamp = rospy.get_rostime()
temp_message.temperature = pressure_data.temperature()
pub_pressure.publish(pressure_message)
pub_temp.publish(temp_message)
else:
print("Sensor read failed!")
rospy.roswarn("Failed to read pressure sensor !")
exit(1)
data = sensor3.get_distance_simple()
if data:
print("Distance: %s\tConfidence: %s%%" %
(data["distance"], data["confidence"]))
sensor = data.get("distance")
pub1.publish(sensor)
else:
print("Failed to get distance data")
rospy.Rate(10).sleep()
#pub2.publish(pressure_data_send)
#pub3.publish(data1)
