def sensor_callback(self, data): status = PendulumPose() status.header = Header() status.header.stamp = rospy.Time.now() now = rospy.get_time() delta = now - self.prev_time status.x = self.pos_filter.run(self.get_position(data.data[1])) if(abs(status.x) > self.soft_edge): self.edge_hit = True status.edge = self.edge_hit if self.leftLim: status.x = -self.track_length/2 if self.rightLim: status.x = self.track_length/2 status_bits = data.data[2] >> 8; md = status_bits & (1 << 5); ml = status_bits & (1 << 4); mh = status_bits & (1 << 3); #override magnet detection md = 1 if md: self.current_unrotated_theta = (data.data[0] & 0xFFFF) * self.theta_conv status.theta = self.rotate(self.current_unrotated_theta) else: status.theta = 0 #rospy.logerr("magnet not detected!") if ml: rospy.loginfo("magnet strength low") if mh: rospy.loginfo("magnet strength high") try: status.thetaDot = (status.theta - self.prev_theta)/delta status.xDot = self.vel_filter.run((status.x - self.prev_x)/delta) except ZeroDivisionError: rospy.logwarning("delta is 0, no time passed?") self.sensor_pub.publish(status) self.prev_time = now self.prev_theta = status.theta self.prev_x = status.x
def on_channel_closed(self, channel, reply_code, reply_text): """Invoked by pika when RabbitMQ unexpectedly closes the channel. Channels are usually closed if you attempt to do something that violates the protocol, such as re-declare an exchange or queue with different parameters. In this case, we'll close the connection to shutdown the object. :param pika.channel.Channel: The closed channel :param int reply_code: The numeric reason the channel was closed :param str reply_text: The text reason the channel was closed """ rospy.logwarning('Channel %i was closed: (%s) %s', channel, reply_code, reply_text) self._connection.close()
def on_connection_closed(self, connection, reply_code, reply_text): """This method is invoked by pika when the connection to RabbitMQ is closed unexpectedly. Since it is unexpected, we will reconnect to RabbitMQ if it disconnects. :param pika.connection.Connection connection: The closed connection obj :param int reply_code: The server provided reply_code if given :param str reply_text: The server provided reply_text if given """ self._channel = None if self._closing: self._connection.ioloop.stop() else: rospy.logwarning('Connection closed, reopening in 5s: (%s) %s', reply_code, reply_text) self._connection.add_timeout(5, self.reconnect)
def handleRequest(self, cmdName, request, stopMethod): rospy.logdebug("Received request: {}".format(cmdName)) rospy.logdebug("Duration: {}".format(request.duration)) rospy.logdebug("Magnitude: {}".format(request.magnitude)) # Do not run with invalid parameters if (request.duration == 0): rospy.logwarning("Received action with duration 0") return # Get reference to the service proxy # And initiate the action serviceFunction = SambucaCmdFunc[cmdName] serviceFunction(request.magnitude) # Run a timer callback to stop the action duration = rospy.Duration(request.duration) rospy.Timer(period=duration, callback=stopMethod, oneshot=True)
def goal_callback(self, msg): success = False rate = rospy.Rate(1) for curTime in range(self.timeOut): if self.getOdomDataActionServer.is_preempt_requested(): rospy.logwarning("action server pre-empted") success = False break self.odomData.append(self.odomReader.read()) #print(self.odomData) outOfMaze = check_if_out_maze(self.distGoal, self.odomData) if outOfMaze == True: success = True break rate.sleep() if success == True: self.result.result_odom_array = self.odomData self.getOdomDataActionServer.set_succeeded(self.result) else: rospy.loginfo("spehroDistActionServer timed out")
def callback(self, goal): rospy.loginfo("odomActionServer goal requested") success = False rate = rospy.Rate(1) for curTime in range(self.timeOut): if self.actionServer.is_preempt_requested(): rospy.logwarning("action server pre-empted") success = False break self.odomData.append(self.odomReader.read()) #print(self.odomData) outOfMaze = self.check_if_out_maze(self.distTarget, self.odomData) if outOfMaze == True: success = True break rate.sleep() self.result.result_odom_array = self.odomData if success == True: self.actionServer.set_succeeded(self.result) else: rospy.loginfo("odomActionServer timed out") self.actionServer.set_aborted()
is_calib = ros.get_param('~calibration', False) distmodel = ros.get_param('~distortion_model', 'fov') bridge = CvBridge() sequence = ros.get_param('~sequence') dataset = Sequence(sequence, calibration=is_calib) msg = 'Playback started [%s]' % sequence if loop: msg += ' [LOOP]' if start: msg += ' [START: %s s]' % start else: start = None if end: msg += ' [END: %s s]' % end else: end = None if speed != 1: msg += ' [%.2fx]' % speed if speed == 0: ros.logwarning('Speed is zero, results might not be as expected') ros.loginfo(msg) # Create Publishers clock = ros.Publisher('/clock', Clock, queue_size=10) tffer = tf.TransformBroadcaster() imu = ros.Publisher('/imu', Imu, queue_size=10) CamPub = namedtuple('CamPub', 'img cam exp') def create_camera_publisher(prefix): return (prefix, CamPub(img=ros.Publisher(prefix + '/' + img_topic, Image, queue_size=1), cam=ros.Publisher(prefix + '/camera_info',
def execute(self, goal): ###################################################################### # helper variables # Override for debug: # self.f.write(goal) r = rospy.Rate(1000) success = True rospy.logdebug("-D- robot_action in execute_cb") # publish info to the console for the user rospy.loginfo('%s: Executing trajectory' % self._action_name) # start executing the action if self._as.is_preempt_requested(): # rospy.loginfo('%s: Preempted' % self._action_name) self._as.set_preempted() success = False else: #self.command_msg.header.stamp = rospy.Time.now() #self.command_msg.name = goal.trajectory.joint_names #self._feedback.joint_names = goal.trajectory.joint_names self.set_jointstate_lookup(goal.trajectory.joint_names) rospy.loginfo("robot_action looping over %d points" % len(goal.trajectory.points)) start_time = rospy.Time.now() for i in range(len(goal.trajectory.points)): t = goal.trajectory.points[i].time_from_start rospy.loginfo("setting joints to %s, t: %s, time since start: %s" % ( str(goal.trajectory.points[i].positions), str(t), str(rospy.Time.now() - start_time))) ## check for empty arrays: # rospy.loginfo("lengths: command_msg.joints: %d robot_state_index: %d positions: %d" % ( # len( self.command_msg.joints ), len( self.robot_state_index), len( goal.trajectory.points[i].positions))) if len( self.robot_state_index )== 0: rospy.logwarn( "robot_state_index is empty" ) rospy.sleep(0.001) continue if len( self.command_msg.joints )== 0: rospy.logwarning( "command_msg is empty" ) rospy.sleep(0.001) continue for j in range(len(goal.trajectory.points[i].positions)): self.command_msg.joints[ self.robot_state_index[j] ] = goal.trajectory.points[i].positions[j] # rospy.logdebug("set command_msg.joints to %s" % str( self.command_msg.joints )) self.command_pub.publish(self.command_msg) # rospy.logdebug("robot_action waiting to reach goal #%d" % i) success = self.waitToReachGoal(self.command_msg) # wait for arrival time to make sure it's timed right while rospy.Time.now() - start_time < t: rospy.sleep(0.025) if success: rospy.loginfo("robot_action reached goal") #self._feedback.actual.positions = self.robot_state #self._as.publish_feedback(self._feedback) if not success: rospy.logerr("%s: error: did not reach goal" % self._action_name) #self._as.set_aborted(self._result) # self._as.set_aborted() return # r.sleep() # publish the feedback #self._as.publish_feedback(self._feedback) if success: rospy.loginfo('%s: Succeeded' % self._action_name) #self._as.set_succeeded(self._result) self._as.set_succeeded()
def run(self): ''' while not rospy.is_shutdown(): ret, frame = self.cap.read() self.r.sleep() ''' for frame in self.cam.capture_continuous(self.rawCapture, format="bgr", use_video_port=True): #--------- img pre-process---------------- cv_image = frame.array self.grey = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) self.grey = cv2.equalizeHist(self.grey) self.grey = cv2.GaussianBlur(self.grey, (7, 7), 0) self.rawCapture.truncate(0) # ???? # for dispaly purpose vis = self.grey.copy() #--------- Subtractor - Track every frame motion ------------ if self.robot_view_motion is False: if self.Vp_MD_Subtr_UseMog: cnts = self.subtrMog(self.grey) elif Vp_MD_Subtr_UseFrmDiff: cnts = FrmDiff(self.prev_grey, self.grey) else: # use avg cnts = avgDiff(self.grey) # got diff. loop over the contours if len(cnts) > 0: c_max = cnts[0] for c in cnts: # discard little change if cv2.contourArea(c) < self.min_area: continue if cv2.contourArea(c_max) < cv2.contourArea(c): c_max = c if DBG_WINDOW_DETECT_BOX: (x_, y_, w_, h_) = cv2.boundingRect(c) cv2.rectangle(vis, (x_, y_), (x_ + w_, y_ + h_), (255, 0, 0), 2) if cv2.contourArea( c_max ) > self.min_area: # avoid many too small c in frame (x, y, w, h) = cv2.boundingRect(c_max) cv2.rectangle(vis, (x, y), (x + w, y + h), (0, 255, 0), 4) if DBG_WINDOW_DETECT_BOX: cv2.imshow("[1]subtractor_boundingRect", vis) key = cv2.waitKey(1) self.detect_box = (x, y, w, h) self.motionDetected = True self.motionDetectedHold = True print("~~~ Motion detected. motionDetected = True") else: print( "//// No motion detected. motionDetected = Fasle") if self.Vp_ME_UseLkTractor: # if do Motion Estimation, get key points firstly self.keypoints = self.get_keypoints( self.grey, self.detect_box) print("@@@@@@ self.frame_idx, pre get keypoints:", self.frame_idx, self.keypoints) # if have points, display them if self.keypoints is not None and len(self.keypoints) > 0: for x, y in self.keypoints: cv2.circle(vis, (x, y), self.feature_size, (255, 0, 0), cv.CV_FILLED, 8, 0) if DBG_WINDOW_GET_KEYPOINTS: cv2.imshow("[1]get_keypoints", vis) key = cv2.waitKey(10) # visualMsg.motion_detected, #cv2.putText() else: # len(cnts) <= 0 self.motionDetected = False #self.detect_box = None print("//// No motion detected. motionDetected = Fasle") else: rospy.logwarning( "Err:motion detect when robot view itself has motion. Not Implemented!" ) # -------- if find motion, track it ---------- # LK Tracker if self.Vp_ME_UseLkTractor: print("///// self.keypoints before lk track is:", self.keypoints) if self.keypoints is not None and len(self.keypoints) > 0: self.md_visualMsg.roi = self.lk_tracker.run( self.prev_grey, self.grey, self.keypoints) #----- if interval , pub info ----- if self.frame_idx % self.detect_interval == 0: self.md_visualMsg.motion_detected = self.motionDetectedHold print("$$$$$ md_visualMsg.motion_detected is:", self.md_visualMsg.motion_detected) (self.md_visualMsg.roi.x, self.md_visualMsg.roi.y, self.md_visualMsg.roi.height, self.md_visualMsg.roi.width) = self.detect_box self.publish_motion() #clear motion detected hold flag after pub self.motionDetectedHold = False if DEBUG_L2: print( '~loop A interval. motion info Pubed. frame_idx is %d' % self.frame_idx) self.frame_idx += 1 self.prev_gray = self.grey #clear the stream in preparation for next frame self.rawCapture.truncate(0) #if DEBUG_L2: print ('~loop once. frame_idx is %d' %self.frame_idx ) key = cv2.waitKey(10) & 0xFF if key == 27: # esc cv2.destroyAllWindows() break