Пример #1
0
    def run(self):
        standing_wheel = 'right' if self.motor == 'left' else 'left'
        rospy.loginfo('Place the robot with the %s wheel at the center of the T '
                      'and press the central button when the robot is ready',
                      standing_wheel)
        self.ping()
        rospy.wait_for_message('buttons/center', std_msgs.msg.Bool)
        rospy.sleep(3)
        msg = rospy.wait_for_message('aseba/events/ground', AsebaEvent)
        self.ths = [d - self.gap for d in msg.data]
        # count(3)
        rospy.loginfo('Start calibrating %s motor using thresholds %s', self.motor, self.ths)
        _n = 1
        self.pick = None
        for motor_speed in self.motor_speeds:
            rospy.loginfo('Set motor speed to %s', motor_speed)
            _n += self.target_number_of_samples
            self.motor_speed = motor_speed
            if self.motor == 'right':
                data = [0, self.motor_speed]
            else:
                data = [self.motor_speed, 0]

            self.pub.publish(AsebaEvent(data=data))
            while len(self._samples) < _n:
                rospy.sleep(0.05)
        self.pub.publish(AsebaEvent(data=[0, 0]))
        self.sub.unregister()
        rospy.logdebug('Calculate with samples %s', self._samples)
        self.save_samples()
        samples = np.array(self._samples)
        results = calibrate(samples)
        rospy.loginfo('Calibration of motor %s done: %s', self.motor, results)
        self.save_calibration(results)
Пример #2
0
    def __init__(self, node_name):
        ROS2OpenCV2.__init__(self, node_name)
        
        self.match_threshold = rospy.get_param("~match_threshold", 0.7)
        self.find_multiple_targets = rospy.get_param("~find_multiple_targets", False)
        self.n_pyr = rospy.get_param("~n_pyr", 2)
        self.min_template_size = rospy.get_param("~min_template_size", 25)

        self.scale_factor = rospy.get_param("~scale_factor", 1.2)
        self.scale_and_rotate = rospy.get_param("~scale_and_rotate", False)
        
        self.use_depth_for_detection = rospy.get_param("~use_depth_for_detection", False)
        self.fov_width = rospy.get_param("~fov_width", 1.094)
        self.fov_height = rospy.get_param("~fov_height", 1.094)
        self.max_object_size = rospy.get_param("~max_object_size", 0.28)

        # Intialize the detection box
        self.detect_box = None
        
        self.detector_loaded = False
        
        rospy.loginfo("Waiting for video topics to become available...")

        # Wait until the image topics are ready before starting
        rospy.wait_for_message("input_rgb_image", Image)
        
        if self.use_depth_for_detection:
            rospy.wait_for_message("input_depth_image", Image)
            
        rospy.loginfo("Ready.")
    def __init__(self):
        '''
        Initializes storage, gets parameters from parameter server and logs to rosinfo
        '''
        self.bridge = CvBridge()

        # set up Checkerboard and CheckerboardDetector
        self.board = Checkerboard((9, 6), 0.03)
        self.detector = CheckerboardDetector(self.board)
        rospy.init_node(NODE)
        self.counter = 0

        # Get params from ros parameter server or use default
        self.cams = rospy.get_param("~cameras")
        self.camera = [self.cams["reference"]["topic"]]
        for cam in self.cams["further"]:
            self.camera.append(cam["topic"])
        self.numCams = len(self.camera)

        # Init images
        self.image = []
        self.image_received = [False] * self.numCams
        for id in range(self.numCams):
            self.image.append(Image())

        # Subscribe to images
        self.imageSub = []
        for id in range(self.numCams):
            self.imageSub.append(rospy.Subscriber(
                self.camera[id], Image, self._imageCallback, id))

        # Wait for image messages
        for id in range(self.numCams):
            rospy.wait_for_message(self.camera[id], Image, 5)
Пример #4
0
    def __init__(self,name):
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name,
                                                irols.msg.DoSeekAction,
                                                auto_start=False)
        self._as.register_goal_callback(self.goal_cb)
        self._as.register_preempt_callback(self.preempt_cb)

        rospy.wait_for_message('p3at/odom',Odometry)
        self.alt_sp = 10
        self.x_offset = 0
        self.y_offset = 0
        self.pos_sp = PoseStamped()
        self.odom_pub = rospy.Subscriber('p3at/odom',
                                         Odometry,
                                         self.handle_odom)
        self.curr_pos = rospy.wait_for_message('mavros/local_position/pose',
                                               PoseStamped).pose
        self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose',
                                              PoseStamped,
                                              self.handle_local_pose)
        self.state = rospy.wait_for_message('mavros/state',State)
        self.state_sub = rospy.Subscriber('mavros/state',
                                          State,
                                          self.handle_state)
        self.pos_sp_pub = rospy.Publisher('mavros/setpoint_position/local',
                                             PoseStamped,
                                             queue_size=3)
        
        self.set_mode_client = rospy.ServiceProxy('mavros/set_mode',
                                                  SetMode)

        self._as.start()
        rospy.loginfo('{0}: online'.format(self._action_name))
Пример #5
0
    def __init__(self, name, log_level=rospy.INFO, start=True):
        rospy.init_node(name, log_level=log_level)

        self.clear_octomap_service = rospy.ServiceProxy('/octomap_server/clear_bbx', BoundingBoxQuery)
        self.reset_octomap_service = rospy.ServiceProxy('/octomap_server/reset', Empty)
        is_sim = rospy.get_param("~is_simulation", False)
        self.is_ready = True
        rospy.wait_for_service('/obstacle_generator')
        rospy.wait_for_service('/goal_manager/add_goal', 5)
        rospy.wait_for_service('/move_base/make_plan', 5)
        if not is_sim:
            rospy.wait_for_message('/odometry/filtered', Odometry, timeout=None)
        rospy.sleep(0.02)
        self.generate_obstacle_service = rospy.ServiceProxy('/obstacle_generator', GenerateObstacle)
        self.send_goal_service = rospy.ServiceProxy('/goal_manager/add_goal', AddGoal)
        rospy.Subscriber("/goal_manager/current", GoalWithPriority, self.goal_callback)
        rospy.Subscriber("/goal_manager/last_goal_reached", Bool, self.last_goal_callback)

        self.tf_listener = tf.TransformListener()


        self.goal_count = 0

        self.tf_listener.waitForTransform("/odom", "/base_footprint", rospy.Time(), rospy.Duration(6.0))
        self.is_ready = True
        self.save_start_pos()
        rospy.loginfo("StateAi {} started.".format(name))

        if start:
            self.on_start()
            rospy.spin()
    def __init__(self, node_name):
        ROS2OpenCV2.__init__(self, node_name)

        self.node_name = node_name

        self.use_depth_for_detection = rospy.get_param("~use_depth_for_detection", False)
        self.fov_width = rospy.get_param("~fov_width", 1.094)
        self.fov_height = rospy.get_param("~fov_height", 1.094)
        self.max_object_size = rospy.get_param("~max_object_size", 0.28)

        # Intialize the detection box
        self.detect_box = None

        # Initialize a couple of intermediate image variables
        self.grey = None
        self.small_image = None

        # What kind of detector do we want to load
        self.detector_type = "template"
        self.detector_loaded = False

        rospy.loginfo("Waiting for video topics to become available...")

        # Wait until the image topics are ready before starting
        rospy.wait_for_message("input_rgb_image", Image)

        if self.use_depth_for_detection:
            rospy.wait_for_message("input_depth_image", Image)

        rospy.loginfo("Ready.")
 def get_image_topic_name(self, veh):
   image_topic_name = veh + "/camera_node/image_rect"
   try:
     rospy.wait_for_message(image_topic_name, Image, timeout=3)
     return image_topic_name
   except rospy.ROSException, e:
     print "%s" % e
Пример #8
0
    def __init__(self):
        rospy.init_node("position_test")
        rospy.on_shutdown(self.shutdown)

        rate = rospy.get_param("~rate", 100)
        r = rospy.Rate(rate)

        self.head_pan_joint = rospy.get_param("~head_pan_joint", "head_pan_joint")

        self.joints = [self.head_pan_joint]

        self.default_joint_speed = rospy.get_param("~default_joint_speed", 0.2)

        self.joint_state = JointState()
        rospy.Subscriber("joint_states", JointState, self.update_joint_state)

        while not rospy.is_shutdown():
            rospy.sleep(1)

            rospy.loginfo("waiting for joint_states")
            rospy.wait_for_message("joint_states", JointState)

            current_pan = self.joint_state.position[self.joint_state.name.index(self.head_pan_joint)]

            print "A"
            print current_pan
            print "B"
            print current_pan * 180 / math.pi
Пример #9
0
    def takeObservation(self):
        # TODO: Using wait_for_message for now, might change to ApproxTimeSync later 

        poseData = None
        while poseData is None:
          try:
              # poseData = rospy.wait_for_message('/ground_truth_to_tf/pose', PoseStamped, timeout=5)
              poseData = rospy.wait_for_message('/ground_truth/state', Odometry, timeout=5)
          except:
              rospy.loginfo("Current drone pose not ready yet, retrying to get robot pose")

        velData = None
        while velData is None:
          try:
              velData = rospy.wait_for_message('/fix_velocity', Vector3Stamped, timeout=5)
          except:
              rospy.loginfo("Current drone velocity not ready yet, retrying to get robot velocity")

        imuData = None
        while imuData is None:
          try:
              imuData = rospy.wait_for_message('/raw_imu', Imu, timeout=5)
          except:
              rospy.loginfo("Current drone imu not ready yet, retrying to get robot imu")

        motorData = None
        while motorData is None:
          try:
              motorData = rospy.wait_for_message('/motor_status', MotorStatus, timeout=5)
          except:
              rospy.loginfo("Current drone motor status not ready yet, retrying to get robot motor status")
        
        return poseData, imuData, velData, motorData
Пример #10
0
    def __init__(self, image_topic='/spiky/retina_image'):
        super(BaseNetworkIn, self).__init__()

        self._postsynaptic_learning_neurons = []

        # Populations
        self.pop_input_image_r = None
        self.pop_input_image_l = None
        self.pop_encoded_image_l = None
        self.pop_encoded_image_r = None

        self.spikedetector_enc_image_l = None
        self.spikedetector_enc_image_r = None

        # Preparing sensory information subscriber
        self._bridge = CvBridge()
        self._last_frame = None
        self._retina_image_subscriber = rospy.Subscriber(image_topic, Image, self._handle_frame, queue_size=1)
        rospy.wait_for_message(image_topic, Image)

        if self._last_frame is not None:
            shape = np.shape(self._last_frame)
            self._width = shape[0]
            self._height = shape[1]

        self._build_input_layer()
        self._create_spike_detectors()
Пример #11
0
    def test_video_camera(self):
        rospy.init_node('morse_ros_video_testing', log_level=rospy.DEBUG)

        motion_topic = '/robot/motion'
        camera_topic = '/robot/camera/image'
        camnfo_topic = '/robot/camera/camera_info'

        pub_vw(motion_topic, 1, 1)

        old = []
        for step in range(2):
            msg = rospy.wait_for_message(camnfo_topic, CameraInfo, 10)
            camera_info_frame = msg.header.frame_id
            # might want to add more CameraInfo test here

            msg = rospy.wait_for_message(camera_topic, Image, 10)

            self.assertEqual(msg.header.frame_id, camera_info_frame)

            self.assertEqual(len(msg.data), 128*128*4) # RGBA
            # dont use assertNotEqual here
            #   dumping raw image data in log is not relevant
            self.assertTrue(msg.data != old)
            old = msg.data

            time.sleep(0.2) # wait for turning
   def __init__(self):

      # initialize ROS node
      rospy.init_node('target_detector', anonymous=True)

      # initialize publisher for target pose, PoseStamped message, and set initial sequence number
      self.pub = rospy.Publisher('target_pose', PoseStamped, queue_size=1)
      self.pub_pose = PoseStamped()
      self.pub_pose.header.seq = 0

      self.rate = rospy.Rate(1.0)                      # publish message at 1 Hz

      # initialize values for locating target on Kinect v2 image
      self.target_u = 0                        # u is pixels left(0) to right(+)
      self.target_v = 0                        # v is pixels top(0) to bottom(+)
      self.target_d = 0                        # d is distance camera(0) to target(+) from depth image
      self.target_found = False                # flag initialized to False
      self.last_d = 0                          # last non-zero depth measurement

      # target orientation to Kinect v2 image (Euler)
      self.r = 0
      self.p = 0
      self.y = 0

      # Convert image from a ROS image message to a CV image
      self.bridge = CvBridge()

      # Wait for the camera_info topic to become available
      rospy.wait_for_message('/kinect2/qhd/image_color_rect', Image)

      # Subscribe to registered color and depth images
      rospy.Subscriber('/kinect2/qhd/image_color_rect', Image, self.image_callback, queue_size=1)
      rospy.Subscriber('/kinect2/qhd/image_depth_rect', Image, self.depth_callback, queue_size=1)

      self.rate.sleep()                        # suspend until next cycle
Пример #13
0
    def run(self):
        # Waits until everything has been initialized
        rospy.wait_for_message("/position", PoseStamped, timeout=5)

        prev = [0, 0]

        r = rospy.Rate(10)
        if not rospy.is_shutdown():
            for i in self.contours:
                for j in i:
                    if self.first:
                        self.pub_marker.publish(String("0"))
                        print "pen up"
                    else:
                        self.pub_marker.publish(String("1"))
                        print "pen down"
                    pos = [j[0], -j[1]]
                    rot = self.calc_theta(prev[0], prev[1], pos[0], pos[1])

                    self.push_waypoint(pos, rot)
                    prev = pos
                    self.first = False
                print "contour"
                self.first = True  # Marker should be up when going to the first point of each new contour
            self.pub_marker.publish(String("0"))
            self.push_waypoint(0, 0)
            self.th_f = 0
            self.rotate_neato()

        self.pub_vel.publish(Twist())
        cv2.waitKey(0)
