Esempio n. 1
0
    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!")
Esempio n. 2
0
    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!")
Esempio n. 4
0
    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!")
Esempio n. 5
0
    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()
Esempio n. 9
0
    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()
Esempio n. 10
0
    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!")
Esempio n. 13
0
    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()
Esempio n. 14
0
    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
Esempio n. 18
0
    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!")
Esempio n. 19
0
    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()
Esempio n. 20
0
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
Esempio n. 21
0
    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)