Пример #14
0
def move_interrupt():
    g = FollowJointTrajectoryGoal()
    g.trajectory = JointTrajectory()
    g.trajectory.joint_names = JOINT_NAMES
    try:
        joint_states = rospy.wait_for_message("joint_states", JointState)
        joints_pos = joint_states.position
        g.trajectory.points = [
            JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
            JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
            JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
            JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
    
        client.send_goal(g)
        time.sleep(3.0)
        print "Interrupting"
        joint_states = rospy.wait_for_message("joint_states", JointState)
        joints_pos = joint_states.position
        g.trajectory.points = [
            JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
            JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
            JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
            JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
        client.send_goal(g)
        client.wait_for_result()
    except KeyboardInterrupt:
        client.cancel_goal()
        raise
    except:
        raise
    def run(self):
        """Loop, waiting for the user to press enter to take laser+camera snapshots"""

        while not rospy.is_shutdown():
            print "Press enter to grab a camera-laser pair, or q to quit"
            input_str = raw_input()
            if "q" in input_str:
                break
            try:
                laser_msg = rospy.wait_for_message(self.scan_topic, LaserScan, timeout=0.25)
            except rospy.exceptions.ROSException as err:
                print "Failed to receive laser message: " + str(err)
                continue
            try:
                camera_msg = rospy.wait_for_message(self.camera_topic, Image, timeout=0.25)
            except rospy.exceptions.ROSException as err:
                print "Failed to receive camera message: " + str(err)
                continue

            #generate the laser scan string to write to file
            laser_str = self.process_laser_scan(laser_msg)

            #write the image to bmp file
            try:
                self.process_camera_image(camera_msg)
            except CvBridgeError:
                print "Failed to process camera image, tossing data"
                continue

            #only add the laser string to the file if the image wrote successfully
            self.image_number += 1
            self.laser_file.write(laser_str)
Пример #16
0
 def __init__(self):
     self.node_name = "cv_bridge_demo"
     
     rospy.init_node(self.node_name)
     
     # What we do during shutdown
     rospy.on_shutdown(self.cleanup)
     
     # Create the OpenCV display window for the RGB image
     self.cv_window_name = self.node_name
     cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
     cv.MoveWindow(self.cv_window_name, 25, 75)
     
     # And one for the depth image
     cv.NamedWindow("Depth Image", cv.CV_WINDOW_NORMAL)
     cv.MoveWindow("Depth Image", 25, 350)
     
     # Create the cv_bridge object
     self.bridge = CvBridge()
     
     # Subscribe to the camera image and depth topics and set
     # the appropriate callbacks
     self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
     self.depth_sub = rospy.Subscriber("input_depth_image", Image, self.depth_callback, queue_size=1)
     
     rospy.loginfo("Waiting for image topics...")
     rospy.wait_for_message("input_rgb_image", Image)
     rospy.loginfo("Ready.")
    def run(self):
        rospy.wait_for_message('/desired_joint_states', JointState)
        rospy.wait_for_message('/joint_states', JointState)

        while not rospy.is_shutdown():
            with self.lock:
                for controller in self.controller_list:
                    if controller.is_active():

                        num_joints_stopped = 0

                        for joint in controller.joints:
                            desired_position = self.get_desired_position(joint)
                            current_position = self.get_current_position(joint)
                            difference = abs(current_position - desired_position)

                            joint.set_position(desired_position)
                            #rospy.loginfo('current_position: {0}, desired_position: {1}, diff: {2}'.format(current_position, desired_position, difference))


                            if difference < 0.05:
                                num_joints_stopped += 1

                        #TODO: set finished if average change of each joint is little
                        #rospy.loginfo('num_joints_stopped: {0}, joints: {1}'.format(num_joints_stopped, len(controller.joints)))
                        if controller.iterations > 5 and controller.joints_stopped is False and num_joints_stopped == len(controller.joints):
                            controller.joints_stopped = True
                            controller.publish_joints_stopped()

                        controller.iterations += 1

            self.rate.sleep()
    def test_all(self):
        rospy.wait_for_message('/static_image_publisher/output', Image)
        rospy.sleep(2)

        save_dir = rospy.get_param('/save_dir_all')
        save_dir = osp.expanduser(save_dir)
        self.check(save_dir, target='image')
Пример #19
0
	def spin(self):
		li = rospy.wait_for_message("/my_stereo/left/image_rect_color", Image)
		ri = rospy.wait_for_message("/my_stereo/right/image_rect_color", Image)
		left = self.callback_img(li, "left")
		right = self.callback_img(ri, "right")
		sub = self.sub_img(left, right)
		return np.sum(sub)
Пример #20
0
def send_to_joint_vals(q, arm='right'):
    pub_joint_cmd=rospy.Publisher('/robot/limb/' + arm + '/joint_command',JointCommand)
    command_msg=JointCommand()
    command_msg.names = generate_joint_names_for_arm(arm)
    command_msg.command=q
    command_msg.mode=JointCommand.POSITION_MODE
    control_rate = rospy.Rate(100)
    start = rospy.get_time()

    joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
    qc = joint_positions.position[9:16]
    qc = ([qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]])

    print('q,qc')
    print(q)
    print(qc)
    while not rospy.is_shutdown() and numpy.linalg.norm(numpy.subtract(q,qc))> 0.07:
        pub_joint_cmd.publish(command_msg)    # sends the commanded joint values to Baxter
        control_rate.sleep()
        joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
        qc = (joint_positions.position[9:16])
        qc = (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
        print "joint error = ", numpy.linalg.norm(numpy.subtract(q,qc))
        print((q,qc))
    print("In home pose")
    return (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
    def __init__(self, goal) :     
        rospy.loginfo("Waiting for Topological map ...")

        try:
            msg = rospy.wait_for_message('/topological_map', TopologicalMap, timeout=10.0)
            self.top_map = msg
            self.lnodes = msg.nodes
        except rospy.ROSException :
            rospy.logwarn("Failed to get topological map")
            return

        rospy.loginfo("Waiting for Current Node ...")
              
        try:
            msg = rospy.wait_for_message('/closest_node', String, timeout=10.0)
            cnode = msg.data
        except rospy.ROSException :
            rospy.logwarn("Failed to get closest node")
            return

            
        rsearch = TopologicalRouteSearch(self.top_map)
        route = rsearch.search_route(cnode, goal)
        print route
        
        rospy.loginfo("All Done ...")
Пример #22
0
	def __init__(self):
		rospy.init_node('goface',anonymous=True)

		rospy.on_shutdown(self.shutdown)
		
		self.rest_time = rospy.get_param("~rest_time",10)
		self.fake_test = rospy.get_param("~fake_test",False) 
		goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
                               'SUCCEEDED', 'ABORTED', 'REJECTED',
                               'PREEMPTING', 'RECALLING', 'RECALLED','LOST']

		self.move_base = actionlib.SimpleActionClient("move_base",MoveBaseAction)

		rospy.loginfo("Waiting for move_base action server...")

		self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
		self.move_base.wait_for_server(rospy.Duration(60))

		rospy.loginfo("Connected to move base server")

		# A variable to hold the initial pose of the robot to be set by 
		# the user in RViz
		self.initial_pose = PoseWithCovarianceStamped()

		rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
		rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
		self.last_location = Pose()
		rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
		self.goalpub = rospy.Publisher('thegoal',Pose)
		while self.initial_pose.header.stamp == "":
		    rospy.sleep(1)
		self.goal = MoveBaseGoal()
		rospy.loginfo("Starting navigation test")
		rospy.Subscriber("/destination_face",Pose,self.faceCallback)
Пример #23
0
 def __init__(self):
     # Initialize constants
     self.error_threshold = 0.0175 # Report success if error reaches below threshold
     self.signal = 1
     
     # Initialize new node
     rospy.init_node(NAME, anonymous=True)
     
     controller_name = rospy.get_param('~controller')
     
     # Initialize publisher & subscriber for tilt
     self.laser_tilt = JointControllerState()
     self.laser_tilt_pub = rospy.Publisher(controller_name + '/command', Float64)
     self.laser_signal_pub = rospy.Publisher('laser_scanner_signal', LaserScannerSignal)
     self.joint_speed_srv = rospy.ServiceProxy(controller_name + '/set_speed', SetSpeed, persistent=True)
     
     rospy.Subscriber(controller_name + '/state', JointControllerState, self.process_tilt_state)
     rospy.wait_for_message(controller_name + '/state', JointControllerState)
     
     # Initialize tilt action server
     self.result = HokuyoLaserTiltResult()
     self.feedback = HokuyoLaserTiltFeedback()
     self.feedback.tilt_position = self.laser_tilt.process_value
     self.server = SimpleActionServer('hokuyo_laser_tilt_action', HokuyoLaserTiltAction, self.execute_callback)
     
     rospy.loginfo("%s: Ready to accept goals", NAME)
Пример #24
0
def cruise(pose_number):
 marker_pub = rospy.Publisher('curse_markers', Marker, queue_size=1)
 marker=marker_define()

 intial=rospy.wait_for_message("odom",Odometry)
 intial_point=PointStamped()
 intial_point.point=intial.pose.pose.position
 intial_point.header.stamp=rospy.Time.now()
 intial_point.header.frame_id='map'
 marker.points=[intial_point.point]
 pose_list=[]
 for i in range(pose_number):
  rospy.loginfo('请在地图上用 publish point 输入第%s个您希望机器人到达的位置'%(i+1))
  pose=rospy.wait_for_message("clicked_point", PointStamped)
  pose_list.append(pose)
  marker.points.append(pose.point)
  print 'position',1+i,'recieved'
 pose_list.append(intial_point)
 marker_pub.publish(marker)
 quest=raw_input('您希望巡航(即机器人在选的位置之间循环往复)吗?y/n: ')
 if quest=='y':
  times=raw_input('请输入巡航的次数,或者机器人默认将会无限次循环:')
  try:
   times=int(times)
   for i in range(times):
    rospy.loginfo('第%s轮导航开始'%(i+1))
    tasks(pose_number,pose_list)
  except:
   while not rospy.is_shutdown():
    rospy.loginfo('无限次导航开始')
    tasks(pose_number,pose_list)
 else:
  rospy.loginfo('单次导航开始') 
  tasks(pose_number,pose_list)
Пример #25
0
    def __init__(self):

        # Initialize constants
        self.JOINTS_COUNT = 2                           # Number of joints to manage
        self.ERROR_THRESHOLD = 0.01                     # Report success if error reaches below threshold
        self.TIMEOUT_THRESHOLD = rospy.Duration(5.0)    # Report failure if action does not succeed within timeout threshold

        # Initialize new node
        rospy.init_node(NAME, anonymous=True)

        # Initialize publisher & subscriber for left finger
        self.left_finger_frame = 'arm_left_finger_link'
        self.left_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
        self.left_finger_pub = rospy.Publisher('finger_left_controller/command', Float64)
        rospy.Subscriber('finger_left_controller/state', JointControllerState, self.read_left_finger)
        rospy.wait_for_message('finger_left_controller/state', JointControllerState)

        # Initialize publisher & subscriber for right finger
        self.right_finger_frame = 'arm_right_finger_link'
        self.right_finger = JointControllerState(set_point=0.0, process_value=0.0, error=1.0)
        self.right_finger_pub = rospy.Publisher('finger_right_controller/command', Float64)
        rospy.Subscriber('finger_right_controller/state', JointControllerState, self.read_right_finger)
        rospy.wait_for_message('finger_right_controller/state', JointControllerState)

        # Initialize action server
        self.result = SmartArmGripperResult()
        self.feedback = SmartArmGripperFeedback()
        self.feedback.gripper_position = [self.left_finger.process_value, self.right_finger.process_value]
        self.server = SimpleActionServer(NAME, SmartArmGripperAction, self.execute_callback)

        # Reset gripper position
        rospy.sleep(1)
        self.reset_gripper_position()
        rospy.loginfo("%s: Ready to accept goals", NAME)
Пример #26
0
    def __init__(self):
        self._last_frame = None
        self._bridge = CvBridge()
        self.simduration = 10
        self.learner = MockLearner()

        rospy.init_node('SNN_Cockpit', disable_signals=True)
        self._retina_image_subscriber = rospy.Subscriber('/spiky/retina_image', rosmsg.Image, self._handle_frame,
                                                         queue_size=1)
        rospy.wait_for_message('/spiky/retina_image', rosmsg.Image)

        if self._last_frame is not None:
            shape = np.shape(self._last_frame)
            self._width = shape[0]
            self._height = shape[1]

        self.postsynaptic_learning_neurons = [5033, 5034, 5039, 5040, 5041, 5042, 5043, 5044, 5045, 5046, 5047, 5048]
        self.plastic_connections = [[5027, 5039, 0, 50, 0], [5027, 5040, 0, 50, 1], [5027, 5041, 0, 50, 2],
                                    [5027, 5042, 0, 50, 3], [5027, 5043, 0, 50, 4], [5027, 5044, 0, 50, 5],
                                    [5027, 5045, 0, 50, 6], [5027, 5046, 0, 50, 7], [5027, 5047, 0, 50, 8],
                                    [5027, 5048, 0, 50, 9], [5028, 5039, 0, 50, 0], [5028, 5040, 0, 50, 1],
                                    [5028, 5041, 0, 50, 2], [5028, 5042, 0, 50, 3], [5028, 5043, 0, 50, 4],
                                    [5028, 5044, 0, 50, 5], [5028, 5045, 0, 50, 6], [5028, 5046, 0, 50, 7],
                                    [5028, 5047, 0, 50, 8], [5028, 5048, 0, 50, 9], [5039, 5033, 0, 51, 0],
                                    [5039, 5034, 0, 51, 1], [5040, 5033, 0, 51, 0], [5040, 5034, 0, 51, 1],
                                    [5041, 5033, 0, 51, 0], [5041, 5034, 0, 51, 1], [5042, 5033, 0, 51, 0],
                                    [5042, 5034, 0, 51, 1], [5043, 5033, 0, 51, 0], [5043, 5034, 0, 51, 1],
                                    [5044, 5033, 0, 51, 0], [5044, 5034, 0, 51, 1], [5045, 5033, 0, 51, 0],
                                    [5045, 5034, 0, 51, 1], [5046, 5033, 0, 51, 0], [5046, 5034, 0, 51, 1],
                                    [5047, 5033, 0, 51, 0], [5047, 5034, 0, 51, 1], [5048, 5033, 0, 51, 0],
                                    [5048, 5034, 0, 51, 1]]

        self.spike_events = [{'senders': [5027, 5027, 5027]}, {'senders': [5028, 5028]}]

        self.weights = np.random.rand(len(self.plastic_connections))
Пример #27
0
def main():
    rospy.init_node(NAME)
    # Init Globals with their type
    global status_list
    status_list = []
    global cant_reach_list
    cant_reach_list = []

    global actionGoalPublisher

    # Speech stuff
    global soundClient
    soundClient = SoundClient()

    init_point2pont()
    actionStatus = rospy.Subscriber('move_base/status', GoalStatusArray, status_callback)
    actionGoalPublisher = rospy.Publisher('move_base/goal', MoveBaseActionGoal, queue_size=10)
    frontierSub = rospy.Subscriber('grid_frontier', GridCells, frontier_callback)
    rospy.wait_for_message('grid_frontier', GridCells)

    rospy.sleep(1)
    driveTo.goalID = getHighestStatus()

    soundClient.say("Starting Driving")
    global closestGoal
    while not rospy.is_shutdown():
        rospy.loginfo("Waiting for a little while")
        rospy.sleep(2) #Allow the goal to be calculated
        rospy.loginfo("Getting the closest goal")
        closesGoalCopy = getClosestGoal(2)
        rospy.loginfo("Getting the current location")
        driveTo(closesGoalCopy[0], closesGoalCopy[1], closesGoalCopy[2])

    rospy.spin()
Пример #28
0
    def detect(self, timeout=10):
        marker_message = rospy.wait_for_message(self.marker_topic, MarkerArray, timeout=timeout)
        camera_message = rospy.wait_for_message(self.camera_topic, CameraInfo, timeout=timeout)
        camera_matrix = np.array([0])
        for marker in marker_message.markers:
            if marker.id == self.marker_number:
                marker_pose = np.array(
                    quaternion_matrix(
                        [
                            marker.pose.orientation.x,
                            marker.pose.orientation.y,
                            marker.pose.orientation.z,
                            marker.pose.orientation.w,
                        ]
                    )
                )
                marker_pose[0, 3] = marker.pose.position.x
                marker_pose[1, 3] = marker.pose.position.y
                marker_pose[2, 3] = marker.pose.position.z
                marker_pose = np.delete(marker_pose, 3, 0)

                camera_intrinsics = np.array(camera_message.K).reshape(3, 3)

                print camera_intrinsics
                print marker_pose
                camera_matrix = np.dot(camera_intrinsics, marker_pose)
                print camera_matrix
        return camera_matrix
Пример #29
0
def listener1():
#Initializations
	global bridge 
	rospy.init_node('listener', anonymous=True)
	while not rospy.is_shutdown():
		res = raw_input("Press enter to continue")
#Get IR image
		messageir = rospy.wait_for_message("/camera/ir/image_raw", Image)
		print 'received image of type: "%s"' % messageir.header
		print 'received image width = %s and height = %s' %(messageir.width, messageir.height)
		# print 'data type: %s' % data.encoding	
		try:
			cv_image_ir = bridge.imgmsg_to_cv(messageir, "mono16")
		except CvBridgeError, e:
			print e
		cv_image_ir = np.array(cv_image_ir, dtype=np.uint16)
		cv_image_ir = cv2.convertScaleAbs(cv_image_ir, alpha=1.0)

		#cv_image8 = cv2.medianBlur(cv_image8,3) 

#Get RGB image
		messagergb = rospy.wait_for_message("/camera/rgb/image_color", Image)
		print 'received image of type: "%s"' % messagergb.header
		print 'received image width = %s and height = %s' %(messagergb.width, messagergb.height)
		try:
			cv_image_rgb = bridge.imgmsg_to_cv(messagergb, "bgr8")
		except CvBridgeError, e:
			print e
Пример #30
0
    def feedback_cb(self, feedback):
        if not self.in_feedback :
            self.in_feedback=True

            try:
                current = rospy.wait_for_message('/current_node', std_msgs.msg.String, timeout=10.0)
            except rospy.ROSException :
                rospy.logwarn("Failed to get current node")
                return
            
            print current.data
            if current.data == 'none':
                try:
                    pos = rospy.wait_for_message('/robot_pose', Pose, timeout=10.0)
                except rospy.ROSException :
                    rospy.logwarn("Failed to get robot pose")
                    return
                
                add_node = rospy.ServiceProxy('/topological_map_manager/add_topological_node', strands_navigation_msgs.srv.AddNode)
                add_node('',pos)
            
                map_update = rospy.Publisher('/update_map', std_msgs.msg.Time)        
                map_update.publish(rospy.Time.now())            
            
            else:
                rospy.loginfo("I will NOT add a node within the influence area of another! Solve this and try again!")
            
            
            self.timer = Timer(1.0, self.timer_callback)      
            self.timer.start()
Пример #31
0
 def get_question(self, client_context):
     """Wait for new message"""
     return rospy.wait_for_message(self._subscription, String)
Пример #32
0
    def __init__(self):
        rospy.init_node('nav_test', anonymous=True)
        
        rospy.on_shutdown(self.shutdown)

        # How long in seconds should the robot pause at each location?
        self.rest_time = rospy.get_param("~rest_time", 10)
        
        # Are we running in the fake simulator?
        self.fake_test = rospy.get_param("~fake_test", False)
        
        # Goal state return values
        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 
                       'SUCCEEDED', 'ABORTED', 'REJECTED',
                       'PREEMPTING', 'RECALLING', 'RECALLED',
                       'LOST']
        
        # Set up the goal locations. Poses are defined in the map frame.  
        # An easy way to find the pose coordinates is to point-and-click
        # Nav Goals in RViz when running in the simulator.
        # Pose coordinates are then displayed in the terminal
        # that was used to launch RViz.
        locations = dict()
        
	locations['couloir_milieu'] = Pose(Point(-1.35, 9.31, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))
 	locations['i5_droite'] = Pose(Point(1, 13, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))
        locations['i5_gauche'] = Pose(Point(1.13, 6.94, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))
	locations['goal'] = Pose(Point(-1.82, 11.3, 0.00641), Quaternion(0.000, 0.000, 0.892, -0.451))

        # Publisher to manually control the robot (e.g. to stop it)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
        
        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        
        rospy.loginfo("Waiting for move_base action server...")
        
        # Wait 10 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(10))

        # subscribe au topic chatter, avec un appel a la fonction de callback
        rospy.Subscriber("/chatter",String, Move)
	rospy.Subscriber("/arret",String, Stop)
        
        rospy.loginfo("Connected to move base server")
        
        # A variable to hold the initial pose of the robot to be set by 
        # the user in RViz
        initial_pose = PoseWithCovarianceStamped()
        
        # Variables to keep track of success rate, running time,
        # and distance traveled
        n_locations = 3 
        n_goals = 0
        n_successes = 0
        i = n_locations
        distance_traveled = 0
        start_time = rospy.Time.now()
        running_time = 0
        location = ""
        last_location = ""
        
        # Get the initial pose from the user
        rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
        rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
        self.last_location = Pose()
        rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
        
        # Make sure we have the initial pose
        while initial_pose.header.stamp == "":
            rospy.sleep(1)
            
        rospy.loginfo("Starting navigation test")
        

        # Begin the main loop and run through a sequence of locations
        while not rospy.is_shutdown() :
            rospy.loginfo("*** MODE SEQUENTIEL")
            pub_mode = rospy.Publisher("activity_mode", String, queue_size=15)
            pub_mode.publish("SEQUENTIEL")

            # If we've gone through the current sequence,
            # start with a new random sequence
            if i == n_locations:
                i = 0
                #sequence = [sample(locations, n_locations)]

                # On remplace l'aleatoire des positions par un ordre de position bien defini
		sequence = ["i5_droite","couloir_milieu","i5_gauche"]
                rospy.loginfo("sequence "+str(sequence))
                # Skip over first location if it is the same as
                # the last location
                #if sequence[0] == last_location:
                #    i = 1
            
            # Get the next location in the current sequence
            location = sequence[i]
                        
            # Keep track of the distance traveled.
            # Use updated initial pose if available.
            if initial_pose.header.stamp == "":
                distance = sqrt(pow(locations[location].position.x - 
                                    locations[last_location].position.x, 2) +
                                pow(locations[location].position.y - 
                                    locations[last_location].position.y, 2))
            else:
                rospy.loginfo("Updating current pose.")
                distance = sqrt(pow(locations[location].position.x - 
                                    initial_pose.pose.pose.position.x, 2) +
                                pow(locations[location].position.y - 
                                    initial_pose.pose.pose.position.y, 2))
                initial_pose.header.stamp = ""
            
            # Store the last location for distance calculations
            last_location = location
            
            # Increment the counters
            i += 1
            n_goals += 1
        
            # Set up the next goal location
            self.goal = MoveBaseGoal()
            self.goal.target_pose.pose = locations[location]
            self.goal.target_pose.header.frame_id = 'map'
            self.goal.target_pose.header.stamp = rospy.Time.now()
            
            # Let the user know where the robot is going next
            rospy.loginfo("Going to: " + str(location))
            
            # Start the robot toward the next location
            self.move_base.send_goal(self.goal)
            
            # Allow 5 minutes to get there
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) 
            
            # Check for success or failure
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.loginfo("Timed out achieving goal")
            else:
                state = self.move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!")
                    n_successes += 1
                    distance_traveled += distance
                    rospy.loginfo("State:" + str(state))
                else:
                  rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
                


            # How long have we been running?
            running_time = rospy.Time.now() - start_time
            running_time = running_time.secs / 60.0
            
            # Print a summary success/failure, distance traveled and time elapsed
            rospy.loginfo("Success so far: " + str(n_successes) + "/" + 
                          str(n_goals) + " = " + 
                          str(100 * n_successes/n_goals) + "%")
            rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + 
                          " min Distance: " + str(trunc(distance_traveled, 1)) + " m")
            rospy.sleep(self.rest_time)
Пример #33
0
def main():
    # Init ROS node
    rospy.init_node('task', anonymous=True)

    # Create subscriber for position, goal, boost points, and obstacles
    rospy.Subscriber('/goal', Pose, goal_callback)
    rospy.Subscriber('/boost', PoseArray, boost_callback)
    rospy.Subscriber("/dynamic_obstacles", PoseArray,
                     dynamic_obstacles_callback)

    # Wait for resources to become active
    goal = rospy.wait_for_message("/goal", Pose).position
    boosts = rospy.wait_for_message("/boost", PoseArray).poses
    obstacles = rospy.wait_for_message("/dynamic_obstacles", PoseArray).poses

    # Create map service client
    getMap = rospy.ServiceProxy('/GlobalMap', GlobalMap)
    rospy.wait_for_service('/GlobalMap')

    try:
        raw_map = getMap()
    except rospy.ServiceException as e:
        print("Map service error: " + str(e))
        return

    # Get map as 2D list
    world_map = util.parse_map(raw_map)

    # Print resources
    print("Wall layout:")
    util.print_map(world_map)
    print("Boost points:")
    util.print_positions(boosts)
    print("Obstacles at start:")
    util.print_positions(obstacles)

    # Initialize drone
    drone = Drone()
    drone.takeoff()

    # -- For example code --
    target_x = 0
    target_y = 0

    rate = rospy.Rate(30)
    while not rospy.is_shutdown():
        rate.sleep()
        # -------------------------------
        # ------Replace this example-----
        # ---------with your code!-------
        # -------------------------------
        # Find how far we are away from target
        distance_to_target = ((target_x - drone.position.x)**2 +
                              (target_y - drone.position.y)**2)**0.5

        # Do special action if we are close
        if distance_to_target < 0.5:
            # Print current distance to goal. Note that we
            # wont reach the goal, since we just move randomly
            distance_to_goal = ((drone.position.x - goal.x)**2 +
                                (drone.position.y - goal.y)**2)**0.5

            print("Distance to goal is now", distance_to_goal)

            # Generate some random point and rotation
            target_x = random.randint(-3, 3)
            target_y = random.randint(-3, 3)

            # Move to random point
            drone.set_target(target_x, target_y)
        else:
            print("distance to target:", distance_to_target)
Пример #34
0
def inferred_point_callback(msg):
    one_shot = False
    while not one_shot:
        data = rospy.wait_for_message(
            '/dish_edge_detector/output_curvature_edge', PointCloud2)
        one_shot = True
    gen = point_cloud2.read_points(data,
                                   field_names=("x", "y", "z"),
                                   skip_nans=True)
    length = 1
    A = np.empty(3, dtype=float).reshape(1, 3)
    ## Whole edge (x,y,z) points
    for l in gen:
        l = np.array(l, dtype='float')
        l = l.reshape(1, 3)
        A = np.append(A, l, axis=0)
        length += 1
    #
    ## Randomly choose one grasp point
    #idx = np.random.randint(length, size=1) #To do : change 10 to data length
    #Ax = A[idx, 0]
    #Ay = A[idx, 1]
    #Az = A[idx, 2]
    Ax = msg.point.x
    Ay = msg.point.y
    Az = msg.point.z
    print("==============================")
    print(Ax, Ay, Az)
    print("==============================")
    """
    Romdomly choose rotation theta from 0, 45, 90. (currently, 90 for test).
    Other roatation angle is fixed toward middle point.
    But currently, rotation is fixed for test. 
    """
    # euler angle will be strange when converting in eus program. Adjust parameter until solving this problem.
    #phi_list = [math.pi/2, math.pi*4/6, math.pi*5/6]
    phi_list = [math.pi / 2, math.pi * 4 / 6, math.pi * 5 / 6]
    theta = 0  #-1.54
    phi = random.choice(phi_list)  #1.2(recentry, 2.0)
    psi = 0
    phi = math.pi * 5 / 6
    random_grasp_posrot = np.array(
        (Ax, Ay, Az, phi), dtype='float').reshape(1, 1, 4)  #reshape(1,4)
    print("random grasp posrost", random_grasp_posrot)
    random_grasp_posrot_arr = InflateGraspPoint(
        random_grasp_posrot.reshape(1, 4)).calc()
    print(random_grasp_posrot_arr)
    # Inference!
    inferred_posrot_list = []
    grasp_posrot_list = []
    posearray = PoseArray()
    header = posearray.header
    header.stamp = rospy.Time(0)
    header.frame_id = "head_mount_kinect_rgb_optical_frame"
    for i in range(8):
        ts = TestSystem()
        inferred_grasp_point = ts.test(random_grasp_posrot_arr[i])
        inferred_grasp_point = inferred_grasp_point.reshape(4)
        # Find nearest edge point based on inferred point
        nearest = nearest_point(A, inferred_grasp_point[:3:])
        Ax = nearest[0]
        Ay = nearest[1]
        Az = nearest[2]
        phi = inferred_grasp_point[3] - 0.5
        # When converting from here to eus,
        """
        if phi < 1.6:
            q_phi = 0
        else:
            q_phi = phi
        """
        if (phi < 1.6):
            phi = 1.6
        elif (phi > 2.6):
            phi = 2.6
        q = tf.transformations.quaternion_from_euler(theta, phi, psi)
        pose = Pose()
        pose.position.x = Ax
        pose.position.y = Ay
        pose.position.z = Az
        pose.orientation.x = q[0]
        pose.orientation.y = q[1]
        pose.orientation.z = q[2]
        pose.orientation.w = q[3]
        posearray.poses.append(pose)
        # grasp_posrot is actual grasp point
        grasp_posrot = np.array((Ax, Ay, Az, phi), dtype='float').reshape(1, 4)
        grasp_posrot_list.append(grasp_posrot)
        print("nearest_inferred_grasp_point", grasp_posrot)
        # inferred_posrot is a inferred point but not for grasping
        inferred_posrot = np.array(
            (inferred_grasp_point[0], inferred_grasp_point[1],
             inferred_grasp_point[2], inferred_grasp_point[3]))
        inferred_posrot_list.append(inferred_posrot)

    # Save grasp_posrot and inferred_posrot
    now = datetime.datetime.now()
    walltime = str(int(time.time() * 1000000000))
    filename = 'Data/inferred_grasp_point/' + walltime + '.pkl'
    with open(filename, "wb") as f:

        pickle.dump(grasp_posrot, f)
        print("saved inferred grasp point")
    filename = 'Data/inferred_point/' + walltime + '.pkl'
    with open(filename, "wb") as ff:
        pickle.dump(inferred_posrot, ff)
        print("saved inferred point")
    pub.publish(posearray)
class GazeboTurtlebotComplicatedMazeEnv(gazebo_env.GazeboEnv):
    def __init__(self):
        # Launch the simulation with the given launchfile name
        gazebo_env.GazeboEnv.__init__(self,
                                      "TurtlebotComplicatedMaze_v0.launch")
        self.vel_pub = rospy.Publisher('/mobile_base/commands/velocity',
                                       Twist,
                                       queue_size=5)
        self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
        self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
        self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation',
                                              Empty)

        self.name_model = 'mobile_base'
        # self.name_target = 'TargetCylinder'
        self.name_hint = 'Hint'

        #get model state service
        self.model_state = rospy.ServiceProxy('gazebo/get_model_state',
                                              GetModelState)
        self.physics_properties = rospy.ServiceProxy(
            'gazebo/get_physics_properties', GetPhysicsProperties)

        self.action_space = spaces.Discrete(19)
        self.reward_range = (-np.inf, np.inf)
        self.count_loop = [0] * 50

        self.target_pos = [[-3, -3.5], [-3.5, 0], [3.5, -3.5], [3, 1.5]]
        self.rand_target = 0
        self.hint = [[3, 1], [4, 3]]

        self.set_model = rospy.ServiceProxy('gazebo/set_model_state',
                                            SetModelState)

        rospy.wait_for_service('gazebo/set_model_state')

        self.channel = 1
        self.width = 32
        self.height = 32
        self.num_action = 7
        self.last_distance = 10000
        self.old_pose = 0
        self.num_state = 105
        self.num_target = 4

        self._seed()

    def calculate_observation(self, data, distance):
        min_range = 0.21
        target_min_range = 0.21
        done = 0
        for i, item in enumerate(data.ranges):
            if (min_range > data.ranges[i] > 0):
                done = 1
        if (distance < target_min_range):
            done = 2
        return done

    def setTarget(self):
        state = ModelState()
        state.model_name = self.name_target
        state.reference_frame = "world"
        state.pose.position.x = self.target_pos[self.rand_target][0]
        state.pose.position.y = self.target_pos[self.rand_target][1]
        self.set_model(state)

    def discretize_observation(self, data, vel_cmd, pos, new_pose, twist,
                               num_step, distance):
        observation = []
        mod = len(data.ranges) / 100
        for i, item in enumerate(data.ranges):
            if (i % mod == 0):
                if data.ranges[i] == float('Inf') or np.isinf(data.ranges[i]):
                    observation.append(21)
                elif np.isnan(data.ranges[i]):
                    observation.append(0)
                else:
                    observation.append(data.ranges[i])
        observation.append(twist.linear.x)
        observation.append(twist.angular.z)
        observation.append(self.target_pos[self.rand_target][0] - pos.x)
        observation.append(self.target_pos[self.rand_target][1] - pos.y)
        observation.append(distance)
        #observation.append(num_step)
        # print vel_cmd
        # print twist
        # observation = [self.old_pose.position.x, self.old_pose.position.y, self.old_pose.orientation.x,  self.old_pose.orientation.y,  self.old_pose.orientation.z,  self.old_pose.orientation.w, new_pose.position.x, new_pose.position.y, new_pose.orientation.x, new_pose.orientation.y, new_pose.orientation.z, new_pose.orientation.w]
        """image_data = None
        cv_image = None
        n = 0
        while image_data is None:
            try:
                image_data = rospy.wait_for_message('/camera/rgb/image_raw', Image, timeout=5)
                h = image_data.height
                w = image_data.width
                cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")
            except:
                n += 1
                if n == 10:
                    print "Camera error"
                    state = []
                    done = True
                    return state
        cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        cv_image = cv2.resize(cv_image, (self.height, self.width))
        cv2.imshow("image", cv_image)
        cv2.waitKey(3)
        cv_image = cv_image.reshape(1, 32, 32, 1)
        # print cv_image.shape"""
        return np.asarray(observation)

    def _seed(self, seed=None):
        self.np_random, seed = seeding.np_random(seed)
        return [seed]

    def calculate_distance(self, x, y, a, b):
        return math.sqrt((x - a) * (x - a) + (y - b) * (y - b))

    def calculate_reward(self, done, pos, vel_cmd, num_step):
        min_range = 0.21
        target_min_range = 0.21
        reward = -1
        distance = self.calculate_distance(
            pos.x, pos.y, self.target_pos[self.rand_target][0],
            self.target_pos[self.rand_target][1])
        reward = (self.last_distance - distance) * 100
        if done == 2:
            reward = 200
        elif done == 1:
            reward = -200
        """else:
            for i in range(2):
                if (self.calculate_distance(pos.x, pos.y, self.hint[i][0], self.hint[i][1]) < min_range):
                    reward = 10"""

        return reward

    def _step(self, action):
        rospy.wait_for_service('/gazebo/unpause_physics')
        try:
            self.unpause()
        except rospy.ServiceException, e:
            print("/gazebo/unpause_physics service call failed")
        #print action
        num_step = action[1]
        action = action[0]
        vel_cmd = Twist()
        max_linear_speed = 2
        linear_vel = (math.floor(
            action / 7)) * max_linear_speed * 0.1  #(0, 0.2, 0.4)
        vel_cmd.linear.x = 0.2
        max_ang_speed = 3.3
        ang_vel = (action - 3) * max_ang_speed * 0.1  #from (-1 to + 1)

        vel_cmd.angular.z = ang_vel
        self.vel_pub.publish(vel_cmd)

        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
            except:
                pass

        pos = self.model_state(self.name_model, "world").pose
        twist = self.model_state(self.name_model, "world").twist
        new_pose = pos
        pos = pos.position
        #print pos
        distance = math.sqrt((pos.x - self.target_pos[self.rand_target][0]) *
                             (pos.x - self.target_pos[self.rand_target][0]) +
                             (pos.y - self.target_pos[self.rand_target][1]) *
                             (pos.y - self.target_pos[self.rand_target][1]))

        done = self.calculate_observation(data, distance)
        state = self.discretize_observation(data, vel_cmd, pos, new_pose,
                                            twist, num_step, distance)
        rospy.wait_for_service('/gazebo/pause_physics')
        try:
            #resp_pause = pause.call()
            self.pause()
        except rospy.ServiceException, e:
            print("/gazebo/pause_physics service call failed")
            #reset_proxy.call()
            self.reset_proxy()
        except rospy.ServiceException, e:
            print("/gazebo/reset_simulation service call failed")

        # Unpause simulation to make observation
        rospy.wait_for_service('/gazebo/unpause_physics')
        try:
            self.unpause()
        except rospy.ServiceException, e:
            print("/gazebo/unpause_physics service call failed")
        #read laser data
        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
            except:
                pass
        pos = self.model_state(self.name_model, "world").pose
        twist = self.model_state(self.name_model, "world").twist
        self.old_pose = pos
        pos = pos.position
        self.last_distance = math.sqrt(
            (pos.x - self.target_pos[self.rand_target][0]) *
            (pos.x - self.target_pos[self.rand_target][0]) +
            (pos.y - self.target_pos[self.rand_target][1]) *
            (pos.y - self.target_pos[self.rand_target][1]))
        vel_cmd = Twist()
        state = self.discretize_observation(data, vel_cmd, pos, self.old_pose,
                                            twist, 0, self.last_distance)
        rospy.wait_for_service('/gazebo/pause_physics')
Пример #37
0
def take_picture(title):
    rospy.sleep(1)
    img = bridge.imgmsg_to_cv2(
        rospy.wait_for_message('main_camera/image_raw', Image), 'bgr8')
    cv2.imwrite(title, img)
    return img
    pub_nbcomponents = rospy.Publisher("/pose_estimation/nb_components",
                                       std_msgs.msg.String,
                                       queue_size=1)

    tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer)
    broadcaster = tf2_ros.TransformBroadcaster()
    static_broadcaster = tf2_ros.StaticTransformBroadcaster()

    ## Parameters
    distance_threshold_plane = 0.004
    voxel_size = 1.0

    print(" :: Waiting for the scene reconstructed point cloud.")
    data = rospy.wait_for_message("/pose_estimation/scene_reconstructed",
                                  sensor_msgs.msg.PointCloud2,
                                  rospy.Duration(1000.0))
    scene_pcd = orh.rospc_to_o3dpc(data, remove_nans=True)

    print(":: Loading cad_model point cloud.")
    cad_file_path = "/home/carlos/git/3DVisionMobileManipulation/catkin_ws/src/pose_estimation_pkg/data/m200.ply"
    cad_model_pcd, origin_frame = load_cad_model(path=cad_file_path)
    o3d.visualization.draw_geometries([cad_model_pcd, origin_frame])
    source, source_down, source_fpfh, source_down_fpfh = preprocess_source_pcd(
        source=cad_model_pcd, voxel_size=voxel_size)

    distances = source.compute_nearest_neighbor_distance()
    avg_dist = np.mean(distances)
    print(":: Cad model has %d points and avg_dist = %.6f" %
          (len(source.points), avg_dist))
    distances = scene_pcd.compute_nearest_neighbor_distance()
Пример #39
0
    def __init__(self):
        rospy.init_node('arm_tracker')

        rospy.on_shutdown(self.shutdown)

        # Maximum distance of the target before the arm will lower
        self.max_target_dist = 1.2

        # Arm length to center of gripper frame
        self.arm_length = 0.4

        # Distance between the last target and the new target before we move the arm
        self.last_target_threshold = 0.01

        # Distance between target and end-effector before we move the arm
        self.target_ee_threshold = 0.025

        # Initialize the move group for the right arm
        self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Set the reference frame for pose targets
        self.reference_frame = REFERENCE_FRAME

        # Keep track of the last target pose
        self.last_target_pose = PoseStamped()

        # Set the right arm reference frame accordingly
        self.right_arm.set_pose_reference_frame(self.reference_frame)

        # Allow replanning to increase the chances of a solution
        self.right_arm.allow_replanning(False)

        # Set a position tolerance in meters
        self.right_arm.set_goal_position_tolerance(0.05)

        # Set an orientation tolerance in radians
        self.right_arm.set_goal_orientation_tolerance(0.2)

        # What is the end effector link?
        self.ee_link = self.right_arm.get_end_effector_link()

        # Create the transform listener
        self.listener = tf.TransformListener()

        # Queue up some tf data...
        rospy.sleep(3)

        # Set the gripper target to closed position using a joint value target
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)

        # Plan and execute the gripper motion
        right_gripper.go()
        rospy.sleep(1)

        # Subscribe to the target topic
        rospy.wait_for_message('/target_pose', PoseStamped)

        # Use queue_size=1 so we don't pile up outdated target messages
        self.target_subscriber = rospy.Subscriber('/target_pose',
                                                  PoseStamped,
                                                  self.update_target_pose,
                                                  queue_size=1)

        rospy.loginfo("Ready for action!")

        while not rospy.is_shutdown():
            try:
                target = self.target
            except:
                rospy.sleep(0.5)
                continue

            # Timestamp the target with the current time
            target.header.stamp = rospy.Time()

            # Get the target pose in the right_arm shoulder lift frame
            #target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
            target_arm = self.listener.transformPose('right_rotate', target)

            # Convert the position values to a Python list
            p0 = [
                target_arm.pose.position.x, target_arm.pose.position.y,
                target_arm.pose.position.z
            ]

            # Compute the distance between the target and the shoulder link
            dist_target_shoulder = euclidean(p0, [0, 0, 0])

            # If the target is too far away, then lower the arm
            if dist_target_shoulder > self.max_target_dist:
                rospy.loginfo("Target is too far away")
                self.right_arm.set_named_target('r_start')
                self.right_arm.go()
                rospy.sleep(1)
                continue

            # Transform the pose to the base reference frame
            target_base = self.listener.transformPose(self.reference_frame,
                                                      target)

            # Compute the distance between the current target and the last target
            p1 = [
                target_base.pose.position.x, target_base.pose.position.y,
                target_base.pose.position.z
            ]
            p2 = [
                self.last_target_pose.pose.position.x,
                self.last_target_pose.pose.position.y,
                self.last_target_pose.pose.position.z
            ]

            dist_last_target = euclidean(p1, p2)

            # Move the arm only if we are far enough away from the previous target
            if dist_last_target < self.last_target_threshold:
                rospy.loginfo("Still close to last target")
                rospy.sleep(0.5)
                continue

            # Get the pose of the end effector in the base reference frame
            ee_pose = self.right_arm.get_current_pose(self.ee_link)

            # Convert the position values to a Python list
            p3 = [
                ee_pose.pose.position.x, ee_pose.pose.position.y,
                ee_pose.pose.position.z
            ]

            # Compute the distance between the target and the end-effector
            dist_target = euclidean(p1, p3)

            # Only move the arm if we are far enough away from the target
            if dist_target < self.target_ee_threshold:
                rospy.loginfo("Already close enough to target")
                rospy.sleep(1)
                continue

            # We want the gripper somewhere on the line connecting the shoulder and the target.
            # Using a parametric form of the line, the parameter ranges from 0 to the
            # minimum of the arm length and the distance to the target.
            t_max = min(self.arm_length, dist_target_shoulder)

            # Bring it back 10% so we don't collide with the target
            t = 0.9 * t_max

            # Now compute the target positions from the parameter
            try:
                target_arm.pose.position.x *= (t / dist_target_shoulder)
                target_arm.pose.position.y *= (t / dist_target_shoulder)
                target_arm.pose.position.z *= (t / dist_target_shoulder)
            except:
                rospy.sleep(1)
                rospy.loginfo("Exception!")
                continue

            # Transform to the base_footprint frame
            target_ee = self.listener.transformPose(self.reference_frame,
                                                    target_arm)

            # Set the target gripper orientation to be horizontal
            target_ee.pose.orientation.x = 0
            target_ee.pose.orientation.y = 0
            target_ee.pose.orientation.z = 0
            target_ee.pose.orientation.w = 1

            # Update the current start state
            self.right_arm.set_start_state_to_current_state()

            # Set the target pose for the end-effector
            self.right_arm.set_pose_target(target_ee, self.ee_link)

            # Plan and execute the trajectory
            success = self.right_arm.go()

            if success:
                # Store the current target as the last target
                self.last_target_pose = target

            # Pause a bit between motions to keep from locking up
            rospy.sleep(0.5)
Пример #40
0
def give_human(req):
    p.publish("start give")
    data = rospy.wait_for_message("/arm_controller/state",
                                  JointTrajectoryControllerState, 5)
    positions_arm = data.actual.positions

    global client
    goal = TtsGoal()
    goal.rawtext.text = str("please take the cup")
    goal.rawtext.lang_id = "EN"
    goal.wait_before_speaking = float(0)

    if positions_arm[5] < 0 and positions_arm[6] < 0:
        moveit_commander.roscpp_initialize(sys.argv)

        robot = moveit_commander.RobotCommander()
        scene = moveit_commander.PlanningSceneInterface()

        group_name = "arm"
        group = moveit_commander.MoveGroupCommander(group_name)

        display_trajectory_publisher = rospy.Publisher(
            '/move_group/display_planned_path',
            moveit_msgs.msg.DisplayTrajectory,
            queue_size=20)
        waypoints = []

        wpose = group.get_current_pose().pose
        scaley = 0.2 - wpose.position.y
        # wpose.position.z -= scale * 0.1  # First move up (z)
        wpose.position.y += scaley * 1.0  # and sideways (y)
        waypoints.append(copy.deepcopy(wpose))

        scalex = 0.7 - wpose.position.x
        print(wpose.position.x)
        wpose.position.x += scalex * 1.0  # Second move forward/backwards in (x)
        waypoints.append(copy.deepcopy(wpose))

        scalez = 1.0 - wpose.position.z
        wpose.position.z += scalez * 1.0  # Second move forward/backwards in (x)
        waypoints.append(copy.deepcopy(wpose))

        # We want the Cartesian path to be interpolated at a resolution of 1 cm
        # which is why we will specify 0.01 as the eef_step in Cartesian
        # translation.  We will disable the jump threshold by setting it to 0.0 disabling:
        (plan, fraction) = group.compute_cartesian_path(
            waypoints,  # waypoints to follow
            0.005,  # eef_step
            0.0)  # jump_threshold

        display_trajectory = moveit_msgs.msg.DisplayTrajectory()
        display_trajectory.trajectory_start = robot.get_current_state()
        display_trajectory.trajectory.append(plan)
        # Publish
        display_trajectory_publisher.publish(display_trajectory)

        group.execute(plan, wait=True)
        group.execute(plan, wait=True)
        group.execute(plan, wait=True)

        client.send_goal(goal, feedback_cb="")
        client.wait_for_result()
        return ser_messageResponse(True)
    else:
        moveit_commander.roscpp_initialize(sys.argv)

        robot = moveit_commander.RobotCommander()
        scene = moveit_commander.PlanningSceneInterface()

        group_name = "arm"
        group = moveit_commander.MoveGroupCommander(group_name)

        display_trajectory_publisher = rospy.Publisher(
            '/move_group/display_planned_path',
            moveit_msgs.msg.DisplayTrajectory,
            queue_size=20)
        waypoints = []

        wpose = group.get_current_pose().pose
        scaley = -0.2 - wpose.position.y
        # wpose.position.z -= scale * 0.1  # First move up (z)
        wpose.position.y += scaley * 1.0  # and sideways (y)
        waypoints.append(copy.deepcopy(wpose))

        scalex = 0.7 - wpose.position.x
        print(wpose.position.x)
        wpose.position.x += scalex * 1.0  # Second move forward/backwards in (x)
        waypoints.append(copy.deepcopy(wpose))

        scalez = 1.0 - wpose.position.z
        wpose.position.z += scalez * 1.0  # Second move forward/backwards in (x)
        waypoints.append(copy.deepcopy(wpose))

        # We want the Cartesian path to be interpolated at a resolution of 1 cm
        # which is why we will specify 0.01 as the eef_step in Cartesian
        # translation.  We will disable the jump threshold by setting it to 0.0 disabling:
        (plan, fraction) = group.compute_cartesian_path(
            waypoints,  # waypoints to follow
            0.005,  # eef_step
            0.0)  # jump_threshold

        display_trajectory = moveit_msgs.msg.DisplayTrajectory()
        display_trajectory.trajectory_start = robot.get_current_state()
        display_trajectory.trajectory.append(plan)
        # Publish
        display_trajectory_publisher.publish(display_trajectory)

        group.execute(plan, wait=True)
        #     group.execute(plan, wait=True)
        #     group.execute(plan, wait=True)
        client.send_goal(goal, feedback_cb="")
        client.wait_for_result()
        p.publish("finish give")
        return ser_messageResponse(True)
Пример #41
0
def main():
    global bridge, saveImage, saveImageFinish
    rospy.init_node("baxter_pick_and_place_demo")
    # Set up your subscriber and define its callback
    rospy.Subscriber("/camera/depth/image_raw", Image, depth_image_callback)
    rospy.Subscriber("/camera/rgb/image_raw", Image, color_image_callback)

    # Load Gazebo Models via Spawning Services
    # Note that the models reference is the /world frame
    # and the IK operates with respect to the /base frame

    # Wait for the All Clear from emulator startup
    rospy.wait_for_message("/robot/sim/started", Empty)

    limb = 'left'
    hover_distance = 0.15  # meters
    # Starting Joint angles for left arm
    starting_joint_angles = {
        'left_w0': 0.6699952259595108,
        'left_w1': 1.030009435085784,
        'left_w2': -0.4999997247485215,
        'left_e0': -1.189968899785275,
        'left_e1': 1.9400238130755056,
        'left_s0': -0.08000397926829805,
        'left_s1': -0.9999781166910306
    }
    pnp = PickAndPlace(limb, hover_distance)
    # An orientation for gripper fingers to be overhead and parallel to the obj
    overhead_orientation = Quaternion(0, 1, 0, 0)

    # Feel free to add additional desired poses for the object.
    # Each additional pose will get its own pick and place.
    print('save image')
    cv2.imwrite('./depth.png', depthImage)
    cv2.imwrite('./color.png', colorImage)
    rospy.sleep(1)
    saveImageFinish = True
    print('calculate pose')
    try:
        rospy.wait_for_service('gqcnn_grasp')
        gqcnn_grasp = rospy.ServiceProxy('gqcnn_grasp', GQCNNGrasp)
        req = GQCNNGraspRequest()
        req.color_img_file_path = './color.png'
        req.depth_img_file_path = './depth.png'
        res = gqcnn_grasp(req)
    except rospy.ServiceException as e:
        print("Service call failed: %s" % e)

    res_size = 0
    if res:
        res_size = len(res.q_values)
    if res_size:
        q_values = 0.0
        grasps = copy.deepcopy(res.grasps[0])
        canGrasp = False
        for i in range(0, res_size):

            res.grasps[i].pose.orientation = overhead_orientation
            x = res.grasps[i].pose.position.x
            y = res.grasps[i].pose.position.y
            z = res.grasps[i].pose.position.z
            res.grasps[i].pose.position.x = z + 0.18
            res.grasps[i].pose.position.y = -x - 0.01
            res.grasps[i].pose.position.z = -y + 0.02

            # res.grasps[i].pose.position.y += 0.02
            # res.grasps[i].pose.position.z -= 0.55
            canSolveIK1 = pnp.ik_request(res.grasps[i].pose)
            res.grasps[i].pose.position.z += pnp._hover_distance
            canSolveIK2 = pnp.ik_request(res.grasps[i].pose)
            res.grasps[i].pose.position.z -= pnp._hover_distance
            canSolveIK = False
            if canSolveIK1 and canSolveIK2:
                canSolveIK = True
            if canSolveIK and q_values < res.q_values[i]:
                q_values = res.q_values[i]
                grasps = copy.deepcopy(res.grasps[i])
                canGrasp = True
        print("INFO: Get pose from gqcnn_grasp with max q_values")
        print(grasps, '\n q_value = ', q_values)
        print(pnp.ik_request(res.grasps[i].pose))
        res.grasps[i].pose.position.z += pnp._hover_distance
        print(pnp.ik_request(res.grasps[i].pose))
        res.grasps[i].pose.position.z -= pnp._hover_distance
    else:
        print('ERROR: No return from gqcnn!')
    if not canGrasp:
        print('ERROR: Can not find the grasping pose that can solve IK')
        grasps.pose = Pose(position=Point(x=0.70, y=0.15, z=-0.129),
                           orientation=overhead_orientation)

    pose1 = copy.deepcopy(grasps.pose)
    pose2 = copy.deepcopy(grasps.pose)
    pose2.position.y += 0.1

    # Move to the desired starting angles
    pnp.move_to_start(starting_joint_angles)
    pnp.pick(pose1)
    pnp.place(pose2)
    pnp.move_to_start(starting_joint_angles)
Пример #42
0
def measure():
    global dist
    telem = get_telemetry(frame_id='aruco_map')
    data = rospy.wait_for_message('rangefinder/range', Range)
    dist = telem.z - data.range
    print(dist)
Пример #43
0
#!/usr/bin/python
# -*- coding:utf-8 -*-

import rospy
from sensor_msgs.msg import LaserScan # tipo da mensagem do sensor

# Inicia o node
rospy.init_node("rplidar_plot")

# Configura um Subscriber do tópico /laser/scan
scan = LaserScan()

def scan_cb(msg):
    global scan
    scan = msg

scan_sub = rospy.Subscriber("/laser/scan", LaserScan, scan_cb)

# Após a primeira leitura do sensor, printar a medidas imediatamente
# à frente a uma frequência de 10 Hz.
rospy.wait_for_message("/laser/scan", LaserScan)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
    # scan.ranges é um array de 360 medidas de distância
    print(scan.ranges[0], len(scan.ranges))
    rate.sleep()
Пример #44
0
    def execute(self, userdata):
        # Only do it once.
        move_objects_mode = userdata.data['move_objects_mode']
        userdata.data['move_objects_mode'] = False

        # Lift the gripper up slowly so that it gets a good grip.
        curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
        res = m.move_to_global(curr.position.x,
                               curr.position.y,
                               max(curr.position.z - 0.05,
                                   GRIPPER_MIN_GLOBAL_Z),
                               'gripper',
                               velo_scale=0.02)

        curr = t.current_robot_pose('global_xyz_link', 'gripper_endpoint')
        # Update current pose with new orientation.
        grip_height = curr.position.z
        curr.position.z = GRIPPER_LIFT_HEIGHT_STRAIGHT
        #t.publish_pose_as_transform(curr, 'global_xyz_link', 'MOVE HERE', 1.0)
        res = m.move_to_global(curr.position.x,
                               curr.position.y,
                               curr.position.z,
                               'gripper',
                               orientation=curr.orientation,
                               velo_scale=0.5)
        if not res:
            rospy.logerr('Failed to perform a move (lift object gripper)')
            return 'failed'

        hold_gripper()

        scales_topic = userdata.data['picking_from'] + '_scales/weight'

        if move_objects_mode and userdata.data[
                'move_objects_between_bins_mode'] is None:
            mid_x, mid_y = POSITION_LIMITS[
                userdata.data['picking_from']]['centre']
            if curr.position.x > mid_x:
                curr.position.x -= 0.15
            else:
                curr.position.x += 0.15
            if curr.position.y > mid_y:
                curr.position.y -= 0.15
            else:
                curr.position.y += 0.15
            res, _, _ = m.move_to_global_monitor_weight(curr.position.x,
                                                        curr.position.y,
                                                        grip_height - 0.13,
                                                        'gripper',
                                                        scales_topic,
                                                        velo_scale=0.1)
            return 'grip_failed'

        if userdata.data['weight_before'] == -1:
            rospy.logerr('No pre-pick weight, ignoring weight check')
            return 'succeeded'

        try:
            weight = rospy.wait_for_message(scales_topic, Int16)
        except:
            rospy.logerr('Failed to get weight from %s, ignoring weight check')
            return 'succeeded'

        userdata.data['weight_after'] = weight.data
        dw = userdata.data['weight_before'] - weight.data

        if item_meta[userdata.data['item_to_pick']['label']].get(
                'weight') * 1000.0 < 5:
            # We can't do a weight check.
            return 'succeeded'

        if userdata.data['weight_failures'].get(
                userdata.data['item_to_pick']['label'],
                0) > 3 and (len(userdata.data['visible_objects']) < 6
                            or userdata.data.get('i_think_im_done')):
            # We've failed weight check on this item heaps of times and we're almost finished so just pick it.
            return 'succeeded'

        if dw < 4:
            # We haven't got anything, go back to the camera position and try again.
            rospy.logerr('No detected change in weight, failed grasp.')
            if item_meta[userdata.data['item_to_pick']
                         ['label']]['grasp_type'] == 'grip_suck':
                return 'try_sucker'
            userdata.data['last_failed'][userdata.data['item_to_pick']
                                         ['label']] = 0
            return 'grip_failed'

        if item_meta[userdata.data['item_to_pick']['label']].get(
                'ignore_weight') == True:
            return 'succeeded'

        weight_correct, possible_matches = w.weight_matches(
            userdata.data['item_to_pick']['label'], dw,
            userdata.data['visible_objects'], 3)
        if not weight_correct:
            rospy.logerr(possible_matches)
            if userdata.data.get('i_think_im_done', False) == True:
                # Don't do weight reclassifications at the end.
                pass
            elif userdata.data['move_objects_between_bins_mode'] is not None:
                pass
            elif len(possible_matches) == 1 and possible_matches[0][0] < 5:
                # There's only one thing it could actually be!
                if userdata.data['task'] == 'stow' or (
                        userdata.data['task'] == 'pick'
                        and possible_matches[0][1]
                        in userdata.data['wanted_items']):
                    rospy.logerr("I'M CHANGING THE OBJECT TO %s" %
                                 possible_matches[0][1])
                    userdata.data['weight_reclassifications'].append(
                        (userdata.data['item_to_pick']['label'],
                         possible_matches[0][1]))
                    userdata.data['item_to_pick']['label'] = possible_matches[
                        0][1]
                    return 'succeeded'

            # Put the item back down and bail.
            if userdata.data['item_to_pick']['label'] not in userdata.data[
                    'weight_failures']:
                userdata.data['weight_failures'][userdata.data['item_to_pick']
                                                 ['label']] = 1
            else:
                userdata.data['weight_failures'][userdata.data['item_to_pick']
                                                 ['label']] += 1
            userdata.data['last_failed'][userdata.data['item_to_pick']
                                         ['label']] = 0
            rospy.logerr("WEIGHT DOESN'T MATCH: MEASURED: %s, EXPECTED: %s" %
                         (dw, item_meta[userdata.data['item_to_pick']
                                        ['label']].get('weight') * 1000.0))

            # Move towards the centre.
            mid_x, mid_y = POSITION_LIMITS[
                userdata.data['picking_from']]['centre']
            if curr.position.x > mid_x:
                curr.position.x -= 0.08
            else:
                curr.position.x += 0.08
            if curr.position.y > mid_y:
                curr.position.y -= 0.08
            else:
                curr.position.y += 0.08

            res, _, _ = m.move_to_global_monitor_weight(curr.position.x,
                                                        curr.position.y,
                                                        grip_height - 0.13,
                                                        'gripper',
                                                        scales_topic,
                                                        velo_scale=0.1)
            if not res:
                rospy.logerr('Failed to perform a move')
                return 'failed'

            return 'grip_failed'

        return 'succeeded'
    def callback_no_topic(self):

        data = rospy.wait_for_message("/camera/rgb/image_raw", Image)
        #data = rospy.wait_for_message("usb_cam/image_raw", Image)
        try:
            cv_image_full = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

        if self.first:
            self.r = cv2.selectROI(cv_image_full)
            self.first = False

        if cv2.getTrackbarPos(self.switch, 'camera') == 1:
            self.r = cv2.selectROI(cv_image_full)

        cv_image = cv_image_full[int(self.r[1]):int(self.r[1] + self.r[3]),
                                 int(self.r[0]):int(self.r[0] + self.r[2])]

        # BACKGROUND SUBSTRACTION
        output = cv_image
        cv_image = cv2.filter2D(cv_image, -1, self.blur)

        # CONVERT TO HSV
        hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        # BLUR TO GET RID OF NOISE
        cv_image = cv2.filter2D(cv_image, -1, self.blur)

        # get info from track bar and appy to result
        h_low = cv2.getTrackbarPos('H_low', 'camera')
        s_low = cv2.getTrackbarPos('S_low', 'camera')
        v_low = cv2.getTrackbarPos('V_low', 'camera')
        h_high = cv2.getTrackbarPos('H_high', 'camera')
        s_high = cv2.getTrackbarPos('S_high', 'camera')
        v_high = cv2.getTrackbarPos('V_high', 'camera')

        lower_green = np.array([h_low, s_low, v_low])
        upper_green = np.array([h_high, s_high, v_high])

        mask = cv2.inRange(hsv, lower_green, upper_green)
        mask2 = mask
        #mask = cv2.medianBlur(mask, 5)
        #mask = cv2.medianBlur(mask, 5)
        mask3 = mask
        mask3 = cv2.bitwise_not(mask3)
        invert = mask3

        #cv_image3 = cv_image
        #cv_image2 = cv2.bitwise_and(cv_image, cv_image, mask=mask)
        #cv_image = cv_image2

        #imgray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)

        # TEST EXTRAIRE OBJET
        im2, contours, hierarchy = cv2.findContours(mask3, cv2.RETR_TREE,
                                                    cv2.CHAIN_APPROX_SIMPLE)

        #im2, contours, hierarchy = cv2.findContours(imgray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        if len(contours) != 0:
            # draw in blue the contours that were founded
            #cv2.drawContours(output, contours, -1, 255, 3)

            # find the biggest area
            c = max(contours, key=cv2.contourArea)
            cv2.drawContours(output, c, -1, 255, 3)
            x, y, w, h = cv2.boundingRect(c)
            # draw the book contour (in green)
            cv2.rectangle(output, (x, y), (x + w, y + h), (0, 255, 0), 2)

        #cv2.drawContours(cv_image, contours, -1, (0, 255, 0), 3)

        #cv2.grabCut(cv_image, mask, rect, bgdModel, fgdModel, 5, cv2.GC_INIT_WITH_RECT)
        #mask2 = np.where((mask == 2) | (mask == 0), 0, 1).astype('uint8')
        #cv_image = cv_image * mask2[:, :, np.newaxis]

        #cv_image = self.add_blobs(cv_image)
        #cv2.rectangle(cv_image, (x1,y1), (x2,y2), (255,0,0), 2)

        cv2.imshow('inRange', mask2)
        cv2.imshow('invert', invert)
        cv2.imshow('camera', output)
        cv2.waitKey(1)

        try:
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
        except CvBridgeError as e:
            print(e)
                else:
                    speed.angular.z = 0.3
                    speed.linear.x = 0.3 * min(self.regions['frontLeft'], self.regions['frontRight'])
                if speed.angular.z > 1:
                    speed.angular.z = 0.9
                elif speed.angular.z < -1:
                    speed.angular.z = -0.9

        self.pubVel.publish(speed)



if __name__ == '__main__':
    #instantiate the class and set some parameters
    app = Robot()

    print "receiving goal points..."
    msg = rospy.wait_for_message('goals', PointArray)   #wait until first message of 'goals'
    app.goals = msg.goals                               #fill array with all goals from message
    app.noGoals = len(app.goals)                        #get number of goals to reach
    app.goal.x = app.goals[app.currentGoal].x           #start with first goal at index 0 of goals array
    app.goal.y = app.goals[app.currentGoal].y
    print "First goal is goal nr 1 with the coordinates: \n" + str(app.goal)

    print "I am starting to navigate to my goal points"

    #run the script in an infite loop to continously read and process laserdata
    while(True):
        app.run()
        app.r.sleep()
def get_frame():
	frame = rospy.wait_for_message("/camera/image_color", Image, 0.5)
	return frame
Пример #48
0
if __name__ == '__main__':


    rospy.init_node("rgbd_capturer")
    rospy.loginfo("Starting rgbd_capturer node")
    bridge = cv_bridge.CvBridge()
    
    img_id = 0
    save_root = "./"
    min_depth, max_depth = 0.25, 3.5
    print("min_depth: " + str(min_depth) + "  max_depth: " + str(max_depth))

    while True:
        
        # get rgb-depth from camera
        rgb_msg = rospy.wait_for_message("/rgb/image_raw", Image)
        depth_msg = rospy.wait_for_message("/depth_to_rgb/image_raw", Image)
        rgb = bridge.imgmsg_to_cv2(rgb_msg,desired_encoding='bgr8')
        depth = bridge.imgmsg_to_cv2(depth_msg, desired_encoding='32FC1')

        # depth normalization (0.25 ~ 1.25 -> 0 ~ 255)
        depth_im = depth
        depth_im[depth_im > max_depth] = max_depth
        depth_im[depth_im < min_depth] = min_depth
        depth_im = np.uint8(255 * (depth_im - min_depth) / (max_depth - min_depth))
        depth_im = np.repeat(np.expand_dims(depth_im, -1), 3, -1)
        rgbd_im = np.hstack([rgb, depth_im])
        rgbd_im = cv2.resize(rgbd_im, (1280, 360))
        cv2.imshow("rgbd - img_id: " + str(img_id) + "   S: Save / Q: Quit", rgbd_im )

        key = cv2.waitKey(1)
def main(arg):
    adapt_node_name = arg

    global msg_list, node_list, bus

    #load parameters
    try:
        #can_dev = rospy.get_param("/" + adapt_node_name + "/bus/device") #,'can0')
        #default_vel_ratio = rospy.get_param("/" + adapt_node_name + "/defaults/vel_to_device")
        default_resolution = rospy.get_param("/" + adapt_node_name +
                                             "/defaults/encoder_resolution")
        CAN_nodes = rospy.get_param("/" + adapt_node_name + "/nodes")
        controlled_joint_names = rospy.get_param(
            "/joint_group_position_controller")['joints']
        description = rospy.get_param("robot_description")
    except KeyError:
        rospy.loginfo("Parameter value not set")

    # get transmission from URDF
    robot = xml.dom.minidom.parseString(description).getElementsByTagName(
        'robot')[0]
    for child in robot.childNodes:
        if child.nodeType is child.TEXT_NODE:
            continue
        if child.localName == 'transmission':
            try:
                actuator = child.getElementsByTagName('actuator')[0]
                reduction = actuator.childNodes[
                    1]  #getElementByTagName('mechanicalReduction')[0]
                reduction_ratio = float(reduction.childNodes[0].nodeValue)
                trans_joint = child.childNodes[
                    3]  #getElementByTagName('joint')[0]
                trans_joint_name = trans_joint.getAttribute('name')
            except KeyError:
                rospy.logwarn("No mechanical reduction specified in URDF")
            ratio = reduction_ratio * default_resolution * 0.5 / pi * 10

            for node in CAN_nodes:
                if node['name'] == trans_joint_name:
                    node_list[trans_joint_name] = {
                        'id': node['id'],
                        'ratio': ratio
                    }

    rospy.init_node('module_state_adaptor')
    rospy.Subscriber("/joint_states", JointState, callback)

    pub = rospy.Publisher('/joint_group_position_controller/command',
                          Float64MultiArray,
                          queue_size=1)
    r = rospy.Rate(20)  # 10hz

    #initialize pos
    pos = []
    for i in range(len(controlled_joint_names)):
        pos.append(0)

    rospy.wait_for_message("/joint_states", JointState)
    rospy.sleep(2)

    while not rospy.is_shutdown():
        i = 0
        for jnt in controlled_joint_names:
            pos[i] = msg_list[jnt]['pos']
            i = i + 1

        pub.publish(Float64MultiArray(data=pos))
        r.sleep()
Пример #50
0
    def __init__(self):
        rospy.init_node('nav_test', anonymous=True)

        rospy.on_shutdown(self.shutdown)

        # How long in seconds should the robot pause at each location?
        self.rest_time = rospy.get_param("~rest_time", 2)

        # Are we running in the fake simulator?
        self.fake_test = rospy.get_param("~fake_test", False)

        # Goal state return values
        goal_states = [
            'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
            'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
        ]

        locations = dict()

        locations['A'] = Pose(Point(1.279, -0.265, 0.000),
                              Quaternion(0.000, 0.000, 0.318, 0.948))
        locations['B'] = Pose(Point(2.539, 1.657, 0.000),
                              Quaternion(00.000, 0.000, 0.000, 1.000))
        locations['C'] = Pose(Point(0.134, 1.541, 0.000),
                              Quaternion(0.000, 0.000, 0.000, -1.000))
        locations['D'] = Pose(Point(-0.324, -0.276, 0.000),
                              Quaternion(0.000, 0.000, 0.000, 1.000))

        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base",
                                                      MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))

        rospy.loginfo("Connected to move base server")

        # A variable to hold the initial pose of the robot to be set by
        # the user in RViz
        initial_pose = PoseWithCovarianceStamped()

        # Variables to keep track of success rate, running time,
        # and distance traveled
        n_locations = len(locations)
        n_goals = 0
        n_successes = 0
        i = n_locations
        distance_traveled = 0
        start_time = rospy.Time.now()
        running_time = 0
        location = ""
        last_location = ""
        self.Dist = 0

        # Get the initial pose from the user (how to set)
        rospy.loginfo(
            "*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
        )
        rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
        self.last_location = Pose()

        rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
                         self.update_initial_pose)

        # Make sure we have the initial pose
        while initial_pose.header.stamp == "":
            rospy.sleep(1)

        rospy.loginfo("Starting navigation test")

        ###### Modify by XF 2017. 4.21 #########
        dict_keys = ['A', 'B', 'C', 'D']
        pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5)
        pub2 = rospy.Publisher('/voice/xf_asr_topic', Int32, queue_size=5)
        pub3 = rospy.Publisher('arm', String, queue_size=5)
        pub4 = rospy.Publisher('tracking', Int32, queue_size=5)
        #######################################

        # Begin the main loop and run through a sequence of locations
        while not rospy.is_shutdown():
            #        while i<4:
            if i == n_locations:
                i = 0
                #                sequence = sample(locations, n_locations)
                # Skip over first location if it is the same as
                # the last location
                if dict_keys[0] == last_location:
                    i = 1

            #####################################
            ####### Modify by XF 2017.4.14 ######


#            location = sequence[i]
#           rospy.loginfo("location= " + str(location))
#            location = locations.keys()[i]
            location = dict_keys[i]
            #####################################

            # Keep track of the distance traveled.
            # Use updated initial pose if available.
            if initial_pose.header.stamp == "":
                distance = sqrt(
                    pow(
                        locations[location].position.x -
                        locations[last_location].position.x, 2) + pow(
                            locations[location].position.y -
                            locations[last_location].position.y, 2))
            else:
                rospy.loginfo("Updating current pose.")
                distance = sqrt(
                    pow(
                        locations[location].position.x -
                        initial_pose.pose.pose.position.x, 2) + pow(
                            locations[location].position.y -
                            initial_pose.pose.pose.position.y, 2))
                initial_pose.header.stamp = ""

            # Store the last location for distance calculations
            last_location = location

            # Increment the counters
            i += 1
            n_goals += 1

            # Set up the next goal location
            self.goal = MoveBaseGoal()
            self.goal.target_pose.pose = locations[location]
            self.goal.target_pose.header.frame_id = 'map'
            self.goal.target_pose.header.stamp = rospy.Time.now()

            # Let the user know where the robot is going next
            rospy.loginfo("Going to: " + str(location))

            # Start the robot toward the next location
            self.move_base.send_goal(self.goal)

            # Allow 6 minutes to get there
            finished_within_time = self.move_base.wait_for_result(
                rospy.Duration(360))

            # Check for success or failure
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.loginfo("Timed out achieving goal")
            else:
                state = self.move_base.get_state()

                if state == GoalStatus.ABORTED:  # can not find a plan,give feedback
                    rospy.loginfo("please get out")
                    status_n = "please get out"
                    pub1.publish(status_n)

                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!")
                    n_successes += 1
                    distance_traveled += distance
                    rospy.loginfo("State:" + str(state))

                    ############ add voice by XF 4.25 ##################	The 1st
                    #                    pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5)
                    loc = "I arrived the location " + str(location)
                    pub1.publish(loc)

                    rospy.Subscriber(
                        'dist', Int32,
                        self.callbackDist)  #now robot already adjust correctly
                    rospy.loginfo("Dist:" + str(self.Dist))

                    if str(location) == 'A':
                        commandA1 = "what can I do for you?"
                        pub1.publish(commandA1)

                        ############# now next asr ######################
                        awake = 1
                        pub2.publish(awake)
                        rospy.sleep(13)
                        commandA2 = "ok,I get the task , please wait."
                        pub1.publish(commandA2)
                    ################################################## 	The 2nd
                    if str(location) == 'B':
                        ############## add by XF 5.27 ##################
                        track = 1
                        pub4.publish(track)
                        rospy.sleep(10)
                        rospy.Subscriber('dist', Int32, self.callbackDist
                                         )  #now robot already adjust correctly
                        rospy.loginfo("Dist2:" + str(self.Dist))
                        rospy.loginfo("go away Dist:" +
                                      str(self.Dist / 1000.0 - 0.3))
                        self.goal = MoveBaseGoal()
                        self.goal.target_pose.pose.position.x = self.Dist / 1000.0 - 0.3
                        self.goal.target_pose.pose.orientation.w = 1.0
                        self.goal.target_pose.header.frame_id = 'base_footprint'
                        self.goal.target_pose.header.stamp = rospy.Time.now()
                        self.move_base.send_goal(self.goal)
                        rospy.sleep(10)
                        ################################################
                        commandB1 = "pick"
                        pub3.publish(commandB1)
                        rospy.sleep(10)
                        commandB2 = "Now I find the object,and already grabbed"
                        pub1.publish(commandB2)
                    ##################################################  The 3rd
                    if str(location) == 'C':
                        #subscribe the angle
                        #                    	rospy.Subscriber('xf_xfm_node', Int32, self.callbackXFM)
                        rospy.sleep(10)
                        commandC2 = "Master,give your drink."
                        pub1.publish(commandC2)
                        rospy.sleep(3)
                        commandC1 = "release"
                        pub3.publish(commandC1)
                    ##################################################

                else:
                    rospy.loginfo("Goal failed with error code: " +
                                  str(goal_states[state]))

            # How long have we been running?
            running_time = rospy.Time.now() - start_time
            running_time = running_time.secs / 60.0

            # Print a summary success/failure, distance traveled and time elapsed
            rospy.loginfo("Success so far: " + str(n_successes) + "/" +
                          str(n_goals) + " = " +
                          str(100 * n_successes / n_goals) + "%")
            rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
                          " min Distance: " +
                          str(trunc(distance_traveled, 1)) + " m")

            rospy.sleep(self.rest_time)
Пример #51
0
    def meas_cb(self, msg):
        with self.lock:
            # check if cam_info is valid
            for camera in msg.M_cam:
                P = camera.cam_info.P
                all_zero = True
                for i in range(12):
                    if P[i] != 0:
                        all_zero = False
                if all_zero:
                    rospy.logfatal(
                        "Camera info of %s is all zero. You should calibrate your camera intrinsics first "
                        % camera.camera_id)
                    exit(-1)

            # Modify all the camera info messages to make sure that ROI and binning are removed, and P is updated accordingly
            # Update is done in place
            #for camera in msg.M_cam:
            #    camera.cam_info = camera_info_converter.unbin(camera.cam_info)

            if rospy.has_param('optimizer_conditional_resetter_enable'):
                self.reset_flag = rospy.get_param(
                    'optimizer_conditional_resetter_enable')
            else:
                self.reset_flag = False

            if self.reset_flag == True:
                urdf_cam_ns = rospy.get_param("urdf_cam_ns")
                new_cam_ns = rospy.get_param("new_cam_ns")
                u_info = rospy.wait_for_message(urdf_cam_ns + '/camera_info',
                                                CameraInfo)
                n_info = rospy.wait_for_message(new_cam_ns + '/camera_info',
                                                CameraInfo)
                urdf_cam_frame = u_info.header.frame_id
                new_cam_frame = n_info.header.frame_id
                mounting_frame = rospy.get_param("mounting_frame")

                # if any of the frames has changed...
                if self.prev_3_frames != ():
                    if self.prev_3_frames != (urdf_cam_frame, new_cam_frame,
                                              mounting_frame):
                        self.frames_changed = True

                if self.frames_changed:
                    self.state = None
                    self.meas = []  # clear cache
                    self.timestamps = []
                    self.measurement_count = 0
                    self.prev_tf_transforms = {}
                    rospy.loginfo('Reset calibration state')
                    print "\033[43;1mTarget frames have changed. Clean up everything and start over! \033[0m\n\n"
                    self.frames_changed = False

                if len(self.tf_check_pairs) > 0:
                    self.prev_tf_pair = self.tf_check_pairs[0]
                self.tf_check_pairs = []
                self.tf_check_pairs.append(
                    (mounting_frame, urdf_cam_frame
                     ))  # only keep one pair in the list at a time

                self.prev_3_frames = (urdf_cam_frame, new_cam_frame,
                                      mounting_frame)
                self.prev_tf_pair = (mounting_frame, urdf_cam_frame)

            TheMoment = msg.header.stamp

            # check for tf transform changes
            for (frame1, frame2) in self.tf_check_pairs:
                transform = None
                print "\nLooking up transformation \033[34;1mfrom\033[0m %s \033[34;1mto\033[0m %s ..." % (
                    frame1, frame2)
                while not rospy.is_shutdown():
                    try:
                        self.tf_listener.waitForTransform(
                            frame1, frame2, TheMoment, rospy.Duration(10))
                        transform = self.tf_listener.lookupTransform(
                            frame1, frame2, TheMoment)  #((),())
                        print "found\n"
                        break
                    except (tf.LookupException, tf.ConnectivityException):
                        print "transform lookup failed, retrying..."

                if self.measurement_count > 0:
                    prev_transform = self.prev_tf_transforms[(frame1, frame2)]
                    dx = transform[0][0] - prev_transform[0][0]
                    dy = transform[0][1] - prev_transform[0][1]
                    dz = transform[0][2] - prev_transform[0][2]
                    dAngle = self.getAngle(transform[1]) - self.getAngle(
                        prev_transform[1])
                    a = self.getAxis(transform[1])
                    b = self.getAxis(prev_transform[1])
                    dAxis = (a[0] * b[0] + a[1] * b[1] +
                             a[2] * b[2]) / 1.  # a dot b

                    #print "\n"
                    #print "\033[34;1m-- Debug Info --\033[0m"
                    print "checking for pose change..."
                    print "measurement_count = %d" % self.measurement_count
                    print "    dx = %10f | < 0.0002 m" % dx
                    print "    dy = %10f | < 0.0002 m" % dy
                    print "    dz = %10f | < 0.0002 m" % dz
                    print "dAngle = %10f | < 0.00174 rad" % dAngle
                    print " dAxis = %10f | > 0.99999\n" % dAxis

                    if ((math.fabs(dx) > 0.0002) or (math.fabs(dy) > 0.0002)
                            or (math.fabs(dz) > 0.0002)
                            or (math.fabs(dAngle) > 0.00174) or
                        (math.fabs(dAxis) <
                         0.99999)):  # if transform != previous_transform:
                        print "\033[44;1mDetect pose change: %s has changed pose relative to %s since last time!\033[0m\n\n" % (
                            frame1, frame2)
                        ###self.reset()  # no, you cannot call reset() here.

                        self.state = None
                        count = len(self.meas)
                        self.meas = []  # clear cache
                        self.timestamps = []
                        rospy.loginfo('Reset calibration state')
                        print "\033[33;1m%ld\033[0m previous measurements in the cache are deleted.\n\n" % count
                    self.measurement_count = 0

                self.prev_tf_transforms[(frame1, frame2)] = transform

            # add timestamp to list
            self.timestamps.append(msg.header.stamp)

            # add measurements to list
            self.meas.append(msg)
            print "MEAS", len(self.meas)
            for m in self.meas:
                print " - stamp: %f" % m.header.stamp.to_sec()

            print "\n"

            self.measurement_count += 1

            print "\nNo. of measurements fed to optimizer: %d\n\n" % self.measurement_count

            ## self.last_stamp_pub.publish(self.meas[-1].header.stamp)

            # initialize state if needed
            if True:  #not self.state:
                self.state = CalibrationEstimate()
                camera_poses, checkerboard_poses = init_optimization_prior.find_initial_poses(
                    self.meas)
                self.state.targets = [
                    posemath.toMsg(checkerboard_poses[i])
                    for i in range(len(checkerboard_poses))
                ]
                self.state.cameras = [
                    CameraPose(camera_id, posemath.toMsg(camera_pose))
                    for camera_id, camera_pose in camera_poses.iteritems()
                ]

            print "Proceed to optimization...\n"

            # run optimization
            self.state = estimate.enhance(self.meas, self.state)

            print "\nOptimized!\n"

            # publish calibration state
            res = CameraCalibration()  ## initialize a new Message instance
            res.camera_pose = [camera.pose for camera in self.state.cameras]
            res.camera_id = [camera.camera_id for camera in self.state.cameras]
            res.time_stamp = [timestamp
                              for timestamp in self.timestamps]  #copy
            res.m_count = len(res.time_stamp)
            #  self.measurement_count
            self.pub.publish(res)
def main():
    global img_ball
    global img_arr
    global depth_ball
    global img_ballx
    global img_bally
    rospy.init_node("colorOfBall")
    image_sub = rospy.Subscriber("/camera/rgb/image_raw/compressed",
                                 CompressedImage, imageCallback)
    depth_sub = rospy.Subscriber(
        "/camera/depth_registered/image_raw/compressedDepth", CompressedImage,
        depthCallback)

    print("before wait")
    rospy.wait_for_message('/camera/rgb/image_raw/compressed', CompressedImage)
    rospy.wait_for_message(
        '/camera/depth_registered/image_raw/compressedDepth', CompressedImage)
    print("after wait")

    rate = rospy.Rate(20)

    while not rospy.is_shutdown():
        rospy.wait_for_message("/camera/rgb/image_raw/compressed",
                               CompressedImage)
        cv_image_hsv = cv2.cvtColor(img_ball, cv2.COLOR_BGR2HSV)
        (rows, cols, channels) = img_ball.shape
        #print([rows,cols,channels])
        #cv2.circle(cv_image,(320,240),10,(255,0,0))

        #blurred = cv2.GaussianBlur(cv_image,(11,11),0)
        mask = cv2.inRange(cv_image_hsv, orange_lower, orange_upper)
        mask = cv2.erode(mask, None, iterations=1)
        mask = cv2.dilate(mask, None, iterations=1)

        # find contours in the mask and initialize the current
        # (x, y) center of the ball
        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
                                cv2.CHAIN_APPROX_SIMPLE)
        cnts = imutils.grab_contours(cnts)
        center = None
        #print (cnts)
        # only proceed if at least one contour was found
        if len(cnts) > 0:
            # find the largest contour in the mask, then use
            # it to compute the minimum enclosing circle and
            # centroid
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            M = cv2.moments(c)
            #center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
            img_ballx = x
            img_bally = y
            #depth_ball = 0
            depth_ball = toDistance(float(img_arr[y, x]))
            # only proceed if the radius meets a minimum size
            if radius > 5 and radius < 160 and y > 140:
                # draw the circle and centroid on the frame,
                # then update the list of tracked points
                cv2.circle(img_ball, (int(x), int(y)), int(radius),
                           (0, 0, 255), 2)
                print([int(x), int(y), int(radius), depth_ball])
                #cv2.circle(mask, center, 5, (0, 0, 255), -1)

        cv2.imshow("Image window", img_ball)
        #print([0,0,0] + cv_image_hsv[320,240,:])
        cv2.waitKey(1) & 0xFF
        plt.imshow(img_arr[:, :])
        plt.draw()
        plt.show()
        rate.sleep()
    cv2.destroyAllWindows()
Пример #53
0
    rospy.Subscriber("/sim/left_arm/cartesianforce", WrenchStamped,
                     subscribe_force_callback_left)
    rate.sleep()

    global rf
    global lf

    pub_r = rospy.Publisher('/movo/right_arm/cartesianforce',
                            JacoCartesianVelocityCmd,
                            queue_size=10)
    pub_l = rospy.Publisher('/movo/left_arm/cartesianforce',
                            JacoCartesianVelocityCmd,
                            queue_size=10)

    while not rospy.is_shutdown():
        pub_r.publish(rf)
        rate.sleep()
        pub_l.publish(lf)
        rate.sleep()


if __name__ == "__main__":
    # Create a node
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node("lx_cartesianforce")
    rate = rospy.Rate(10)
    rospy.wait_for_message('/sim_initialized', Bool)

    get_force()

    rospy.spin()
Пример #54
0
        # also last_goal flag
        if enable_p2p == True or last_goal == True:
            i += 1
        if i == total_wp - 1:
            last_goal = True


if __name__ == '__main__':
    rospy.init_node('p2p_py')
    location_target = -1

    pix_bot_center = Pose()
    pix_bot_theta = 0

    rospy.Subscriber(ODOM_INF, Odometry, vehicleStateCallback)
    rospy.wait_for_message(ODOM_INF, Odometry, 5)
    #rospy.Subscriber("/state", Int8, stateCallback)
    rospy.Subscriber("SM_output", StateOut, stateCallback)
    rospy.wait_for_message("SM_output", StateOut, 3)
    print("[P2P] Started")
    sm_pub = rospy.Publisher("SM_input", StateIn, queue_size=1)
    last_loc_target = -1
    while not rospy.is_shutdown():
        if (location_target != -1 and enable_p2p == True):
            print("Target ID:", location_target)
            # if location_target == 12:
            #     waypoints = np.array([[0,0,0],[10,0,0],[20,0,0], [25,0,0],[32.5,10,np.pi/2], [32.5,28,np.pi/2], [32.5,30,np.pi/2]])
            # elif location_target == 3:
            #     waypoints = np.array([[32.5,38,np.pi/2], [32.5,40,np.pi/2], [25, 47, np.pi], [12.3,47, np.pi], [-10.87, 46.7, np.pi], [-24, 40, -np.pi/2], [-24, 31.5, -np.pi/2], [-50.7,28,-np.pi], [-51.7,28,-np.pi]])
            # else:
            #     print("Unknown Goal Given")
Пример #55
0
    def launch_simulation(self):
        # Wait for Gazebo services
        rospy.loginfo("Starting unreal MAV simulation setup coordination...")
        rospy.wait_for_service(self.ns_gazebo + "/unpause_physics")
        rospy.wait_for_service(self.ns_gazebo + "/set_model_state")

        # Prepare initialization trajectory command
        traj_pub = rospy.Publisher(self.ns_mav + "/command/trajectory",
                                   MultiDOFJointTrajectory,
                                   queue_size=10)
        traj_msg = MultiDOFJointTrajectory()
        traj_msg.joint_names = ["base_link"]
        transforms = Transform()
        transforms.translation.x = self.initial_position[0]
        transforms.translation.y = self.initial_position[1]
        transforms.translation.z = self.initial_position[2]
        point = MultiDOFJointTrajectoryPoint([transforms], [Twist()],
                                             [Twist()], rospy.Duration(0))
        traj_msg.points.append(point)

        # Prepare initialization Get/SetModelState service
        set_model_srv = rospy.ServiceProxy(self.ns_gazebo + "/set_model_state",
                                           SetModelState)
        get_model_srv = rospy.ServiceProxy(self.ns_gazebo + "/get_model_state",
                                           GetModelState)
        mav_name = self.ns_mav[np.max(
            [i
             for i in range(len(self.ns_mav)) if self.ns_mav[i] == "/"]) + 1:]
        pose = Pose()
        pose.position.x = self.initial_position[0]
        pose.position.y = self.initial_position[1]
        pose.position.z = self.initial_position[2]
        model_state_set = ModelState(mav_name, pose, Twist(), "world")

        # Wake up gazebo
        rospy.loginfo("Waiting for gazebo to wake up ...")
        unpause_srv = rospy.ServiceProxy(self.ns_gazebo + "/unpause_physics",
                                         Empty)
        while not unpause_srv():
            rospy.sleep(0.1)
        rospy.loginfo("Waiting for gazebo to wake up ... done.")

        # Wait for drone to spawn (imu is publishing)
        rospy.loginfo("Waiting for MAV to spawn ...")
        rospy.wait_for_message(self.ns_mav + "/imu", Imu)
        rospy.loginfo("Waiting for MAV to spawn ... done.")

        # Initialize drone stable at [0, 0, 0]
        rospy.loginfo("Waiting for MAV to stabilize ...")
        dist = 10  # Position and velocity
        while dist >= 0.1:
            traj_msg.header.stamp = rospy.Time.now()
            traj_pub.publish(traj_msg)
            set_model_srv(model_state_set)
            rospy.sleep(0.1)
            state = get_model_srv(mav_name, "world")
            pos = state.pose.position
            twist = state.twist.linear
            dist = np.sqrt((pos.x - self.initial_position[0])**2 +
                           (pos.y - self.initial_position[1])**2 +
                           (pos.z - self.initial_position[2])**2) + np.sqrt(
                               twist.x**2 + twist.y**2 + twist.z**2)
        rospy.loginfo("Waiting for MAV to stabilize ... done.")

        # Wait for unreal client
        rospy.loginfo("Waiting for unreal client to setup ...")
        rospy.wait_for_message("ue_out_in", PointCloud2)
        rospy.loginfo("Waiting for unreal client to setup ... done.")

        # Finish
        self.ready_pub.publish(String("Simulation Ready"))
        rospy.loginfo("Unreal MAV simulation setup successfully.")
Пример #56
0
Файл: arm.py Проект: Dridia1/ROS
    cur_pose = msg


mode_proxy = None
arm_proxy = None

rospy.init_node('multi', anonymous=True)

#Comm for drones
mode_proxy = rospy.ServiceProxy('mavros/set_mode', SetMode)
arm_proxy = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)

print 'communication initialization complete'
try:
    data = rospy.wait_for_message('mavros/global_position/rel_alt',
                                  Float64,
                                  timeout=5)
except:
    pass

print "wait for service"
rospy.wait_for_service('mavros/set_mode')
print "got service"

rate = rospy.Rate(10)

while not rospy.is_shutdown():
    success = None
    try:
        success = mode_proxy(1, 'OFFBOARD')
    except rospy.ServiceException, e:
def main():
    """RSDK Inverse Kinematics Pick and Place Example

    A Pick and Place example using the Rethink Inverse Kinematics
    Service which returns the joint angles a requested Cartesian Pose.
    This ROS Service client is used to request both pick and place
    poses in the /base frame of the robot.

    Note: This is a highly scripted and tuned demo. The object location
    is "known" and movement is done completely open loop. It is expected
    behavior that Baxter will eventually mis-pick or drop the block. You
    can improve on this demo by adding perception and feedback to close
    the loop.
    """
    rospy.init_node("ik_pick_and_place_demo")
    # Load Gazebo Models via Spawning Services
    # Note that the models reference is the /world frame
    # and the IK operates with respect to the /base frame
    load_gazebo_models()
    # Remove models from the scene on shutdown
    rospy.on_shutdown(delete_gazebo_models)

    # Wait for the All Clear from emulator startup
    rospy.wait_for_message("/robot/sim/started", Empty)

    limb = 'left'
    hover_distance = 0.15 # meters
    # Starting Joint angles for left arm
    starting_joint_angles = {'left_w0': 0.6699952259595108,
                             'left_w1': 1.030009435085784,
                             'left_w2': -0.4999997247485215,
                             'left_e0': -1.189968899785275,
                             'left_e1': 1.9400238130755056,
                             'left_s0': -0.08000397926829805,
                             'left_s1': -0.9999781166910306}
    pnp = PickAndPlace(limb, hover_distance)
    # An orientation for gripper fingers to be overhead and parallel to the obj
    overhead_orientation = Quaternion(
                             x=-0.0249590815779,
                             y=0.999649402929,
                             z=0.00737916180073,
                             w=0.00486450832011)
    block_poses = list()
    # The Pose of the block in its initial location.
    # You may wish to replace these poses with estimates
    # from a perception node.
    block_poses.append(Pose(
        position=Point(x=0.7, y=0.15, z=-0.129),
        orientation=overhead_orientation))
    # Feel free to add additional desired poses for the object.
    # Each additional pose will get its own pick and place.
    block_poses.append(Pose(
        position=Point(x=0.75, y=-0.2, z=-0.129),
        orientation=overhead_orientation))
    # NEW - BLOCK 2 POSES
    block_poses.append(Pose(
        position=Point(x=0.7, y=-0.1, z=-0.129),
        orientation=overhead_orientation))
    block_poses.append(Pose(
        position=Point(x=0.8, y=0.15, z=-0.129),
        orientation=overhead_orientation))
    # Move to the desired starting angles
    pnp.move_to_start(starting_joint_angles)
    idx = 0
    while not rospy.is_shutdown():
        #if idx!=0:
        #    idx = (idx+1) % len(block_poses)
        print("\nPicking...")
        pnp.pick(block_poses[idx])
        print("\nPlacing...")
        idx = (idx+1) % len(block_poses)
        pnp.place(block_poses[idx])
        # BLOCK 2
        print("\nPicking...")
        idx = (idx+1) % len(block_poses)
        pnp.pick(block_poses[idx])
        print("\nPlacing...")
        idx = (idx+1) % len(block_poses)
        pnp.place(block_poses[idx])
    return 0
                self.magreader.mag_data.y > 0
        ):  # If the robot is pointing towards the North then we stop its rotation
            self.vel_data.angular.z = 0
            self._vel_pub.publish(self.vel_data)

        #  We've left a case untouched - if (abs(self.magreader.mag_data.x) < self.tol) and (self.magreader.mag_data.y < 0) - this means that the robot is practically fully aligned with the South direction. If this happens, by skipping it we neither bring the robot to a stop, nor do we change the angular_speed, hence the robot continues moving at the
        #  angular_speed at which it entered this "window". This is a form of hysteresis, added here to prevent rapid switching of the angular_speed when the robot goes through the state of looking South - if not it could start jittering in place.
        #  However, we need to consider the case when the robot is looking South and it isn't moving. This could happen at startup, and if it happens we have to tell the robot to start moving:
        else:
            if self.vel_data.angular.z == 0:
                self.vel_data.angular.z = self.max_angular_speed
                self._vel_pub.publish(self.vel_data)

        print(
            'Robot angular speed: ' + str(self.vel_data.angular.z)
        )  # We print the angular_speed of the robot for debugging purposes
        self.r.sleep()  # wait so that robot motion is smoother


if __name__ == '__main__':
    rospy.init_node('orient_north', anonymous=False)
    northorienter = NorthOrienter()
    rospy.wait_for_message(
        '/imu/mag', MagneticField, timeout=10
    )  # wait for an initial message of type MagneticField to be published to the /imu/mag topic that we will be listening to. Timeout of 10s
    while not rospy.is_shutdown():
        try:
            northorienter.orientate_robot()
        except rospy.ROSInterruptException:
            pass
Пример #59
0
    def execute_cb(self, goal):
        # move the ptu
        self.send_feedback("Moving PTU to %f,  %f" %
                           (goal.ptu_pan, goal.ptu_tilt))
        self.command_ptu(goal.ptu_pan, goal.ptu_tilt)

        observation = Observation.make_observation(
            topics=[("/amcl_pose", PoseWithCovarianceStamped
                     ), ("/head_xtion/rgb/image_color",
                         Image), ("/head_xtion/rgb/camera_info", CameraInfo),
                    ("/head_xtion/depth_registered/points",
                     PointCloud2), ("/head_xtion/depth/camera_info",
                                    CameraInfo), ("/ptu/state", JointState)])

        self.send_feedback("Getting pointcloud")
        try:
            #      cloud = rospy.wait_for_message("/head_xtion/depth_registered/points", PointCloud2, 5)
            cloud = observation.get_message(
                '/head_xtion/depth_registered/points')
        except:
            self.send_feedback("Failed to get a pointcloud")
            self._result.found = 0
            self._as.set_succeeded(self._result)
            return

        self.send_feedback("Returning PTU home")
        self.command_ptu(0, 0)

        object_searched = goal.object_id
        rospy.loginfo("Looking for object with id: %s" % object_searched)

        self.send_feedback("Calling object identification service.")
        try:
            result = self.id_service(cloud)
        except:
            self._result.found = 0
            self._as.set_succeeded(self._result)
            return

        rospy.loginfo("Number of objects found: %s" % len(result.ids))
        found = False
        world = World()
        objects = ""
        for i in result.ids:
            rospy.loginfo(" - found %s" % i.data)
            objects += "* %s\n" % i.data[:-4]

            # TODO: Should project all other objects into observation and "kill" if not visible
            new_object = world.create_object()

            # TODO: store object pose etc

            # TODO: the classification should include class info

            classification = {}
            identification = {i.data[:-4]: 1}

            new_object.add_identification(
                "ObjectInstanceRecognition",
                ObjectIdentification(classification, identification))

            if i.data == object_searched:
                found = True
                break

        if found:
            rospy.loginfo("Found %s" % object_searched)
            self._result.found = 1
            self._as.set_succeeded(self._result)
        else:
            rospy.loginfo("Did not find %s" % object_searched)
            self._result.found = 0
            self._as.set_succeeded(self._result)

        try:
            waypoint = rospy.wait_for_message("/current_node", String, 5)
        except:
            waypoint = String("-")
        title = 'Fire Extinguisher Check'
        body = 'I checked for `%s` at %s and it was ' % (object_searched[:-4],
                                                         waypoint.data)
        if found:
            body += '*present*'
        else:
            body += '*not in place*'
        body += '.\n\nHere is an image:\n\n'

        msg_store_blog = MessageStoreProxy(collection='robblog')
        img = observation.get_message('/head_xtion/rgb/image_color')
        img_id = msg_store_blog.insert(img)
        body += '![Image of the location](ObjectID(%s))' % img_id
        if len(objects) > 0:
            body += '\n\nI found:\n\n'
            body += objects

        e = rb_utils.create_timed_entry(title=title, body=body)
        self.blog_msg_store.insert(e)
Пример #60
0
    def start(self):
        groundtruth_topics = self.param.topics.groundtruth

        # Create groundtruth service proxies
        section_proxy = rospy.ServiceProxy(
            groundtruth_topics.section, SectionSrv, persistent=True
        )
        lane_proxy = rospy.ServiceProxy(groundtruth_topics.lane, LaneSrv, persistent=True)
        obstacle_proxy = rospy.ServiceProxy(
            groundtruth_topics.obstacle, LabeledPolygonSrv, persistent=True
        )
        surface_marking_proxy = rospy.ServiceProxy(
            groundtruth_topics.surface_marking, LabeledPolygonSrv, persistent=True
        )
        traffic_sign_proxy = rospy.ServiceProxy(
            groundtruth_topics.traffic_sign, LabeledPolygonSrv, persistent=True
        )
        self.pause_physics_proxy = rospy.ServiceProxy(
            self.param.topics.pause_gazebo, EmptySrv
        )
        self.unpause_physics_proxy = rospy.ServiceProxy(
            self.param.topics.unpause_gazebo, EmptySrv
        )
        self._proxies = [
            section_proxy,
            lane_proxy,
            obstacle_proxy,
            surface_marking_proxy,
            traffic_sign_proxy,
            self.unpause_physics_proxy,
            self.pause_physics_proxy,
        ]

        rospy.wait_for_service(groundtruth_topics.section)

        self.groundtruth_status_subscriber = rospy.Subscriber(
            self.param.topics.groundtruth.status,
            GroundtruthStatus,
            callback=self.receive_groundtruth_update,
        )

        # Add all speakers
        self.label_speaker = LabelSpeaker(
            section_proxy=section_proxy,
            lane_proxy=lane_proxy,
            obstacle_proxy=obstacle_proxy,
            surface_marking_proxy=surface_marking_proxy,
            sign_proxy=traffic_sign_proxy,
        )

        # Subscribe to carstate
        self.car_state_subscriber = rospy.Subscriber(
            self.param.topics.car_state.car_state,
            CarStateMsg,
            self.receive_car_state,
            queue_size=1,
        )
        self.image_subscriber = rospy.Subscriber(
            "/camera/image_raw", ImageMsg, self.receive_image, queue_size=1
        )
        self.image_publisher = rospy.Publisher(
            self.param.topics.debug_image, ImageMsg, queue_size=1
        )
        self.label_publisher = rospy.Publisher(
            self.param.topics.image_labels, ImageLabelsMsg, queue_size=100
        )

        self.listener = tf2_ros.Buffer()
        tf2_ros.TransformListener(self.listener)

        rospy.wait_for_message(self.param.topics.car_state.car_state, CarStateMsg)