def errorCheck(pub): r = rospy.Rate(10) global count precount = 0 thread_unloop_count = 0 while True: error_flag = Bool() if (count - precount) > 0: thread_unloop_count = 0 precount = count elif (count - precount) == 0: thread_unloop_count += 1 precount = count else: thread_unloop_count = 0 precount = 0 if thread_unloop_count > 2: error_flag.data = True print'********++++++++++------------------AMU is rebooting now!!------------------++++++++******' else: error_flag.data = False #commented out by hagiwara 0224 #print'********++++++++++------------------AMU is OK!!------------------++++++++******' pub.publish(error_flag) r.sleep()
def publishOrderBegun(): pubBegunMsg = Bool() if(orderProcess == True): pubBegunMsg.data = 1 else: pubBegunMsg.data = 0 global pubOrderBegun pubOrderBegun.publish(pubBegunMsg)
def run_dryer(self, run_it=True): cmd = Bool() if run_it == True: cmd.data = True else: cmd.data = False "Updating dryer" self.dryer_pub.publish(cmd)
def pauseSystem(self, pausing): learningStatus = Bool() controlStatus = Bool() if not pausing: self.expStatus = "monitor" self.backToInit = not pausing learningStatus.data = not pausing self.learningMF_pub.publish(learningStatus) self.learningMB_pub.publish(learningStatus) self.learningMB2_pub.publish(learningStatus) controlStatus.data = not pausing self.control_pub.publish(controlStatus)
def ft_sleeve_cb(self, msg): if not self.ft_sleeve_biased: rospy.sleep(0.5) self.time_since_last_cb = rospy.Time.now() self.ft_sleeve_bias_x = msg.wrench.force.x self.ft_sleeve_bias_y = msg.wrench.force.y self.ft_sleeve_bias_z = msg.wrench.force.z self.ft_sleeve_bias_t_x = msg.wrench.torque.x self.ft_sleeve_bias_t_y = msg.wrench.torque.y self.ft_sleeve_bias_t_z = msg.wrench.torque.z self.ft_sleeve_biased = True if rospy.Time.now().to_sec() - self.time_since_last_cb.to_sec() > 0.05: print 'The force-torque sensor callback is too slow. That is potentially very bad. Aborting everything!!!' self.z.estop() self.z = None self.time_since_last_cb = rospy.Time.now() x_force = msg.wrench.force.x-self.ft_sleeve_bias_x y_force = msg.wrench.force.y-self.ft_sleeve_bias_y z_force = msg.wrench.force.z-self.ft_sleeve_bias_z x_torque = msg.wrench.torque.x-self.ft_sleeve_bias_t_x y_torque = msg.wrench.torque.y-self.ft_sleeve_bias_t_y z_torque = msg.wrench.torque.z-self.ft_sleeve_bias_t_z # pos = self.z.get_position() if self.recording: t = rospy.Time.now() - self.start_record_time self.sleeve_file.write(''.join([str(t.to_sec()), ' %f %f %f %f %f %f %f \n' % (self.zenither_pose, x_force, y_force, z_force, x_torque, y_torque, z_torque)])) self.array_to_save[self.array_line] = [t.to_sec(), self.zenither_pose, x_force, y_force, z_force] self.array_line += 1 if self.pulling: threshold = self.pulling_force_threshold else: threshold = self.reset_force_threshold # mag_force = np.linalg.norm(np.abs([x_force, y_force, z_force])) if (np.linalg.norm(np.abs([x_force, y_force, z_force])) > threshold): out = Bool() out.data = True self.force_threshold_exceeded_pub.publish(out) else: out = Bool() out.data = False self.output_stuck = 0 self.force_threshold_exceeded_pub.publish(out)
def test1(self): msg = Bool() msg.data = True # wait until advertise and receive time.sleep(0.5) self._pub.publish(msg) time.sleep(0.1) self.assertTrue(self._true) self.assertFalse(self._false) msg.data = False self._pub.publish(msg) time.sleep(0.1) self.assertTrue(self._false) self.assertFalse(self._true)
def prepare_std_msg(self): rospy.loginfo("Preparing std_msgs......") ''' Fill Bool message ''' bool_msg = Bool() bool_msg.data = False ''' Fill Float32 message ''' float32_msg = Float32() float32_msg.data = 1.0 ''' Fill Int32 message Please, do it your self for practice ''' ''' Fill String message Please, do it your self for practice ''' rospy.loginfo("std_msgs preparation done......")
def newSample(self, msg): newFFT = self.stft.ingestSample(msg.data) if newFFT is not None: self.FFT = newFFT # Mask and average data guard1 = np.mean(newFFT[self.G1_mask, :][:, self.channel_mask]) alpha = np.mean(newFFT[self.Al_mask, :][:, self.channel_mask]) guard2 = np.mean(newFFT[self.G2_mask, :][:, self.channel_mask]) detected = self.movavg.step(alpha > (guard1 + guard2)*1.1) > 0.5 if detected and not self.ignore.test(): self.movavg.reset() self.ignore.reset(4) else: detected = False # Publish messages msg = Float32() msg.data = guard1 self.pub_guard1.publish(msg) msg = Float32() msg.data = alpha self.pub_alpha.publish(msg) msg = Float32() msg.data = guard2 self.pub_guard2.publish(msg) msg = Bool() msg.data = detected self.pub_eyes.publish(msg)
def model_stateCB(self, msg): if not self.active: return quaternion_msg = [msg.pose[1].orientation.x, msg.pose[1].orientation.y, msg.pose[1].orientation.z, msg.pose[1].orientation.w] euler = tf.transformations.euler_from_quaternion(quaternion_msg) self.robot_pose = (msg.pose[1].position.x, msg.pose[1].position.y, euler[2]) distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose) angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose) if angle_to_destination < 3 * (math.pi/180): print "correct straight line" if distance_to_destination<= self.threshold_proximity: print "ready to grab" self.gazebo_cmd(0,0) self.active = False self.gripper_cmd(0,0.5) msg = Bool() msg.data = True self.pub_grab_object.publish(msg) else: self.gazebo_cmd(self.speed,0) else: print "correct orientation" w = (angle_to_destination + math.pi) / (2 * math.pi) - 0.5 self.gazebo_cmd(0,w)
def do_security(): global STATUS_LOCK, CAR_UPDATE_FLAG, CAMERA_UPDATE_FLAG global CAMERA_CURRENT_TIME, CAR_CURRENT_TIME global GLOBAL_MOVE_PUB, VEL_PUB global NAV_FLAG_PUB, ENABLE_MOVE_FLAG, BAR_FLAG time_now = rospy.Time.now() STATUS_LOCK.acquire() if CAR_UPDATE_FLAG: time1_diff = time_now - CAR_CURRENT_TIME if CAMERA_UPDATE_FLAG: time2_diff = time_now - CAMERA_CURRENT_TIME # 视觉丢失超时保险 # 里程计丢失超时保险 if (CAMERA_UPDATE_FLAG and time2_diff.to_sec() > 180 and not BAR_FLAG) or time1_diff.to_sec() > 5.: # 发布全局禁止flag global_move_flag = Bool() global_move_flag.data = False GLOBAL_MOVE_PUB.publish(global_move_flag) car_twist = Twist() VEL_PUB.publish(car_twist) CAMERA_UPDATE_FLAG = False CAR_UPDATE_FLAG = False CAMERA_CURRENT_TIME = rospy.Time.now() CAR_CURRENT_TIME = rospy.Time.now() STATUS_LOCK.release()
def talker(): rospy.init_node("frobit_test_set_speed") deadman_pub = rospy.Publisher("/fmSignals/deadman", Bool) deadman = Bool() cmd_vel_left_pub = rospy.Publisher("/fmSignals/cmd_vel_left", TwistStamped) twist_left = TwistStamped() cmd_vel_right_pub = rospy.Publisher("/fmSignals/cmd_vel_right", TwistStamped) twist_right = TwistStamped() r = rospy.Rate(node_upd_freq) # set talker update frequency while not rospy.is_shutdown(): deadman.data = 1 # publish deadman topic deadman_pub.publish(deadman) twist_left.header.stamp = rospy.Time.now() twist_left.twist.linear.x = spd_left # publish cmd_vel topic for left wheel cmd_vel_left_pub.publish(twist_left) twist_right.header.stamp = rospy.Time.now() twist_right.twist.linear.x = spd_right # publish cmd_vel topic for right wheel cmd_vel_right_pub.publish(twist_right) r.sleep()
def pub_e( self , enable ): """ pub enable status """ msg = Bool() msg.data = enable self.pub_enable.publish( msg )
def run(self): while not rospy.is_shutdown(): if time.time() - self.current_time > self.time: shutoff = Bool() shutoff.data = True self.pub_shutoff.publish(shutoff) time.sleep(1)
def navToPose(goal): """Drive to a goal subscribed to from /move_base_simple/goal""" #compute angle required to make straight-line move to desired pose global xPosition global yPosition global theta #capture desired x and y positions desiredY = goal.pose.position.y desiredX = goal.pose.position.x #capture desired angle quat = goal.pose.orientation q = [quat.x, quat.y, quat.z, quat.w] roll, pitch, yaw = euler_from_quaternion(q) desiredT = yaw * (180.0/math.pi) #compute distance to target goalDistance = math.sqrt(math.pow((desiredX - xPosition),2) + math.pow((desiredY-yPosition),2))#Distance formula #compute initial turn amount initialTurn = math.atan((desiredY-yPosition)/(desiredX-xPosition)) print "spin!" #turn to calculated angle rotate(initialTurn) print "move!" #move in straight line specified distance to new pose driveStraight(0.25, goalDistance) rospy.sleep(2) print "spin!" #spin to final angle rotate(desiredT) print "done" arrival = Bool() arrival.data = True arrival_pub.publish(arrival)
def model_stateCB(self, msg): if not self.active: return quaternion_msg = [msg.pose[1].orientation.x, msg.pose[1].orientation.y, msg.pose[1].orientation.z, msg.pose[1].orientation.w] euler = tf.transformations.euler_from_quaternion(quaternion_msg) self.robot_pose = (msg.pose[1].position.x, msg.pose[1].position.y, euler[2]) # print self.robot_pose # print 'robot_pose:', self.robot_pose#, 'waypoints:', self.waypoints self.destination_pose = self.circleIntersect(self.lookahead_distance) if self.destination_pose == None: self.active = False print "approach destination " self.gazebo_cmd(0,0) msg = Bool() msg.data = True self.pub_finish.publish(msg) else: # print 'destination_pose:', self.destination_pose # print 'waypoint_index:', self.current_waypoint_index self.speed = self.default_speed distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose) angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose) # self.steering_angle = np.arctan((2 * self.robot_length * np.sin(angle_to_destination)) / distance_to_destination) # print "robot_head",euler[2]*180/math.pi,"angle_to_destination", angle_to_destination*180/math.pi w = (angle_to_destination + math.pi) / (2 * math.pi) - 0.5 self.gazebo_cmd(self.speed,w)
def status_updated_callback(self, msg): # GoalStatusArray intermediate_statuses = [GoalStatus.PENDING, GoalStatus.ACTIVE, GoalStatus.RECALLING, GoalStatus.PREEMPTING] for goal in msg.status_list: # status = GoalManager.status_list[goal.status] # rospy.logerr("Goal %s. %s" % (status, goal.text)) goal_idx = self.get_index_of(goal.goal_id.id) if goal.status == GoalStatus.SUCCEEDED: # do a little dance to celebrate rospy.loginfo("Goal %s reached!" % goal.goal_id.id) last_goal_reached = Bool() last_goal_reached.data = not self.loop_goals and goal_idx + 1 == len(self.goals) self.done_pub.publish(last_goal_reached) self.wait_for_goal() if goal_idx + 1 == len(self.goals): if self.loop_goals: rospy.loginfo("Now looping back to first goal") self.reset_goal_list() self.next_goal() else: self.next_goal(goal_idx + 1) elif goal.status in intermediate_statuses: # transitional state, so just wait pass elif goal.status == GoalStatus.PREEMPTED: rospy.logwarn("Goal %s was preempted. %s" % (goal.goal_id.id, goal.text)) else: # the robot's dead status = GoalManager.status_list[goal.status] rospy.logerr("Goal %s %s. %s" % (goal.goal_id.id, status, goal.text))
def receive_1_cb(self, msg): #print 'message received' #print msg im = self.br.imgmsg_to_cv2(msg, desired_encoding='passthrough') for i in range(0, im.shape[0]): for j in range(0, im.shape[1]): #if not (im[i, j, 0] == 255 and im[i, j, 1] == 0 and im[i, j, 2] == 0): if im[i, j, 0] == 255 and im[i, j, 1] == 0 and im[i, j, 2] == 0: #print 'Detect an intruder!' msg_sended = Bool() msg_sended.data = True send = FunctionUnit.create_send(self, self._send_topic, Bool) send.send(msg_sended) virtual_msg = Bool() virtual_msg.data = False self._virtual_send.send(virtual_msg)
def callback_manipulator(self, controller): if controller.killswitch != self.controller.killswitch: shut_down = Bool() shut_down.data = self.controller.killswitch == 1 self.callback_auto_shut_down(shut_down) self.controller = controller self.send_manipulator = True
def cbJoy(self, joy_msg): model_state_msg = Twist() model_state_msg.linear.x = joy_msg.axes[1] * self.v_gain model_state_msg.linear.y = 0 model_state_msg.linear.z = 0 model_state_msg.angular.x = 0 model_state_msg.angular.y = 0 model_state_msg.angular.z = -joy_msg.axes[3] * self.w_gain self.pub_gazebo.publish(model_state_msg) model_state_msg.linear.x = 0 model_state_msg.angular.z = 0 if joy_msg.buttons[0] == 1: model_state_msg.angular.z = -1 elif joy_msg.buttons[1] == 1: model_state_msg.angular.z = 1 self.pub_gazebo_gripper.publish(model_state_msg) if (joy_msg.buttons[7] == 1): reset_msg = Bool() reset_msg.data = True self.pub_encoder_reset.publish(reset_msg)
def grab_close_ball(self): self.switch_to_cam("arm") mode = Bool() mode.data = True self.arm_mode_pub.publish(mode) req = False self.grab_request(req)
def talker(options): pub = rospy.Publisher("pr2_etherCAT/motors_halted", Bool) rospy.init_node(NAME, anonymous=True) while not rospy.is_shutdown(): out = Bool() out.data = options.halt pub.publish(out) rospy.sleep(1.0)
def pauseSystem(self, pausing): learningStatus = Bool() controlStatus = Bool() #if not pausing: # self.expStatus = "monitor" #self.backToInit = not pausing learningStatus.data = not pausing self.learningMF_pub.publish(learningStatus) self.learningMB_pub.publish(learningStatus) self.learningMB2_pub.publish(learningStatus) controlStatus.data = not pausing self.control_pub.publish(controlStatus) self.askStopPlanning = not pausing planDecide = CommandSignal() planDecide.decide = not pausing self.plde_pub.publish(planDecide)
def pub_e( self , enable ): """ pub actuator cmd """ msg = Bool() msg.data = enable self.pub_enable.publish( msg )
def send_move_status(msg): """ Send a movement status message. :param msg: A bool, true if moving, false if stopped. """ global move_status_pub status_msg = Bool() status_msg.data = msg move_status_pub.publish(status_msg)
def vacuum_state_cb(msg, arm): print("state call back %8s %7.2f %s"%(arm, g_force[arm], msg.data)) pub_msg = Bool(False) if g_force[arm] > 15 and msg.data is True: pub_msg.data = True if arm == "left": left_vacuum_pub.publish(pub_msg) elif arm == "right": right_vacuum_pub.publish(pub_msg)
def halt_test(): rospy.init_node('halt_test') bool_msg = Bool() bool_msg.data = True rate = rospy.Rate(10) while not rospy.is_shutdown(): pub_halt = rospy.Publisher('/collide/halt', Bool, queue_size = 10) print 'published successfully!' rate.sleep() pub_halt.publish(bool_msg)
def load(self): rospy.loginfo("Sending load cmd") cmd = Bool() cmd.data = True self.loader_pub.publish(cmd) # might need to delay here print "letting screw turn for 1 second" time.sleep(2) # turn dryer on self.dryer_pub.publish(cmd) print "Turning dryer on for 3 seconds" time.sleep(6) # turn dryer off cmd.data = False self.dryer_pub.publish(cmd)
def shutdown(self): rospy.loginfo("Stopping the robot...") # Stop the robot self.cmd_vel_pub.publish(Twist()) rospy.loginfo("enable BarDetectFlag") # enable BarDetectFlag flag=Bool() flag.data=True self.barDetectFlag_pub.publish(flag) rospy.sleep(0.5)
def _stop_teleop_cb(self): ''' Sends the stop teleop command IGC ''' msg = Bool() msg.data = True ret = QMessageBox.question(self._widget, "Stop Arm Teleop", 'Do you want to stop the arm teleoperation?', QMessageBox.Ok, QMessageBox.Cancel) if ret == QMessageBox.Ok: self.teleop_done_pub.publish(msg)
def laser_callback(self, laser_scan): if self.ping_index_ < 0: self.set_scan_properties(laser_scan) # Assume min/max angle are symmetrical and set up evenly on robot # This is the ping index that is straight ahead self.ping_index_ = int( (0 - self.angle_min_) / self.angle_increment_) rospy.loginfo("LIDAR setup: ping_index = %d", self.ping_index_) # Set the beginning and end of our scan zone self.min_index = self.ping_index_ - int(self.ALARM_SCAN_WIDTH / 2) if self.min_index < 0: rospy.logwarn("Lidar alarm scan width is too large!") self.min_index = 0 self.max_index = self.ping_index_ + int(self.ALARM_SCAN_WIDTH / 2) if self.max_index > len(laser_scan.ranges): rospy.logwarn("Lidar alarm scan width is too large") self.max_index = len(laser_scan.ranges) rospy.loginfo("min index is %d", self.min_index) rospy.loginfo("max index is %d", self.max_index) count = 0 num_pings = 0 average = 0 for x in range(self.min_index, self.max_index): if laser_scan.ranges[x] > self.range_min_: if laser_scan.ranges[x] < self.range_max_: num_pings += 1 rospy.logdebug("Ping %d with range %f", num_pings, laser_scan.ranges[x]) average += laser_scan.ranges[x] if laser_scan.ranges[x] < self.MIN_SAFE_DISTANCE: count += 1 rospy.loginfo( "%d valid lidar pings, %d dangerous pings, average distance %f", num_pings, count, float(average) / float(num_pings)) if count > (num_pings * self.ALARM_TRIP_PERCENTAGE): self.laser_alarm = True rospy.loginfo("LIDAR ALARM! DANGER!") else: self.laser_alarm = False lidar_alarm_msg = Bool() lidar_alarm_msg.data = self.laser_alarm self.lidar_alarm_publisher.publish(lidar_alarm_msg) lidar_dist_msg = Float32() lidar_dist_msg.data = self.ping_dist_in_front_ self.lidar_dist_publisher_.publish(lidar_dist_msg)
def compute_people(self, _data): b = Bool() b.data = _data self.people_compute.publish(b) rospy.loginfo("Detecting People: %s" % str(b.data))
def set_wrench_body_orientation_absolute(self, absolute): """Apply body wrench using body orientation (relative/False) or reference frame (absolute/True)""" m = Bool() m.data = absolute self.__set_wrench_body_orientation_absolute_pub.publish(m)
def compute_objects(self, _data): b = Bool() b.data = _data self.obj_compute.publish(b) rospy.loginfo("Detecting Objects: %s" % str(b.data))
def master(sx=2., sy=2., gx=2., gy=2.): # ---------------------------------- INITS ---------------------------------------------- # --- init node --- rospy.init_node('hector_motion') # --- cache global vars / constants --- global msg_motion, turtle_motion, turtle_stop, height msg_motion = None turtle_motion = None turtle_stop = None height = None # --- Publishers --- pub_target = rospy.Publisher('/hector/target', PointStamped, latch=True, queue_size=1) msg_target = PointStamped() msg_target.header.frame_id = "map" msg_target_position = msg_target.point msg_target_position.x = sx msg_target_position.y = sy pub_target.publish(msg_target) pub_state = rospy.Publisher('/hector/state', Int8, latch=True, queue_size=1) msg_state = Int8() msg_state.data = STATE_TAKEOFF pub_state.publish(msg_state) pub_stop = rospy.Publisher('/hector/stop', Bool, latch=True, queue_size=1) msg_stop = Bool() msg_stop.data = False pub_stop.publish(msg_stop) # ping to others it is ready # --- Subscribers --- rospy.Subscriber('/hector/motion', EE4308MsgMotion, subscribe_motion, queue_size=1) rospy.Subscriber('/turtle/motion', EE4308MsgMotion, subscribe_turtle_motion, queue_size=1) rospy.Subscriber('/turtle/stop', Bool, subscribe_turtle_stop, queue_size=1) rospy.Subscriber('/hector/sonar_height', Range, subscribe_sonar, queue_size=1) while (height is None or msg_motion is None or turtle_stop is None or \ turtle_motion is None or rospy.get_time() == 0) and not rospy.is_shutdown(): pass if rospy.is_shutdown(): return ###################################################### t = rospy.get_time() while not rospy.is_shutdown(): if rospy.get_time() > t: # --- FSM --- # check if close enough to targets and goals with CLOSE_ENOUGH ## from project 1 if check_distance: # check if close enough to target Di = target_x - msg_motion.x Dj = target_y - msg_motion.y if Di*Di + Dj*Dj <= CLOSE_ENOUGH_SQ:# if target reached target_idx += 1 if target_idx < num_targets:# still have targets remaining previous_x.append(target_x) previous_y.append(target_y) target_x += Dx target_y += Dy msg_target_position.x = target_x msg_target_position.y = target_y pub_target.publish(msg_target) # publish new target else: turnpt_idx -= 1 if turnpt_idx >= 0: update_turnpoint = True else: update_goalpoint = True ## from project 1 # generate targets with TARGET_SEPARATION # fly to CRUISE_ALTITUDE (you decide value) after takeoff # use STATE_... constants for states # --- Publish state --- msg_state.data = STATE_TURTLE pub_state.publish(msg_state) # --- Publish target --- msg_target_position.x = target_x msg_target_position.y = target_y msg_target_position.z = target_z msg_target.header.seq += 1 pub_target.publish(msg_target) # --- Timing --- et = rospy.get_time() - t t += ITERATION_PERIOD if et > ITERATION_PERIOD: print('[HEC MASTER] {} OVERSHOOT'.format(int(et*1000)))
and visual_pose_timeout != 0.0): logger.info('Visual pose data is too old') rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, visual_pose_callback) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, local_pose_callback) rospy.Subscriber('/mavros/setpoint_position/local', PoseStamped, setpoint_position_callback) rospy.Subscriber('/mavros/setpoint_raw/local', PositionTarget, setpoint_raw_callback) rospy.Subscriber('/mavros/state', State, state_callback) rospy.Subscriber('/mavros/distance_sensor/rangefinder', Range, laser_callback) emergency_pub = rospy.Publisher('/emergency', Bool, queue_size=10) rospy.Service('emergency_land', Trigger, emergency_land_service) rospy.Timer(rospy.Duration(0.5), watchdog_callback) while not rospy.is_shutdown(): emergency_msg = Bool() emergency_msg.data = emergency emergency_pub.publish(emergency_msg) rate.sleep()
def done_publish(): msg = Bool() msg.data = True done_pub.publish(msg) return
def set_gravity_compensation(self, gravity_compensation): "Turn on/off gravity compensation in cartesian effort mode" g = Bool() g.data = gravity_compensation self.__set_gravity_compensation_pub.publish(g)
def init_ros(self): # Create ROS Timer for vehicle heartbeat self.heartbeat_received = False self.last_heartbeat_time = datetime.datetime.now() self.heartbeat_timer = self.node.create_timer(1, self.heartbeat_callback) # Create QOS profiles qos_be = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) qos_klr_v = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE, durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE) qos_klr_tl = QoSProfile( history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1, reliability=QoSReliabilityPolicy. RMW_QOS_POLICY_RELIABILITY_RELIABLE, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) # Create Subscribers self.sub_heartbeat = self.node.create_subscription( msg_type=String, topic='heartbeat', callback=self.update_heartbeat, qos_profile=qos_klr_v) self.sub_camera_status = self.node.create_subscription( msg_type=String, topic='camera_status', callback=self.update_camera_status, qos_profile=qos_klr_tl) self.sub_gps_status = self.node.create_subscription( msg_type=String, topic='gps_status', callback=self.update_gps_status, qos_profile=qos_klr_tl) self.sub_storage_status = self.node.create_subscription( msg_type=String, topic='storage_status', callback=self.update_storage_status, qos_profile=qos_klr_tl) self.sub_system_time = self.node.create_subscription( msg_type=String, topic='system_time', callback=self.update_system_time, qos_profile=qos_klr_tl) self.sub_recording_status = self.node.create_subscription( msg_type=String, topic='recording_status', callback=self.update_recording_status, qos_profile=qos_klr_tl) self.sub_recorded_samples = self.node.create_subscription( msg_type=String, topic='recorded_samples', callback=self.update_recorded_samples, qos_profile=qos_klr_tl) self.sub_video_left = self.node.create_subscription( msg_type=CompressedImage, topic='camera_left/image_raw/compressed', callback=self.update_video_image, qos_profile=qos_be) # Create publisher self.pub_req_enable_recording = self.node.create_publisher( msg_type=Bool, topic='recording_enabled_target', qos_profile=qos_klr_tl) # Set initial recording enable target msg = Bool() msg.data = self.recording_enabled_target self.pub_req_enable_recording.publish(msg)
def main(): print "INITIALIZING TORSO NODE IN SIMULATION MODE BY MARCOSOFT..." ###Connection with ROS global pubGoalReached rospy.init_node("torso") br = tf.TransformBroadcaster() jointStates = JointState() jointStates.name = ["arm_lift_joint"] jointStates.position = [0.0] rospy.Subscriber("/hardware/torso/goal_pose", Float32, callbackGoalPose) rospy.Subscriber("/hardware/torso/goal_rel_pose", Float32MultiArray, callbackRelPose) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1) pubCurrentPose = rospy.Publisher("/hardware/torso/current_pose", Float32, queue_size=1) pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached", Bool, queue_size=1) loop = rospy.Rate(30) global goalSpine global spine global newGoal spine = 0.10 goalSpine = spine # goalWaist = waist # goalShoulders = shoulders speedSpine = 0.005 speedWaist = 0.1 speedShoulders = 0.1 msgCurrentPose = Float32() msgGoalReached = Bool() msgCurrentPose.data = [0, 0, 0] newGoal = False while not rospy.is_shutdown(): deltaSpine = goalSpine - spine # deltaWaist = goalWaist - waist; # deltaShoulders = goalShoulders - shoulders; if deltaSpine > speedSpine: deltaSpine = speedSpine if deltaSpine < -speedSpine: deltaSpine = -speedSpine # if deltaWaist > speedWaist: # deltaWaist = speedWaist; # if deltaWaist < -speedWaist: # deltaWaist = -speedWaist; # if deltaShoulders > speedShoulders: # deltaShoulders = speedShoulders; # if deltaShoulders < -speedShoulders: # deltaShoulders = -speedShoulders; spine += deltaSpine # waist += deltaWaist # shoulders += deltaShoulders jointStates.header.stamp = rospy.Time.now() jointStates.position = [spine] pubJointStates.publish(jointStates) msgCurrentPose.data = spine pubCurrentPose.publish(msgCurrentPose) #if newGoal and abs(goalSpine - spine) < 0.02 and abs(goalWaist - waist) < 0.05 and abs(goalShoulders - shoulders) < 0.05: if newGoal and abs(goalSpine - spine) < 0.02: newGoal = False msgGoalReached.data = True pubGoalReached.publish(msgGoalReached) loop.sleep()
def main(): print "INITIALIZING TORSO..." global simul global pubGoalReached portName1 = "/dev/justinaTorso" ###Connection with ROS rospy.init_node("torso") if rospy.has_param('~simul'): simul = rospy.get_param('~simul') else: simul = True if rospy.has_param('~port'): portName1 = rospy.get_param('~port') elif not simul: print_help() sys.exit() jointStates = JointState() jointStates.name = [ "spine_connect", "waist_connect", "shoulders_connect", "shoulders_left_connect", "shoulders_right_connect" ] jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0] pubTorsoPos = rospy.Publisher("/hardware/torso/current_pose", Float32MultiArray, queue_size=1) pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached", Bool, queue_size=1) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1) pubStop = rospy.Publisher("robot_state/stop", Empty, queue_size=1) pubEmergencyStop = rospy.Publisher("robot_state/emergency_stop", Bool, queue_size=1) subRelativeHeight = rospy.Subscriber("/hardware/torso/goal_rel_pose", Float32MultiArray, callbackRelative) subAbsoluteHeight = rospy.Subscriber("/hardware/torso/goal_pose", Float32MultiArray, callbackAbsolute) subStop = rospy.Subscriber("robot_state/stop", Empty, callbackStop) subEmergencyStop = rospy.Subscriber("robot_state/emergency_stop", Bool, callbackEmergency) subTorsoUp = rospy.Subscriber("/hardware/torso/torso_up", String, callbackTorsoUp) subTorsoDown = rospy.Subscriber("/hardware/torso/torso_down", String, callbackTorsoDown) subSimul = rospy.Subscriber("/simulated", Bool, callbackSimul) rate = rospy.Rate(ITER_RATE) global valueRel global valueAbs global absH global relH global stop global torsoUp global torsoDown global eme_stop global new_eme_msg_recv valueAbs = False valueRel = False torsoUp = False torsoDown = False torsoPos = 0 bumper = 0 stop = False msgCurrentPose = Float32MultiArray() msgGoalReached = Bool() msgCurrentPose.data = [0, 0, 0] msgMotor = None initTimeMtrMsg = datetime.now() initTimeSnrMsg = datetime.now() timeoutSnr = 0 timeoutMtr = 0 if not simul: ArdIfc = comm.Comm(portName1) msgSensor = comm.Msg(comm.ARDUINO_ID, comm.MOD_SENSORS, comm.OP_GETCURRENTDIST, [], 0) ArdIfc.send(msgSensor) goalPose = 0 new_eme_msg_recv = False eme_stop = Bool() eme_stop.data = False while not rospy.is_shutdown(): try: initTorso = torsoPos if not simul: timeoutSnr = datetime.now() - initTimeSnrMsg if timeoutSnr.microseconds > MSG_SENSOR_TIMEOUT: ArdIfc.send(msgSensor) initTimeSnrMsg = datetime.now() newMsg = ArdIfc.recv() if newMsg != None: if newMsg.mod == comm.MOD_SENSORS: if newMsg.op == comm.OP_GETCURRENTDIST: torsoPos = newMsg.param[0] #rospy.loginfo("Torso-> Arduino ack GET CURRENT DIST msg received.") if newMsg.mod == comm.MOD_SYSTEM: if newMsg.op == comm.OP_PING: rospy.loginfo( "Torso-> Arduino ack PING msg received.") if newMsg.op == comm.OP_STOP: rospy.loginfo( "Torso-> Arduino Emercenty STOP system received. " ) if eme_stop.data != bool(newMsg.param[0]): eme_stop.data = newMsg.param[0] pubEmergencyStop.publish(eme_stop) if newMsg.mod == comm.MOD_MOTORS: if newMsg.op == comm.OP_SETTORSOPOSE: msgMotor_ack_received = True rospy.loginfo( "Torso-> Arduino ack SETTORSOPOSE msg received." ) if newMsg.op == comm.OP_GOUP: msgMotor_ack_received = True rospy.loginfo( "Torso-> Arduino ack GOUP msg received.") if newMsg.op == comm.OP_GODOWN: msgMotor_ack_received = True rospy.loginfo( "Torso-> Arduino ack GODOWN msg received.") if newMsg.op == comm.OP_STOP_MOTOR: msgMotor_ack_received = True #rospy.loginfo("Torso-> Arduino ack STOP MOTOR msg received.") #until ack received timeoutMtr = datetime.now() - initTimeMtrMsg if msgMotor != None and timeoutMtr.microseconds > MSG_MOTOR_TIMEOUT and not msgMotor_ack_received: ArdIfc.send(msgMotor) initTimeMtrMsg = datetime.now() if valueAbs and not eme_stop.data and absH >= DIST_LIM_INF and absH <= DIST_LIM_SUP: msgMotor_ack_received = False msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_SETTORSOPOSE, int(absH), 1) ArdIfc.send(msgMotor) valueAbs = False goalPose = absH initTimeMtrMsg = datetime.now() elif valueRel and not eme_stop.data and ( torsoPos + relH) >= DIST_LIM_INF and ( torsoPos + relH) <= DIST_LIM_SUP: msgMotor_ack_received = False absCalH = torsoPos + relH msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_SETTORSOPOSE, int(absCalH), 1) ArdIfc.send(msgMotor) goalPose = absCalH valueRel = False initTimeMtrMsg = datetime.now() elif (valueAbs and (absH < DIST_LIM_INF or absH > DIST_LIM_SUP)) or ( valueRel and (torsoPos + relH > DIST_LIM_SUP or torsoPos + relH < DIST_LIM_INF)): rospy.logerr("Torso-> Can not reach te position.") valueAbs = False valueRel = False goalPose = torsoPos elif torsoUp and not eme_stop.data: rospy.loginfo("Torso-> Moving torso up.") msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_GOUP, [], 0) ArdIfc.send(msgMotor) torsoUp = False msgMotor_ack_received = False initTimeMtrMsg = datetime.now() goalPose = torsoPos elif torsoDown and not eme_stop.data: rospy.loginfo("Torso-> Moving torso down.") msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_GODOWN, [], 0) ArdIfc.send(msgMotor) torsoDown = False msgMotor_ack_received = False initTimeMtrMsg = datetime.now() goalPose = torsoPos elif eme_stop.data and new_eme_msg_recv: rospy.loginfo("Torso-> Stop message.") msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_STOP_MOTOR, [], 0) ArdIfc.send(msgMotor) msgMotor_ack_received = False initTimeMtrMsg = datetime.now() new_eme_msg_recv = False torsoDown = False torsoUp = False valueAbs = False valueRel = False jointStates.header.stamp = rospy.Time.now() jointStates.position = [(torsoPos - TORSO_ADJUSTMENT) / 100.0, 0.0, 0.0, 0.0, 0.0] pubJointStates.publish(jointStates) msgCurrentPose.data[0] = (torsoPos - TORSO_ADJUSTMENT) / 100.0 msgCurrentPose.data[1] = 0.0 msgCurrentPose.data[2] = 0.0 pubTorsoPos.publish(msgCurrentPose) msgGoalReached.data = abs(goalPose - torsoPos) < THR_DIFF_POS pubGoalReached.publish(msgGoalReached) else: if valueAbs and not stop: torsoPos = absH valueAbs = False elif valueRel and not stop: torsoPos = torsoPos + relH valueRel = False jointStates.header.stamp = rospy.Time.now() jointStates.position = [(torsoPos * 0.352) / 169733, 0.0, 0.0, 0.0, 0.0] pubJointStates.publish(jointStates) msgCurrentPose.data[0] = ( torsoPos * 0.352) / 169733 #-------------------pulsos a metros msgCurrentPose.data[1] = 0.0 msgCurrentPose.data[2] = 0.0 pubTorsoPos.publish(msgCurrentPose) msgGoalReached.data = abs(initTorso - torsoPos) < 200 pubGoalReached.publish(msgGoalReached) rate.sleep() except: rospy.logerr( "Torso-> Oopps...we have some issues in this node :( ")
def gripperClose(self, bool): msg = Bool() msg.data = bool self.gripper_pub.publish(msg)
def dron_sate(msg): armed = Bool() armed.data = msg.armed armed_pub.publish(armed)
def publish_hotspot_state(self, activated): msg = Bool() msg.data = activated self.hotspot_state_publisher.publish(msg)
def calibrated_cb(self, req): r = Bool() r.data = True self.calibrated_pub.publish(r) return EmptyResponse()
except Exception as e: print("second load marge error:", e.args) if "recipe" in Config: srcpath = re.subn(r".*?/", "/", thispath[::-1], 1)[0][::-1] dirpath = srcpath + Config["recipe"]["dir"] linkpath = srcpath + Config["recipe"]["link"] print("dirpath", dirpath) print("linkpath", linkpath) try: dictlib.merge(Config, rospy.get_param("/config/dashboard")) except Exception as e: print("get_param exception:", e.args) ####Bools mTrue = Bool() mTrue.data = True mFalse = Bool() ####sub pub rospy.Subscriber("~load", String, cb_load) rospy.Subscriber("/message", String, functools.partial(cb_mbox_push, 0)) rospy.Subscriber("/error", String, functools.partial(cb_mbox_push, 2)) rospy.Subscriber("/shutdown", Bool, cb_shutdown) pub_Y3 = rospy.Publisher("~loaded", Bool, queue_size=1) pub_E3 = rospy.Publisher("~failed", Bool, queue_size=1) pub_msg = rospy.Publisher("/message", String, queue_size=1) pub_redraw = rospy.Publisher("/request/redraw", Bool, queue_size=1) ####Layout#### normalfont = (Config["font"]["family"], Config["font"]["size"], "normal") boldfont = (Config["font"]["family"], Config["font"]["size"], "bold")
def cb_notif(event): ret = Bool() ret.data = True pub_Y2.publish(ret)
def stopMove(self,flag): mes = Bool() mes.data = flag for _ in range(15): self.stop.publish(mes)
def report_readiness(self, ready_bool): msg = Bool() msg.data = ready_bool self.controller_ready_pub.publish(msg)
def navigation(self): pose = Pose() bool = Bool() # STATE 1: GO TO THE OTHER SIDE OF THE COURT BYPASSING THE NET #------------------------------------------------------------------- if self.state == 1: if self.is_ball_on_court == True: if np.sign(self.pose_rob.y) != np.sign(self.pose_ball_y): self.x_wp = 6.5*np.sign(self.poserob.x) self.y_wp = 0.0 else: self.state = 2 else : self.state = 3 #------------------------------------------------------------------- # STATE 2: GO THE NEXT BALL TO COLLECT IT #------------------------------------------------------------------- if self.state == 2: if self.is_ball_on_court == True: if np.sign(self.pose_rob.y) != np.sign(self.pose_ball_y): state = 1 else: self.x_wp = self.pose_ball_x self.y_wp = self.pose_ball_y else: self.state = 3 #------------------------------------------------------------------- # STATE 3: BRING THE BALL INTO THE COLLECTING AREAS #------------------------------------------------------------------- if self.state == 3: if self.is_ball_collected == True: self.x_wp = 7.0*np.sign(self.pose_rob.y) self.y_wp = 14.0*np.sign(self.pose_rob.y) else: self.state = 2 #------------------------------------------------------------------- # STATE 0: IDLE #------------------------------------------------------------------- else: # state == 0 if self.is_ball_on_court == True: self.state = 2 self.move == True else: if 6.0 < abs(self.pose_rob.x) < 7.0 and abs(self.pose_rob.y) < .5: # stop robot self.move = False else: self.state = 1 self.move = True #------------------------------------------------------------------- # PUBLISHING WAYPONT (POSE) AND MOVING COMMAND (BOOL) #------------------------------------------------------------------- pose.pose.position.x = self.x_wp pose.pose.position.y = self.y_wp self.wp_pub.publish(pose) self.get_logger().info("Publishing: {}, {}, {}".format(pose.x, pose.y)) bool.data = self.move self.move_pub.publish(bool)
def path_follow(pose_msg_in): # First assign the incoming message global estimated_pose, path_segment_spec estimated_pose = pose_msg_in pose_xy = np.array([estimated_pose.x, estimated_pose.y]) pose_theta = np.array([estimated_pose.theta]) # Set up global variables global initialize_psi_world, estimated_pose_previous, estimated_x_local_previous, estimated_theta_local_previous, path_segment_curvature_previous, estimated_psi_world_previous, estimated_segment_completion_fraction_previous # Create a vector variable for the Origin of the path segment path_segment_Origin = np.array( [path_segment_spec.x0, path_segment_spec.y0]) # Forward distance relative to a Line path is computed along the Forward axis. # Therefore Define the Forward Axis: path_segment_y0_vector = np.array( [-np.sin(path_segment_spec.theta0), np.cos(path_segment_spec.theta0)]) # local X is computed perpendicular to the segment. # Therefore define the Perpendicular axis. path_segment_x0_vector = np.array([ -np.sin(path_segment_spec.theta0 - np.pi / 2), np.cos(path_segment_spec.theta0 - np.pi / 2) ]) # Define path curvature path_segment_curvature = 1 / path_segment_spec.Radius # ============================================================================= # ### First Compute the robot's position relative to the path (x, y, theta) # ### and the local path properties (curvature 1/R and segment completion percentage) # ============================================================================= # Line (Infinite radius) if np.isinf(path_segment_spec.Radius): path_segment_endpoint = path_segment_Origin + path_segment_spec.Length * path_segment_y0_vector # compute position relative to Segment: estimated_xy_rel_to_segment_origin = ( pose_xy - path_segment_Origin ) # XY vector from Origin of segment to current location of robot. # Projection of vector from path origin to current position estimated_segment_forward_pos = estimated_xy_rel_to_segment_origin.dot( path_segment_y0_vector) # The fraction completion can be estimated as the path length the robot has gone through, as a fraction of total path length on this segment estimated_segment_completion_fraction = estimated_segment_forward_pos / path_segment_spec.Length # Position of the robot to the Right of the segment Origin estimated_segment_rightward_pos = estimated_xy_rel_to_segment_origin.dot( path_segment_x0_vector) estimated_y_local = 0 # y_local = 0 by definition: Local coords are defined relative to the closest point on the path. estimated_x_local = estimated_segment_rightward_pos estimated_theta_local = pose_theta - path_segment_spec.theta0 # Arc else: curve_sign = np.sign(path_segment_spec.Radius) path_segment_circle_center = path_segment_Origin + path_segment_spec.Radius * -path_segment_x0_vector # determine the angular displacement of this arc. SIGNED quantity! path_segment_angular_displacement = path_segment_spec.Length / path_segment_spec.Radius path_segment_ThetaEnd = path_segment_spec.theta0 + path_segment_angular_displacement estimated_xy_rel_to_circle_center = (pose_xy - path_segment_circle_center) # Compute angle of a vector from circle center to Robot, in the world frame, relative to the +Yworld axis. # Note how this definition affects the signs of the arguments to "arctan2" estimated_psi_world = np.arctan2(-estimated_xy_rel_to_circle_center[0], estimated_xy_rel_to_circle_center[1]) # unwrap the angular displacement if initialize_psi_world: estimated_psi_world_previous = estimated_psi_world initialize_psi_world = False while estimated_psi_world - estimated_psi_world_previous > np.pi: # was negative, is now positive --> should be more negative. estimated_psi_world += -2 * np.pi while estimated_psi_world - estimated_psi_world_previous < -np.pi: # was positive and is now negative --> should be more positive. estimated_psi_world += 2 * np.pi # update the "previous angle" memory. estimated_psi_world_previous = estimated_psi_world # The local path forward direction is perpendicular (clockwise) to this World frame origin-to-robot angle. estimated_path_theta = estimated_psi_world + np.pi / 2 * curve_sign # The fraction completion can be estimated as the path angle the robot has gone through, as a fraction of total angular displacement on this segment estimated_segment_completion_fraction = ( estimated_path_theta - path_segment_spec.theta0) / path_segment_angular_displacement estimated_y_local = 0 # by definition of local coords # x_local is positive Inside the circle for Right turns, and Outside the circle for Left turns estimated_x_local = curve_sign * ( np.sqrt(np.sum(np.square(estimated_xy_rel_to_circle_center))) - np.abs(path_segment_spec.Radius)) estimated_theta_local = pose_theta - estimated_path_theta ## Whether Line or Arc, update the "local" coordinate state and path properties: estimated_theta_local = m439rbt.fix_angle_pi_to_neg_pi( estimated_theta_local) # Update the "previous" values estimated_pose_previous = estimated_pose estimated_x_local_previous = estimated_x_local estimated_theta_local_previous = estimated_theta_local path_segment_curvature_previous = path_segment_curvature estimated_segment_completion_fraction_previous = estimated_segment_completion_fraction # ============================================================================= # ### CONTROLLER for path tracking based on local position and curvature. # # parameters for the controller are # # Vmax: Maximum allowable speed, # # and controller gains: # # Beta (gain on lateral error, mapping to lateral speed) # # gamma (gain on heading error, mapping to rotational speed) # # and a control variable for the precision of turns, # # angle_focus_factor # ============================================================================= global Vmax, Beta, gamma, angle_focus_factor # First set the speed with which we want the robot to approach the path xdot_local_desired = -Beta * estimated_x_local # limit it to +-Vmax xdot_local_desired = np.min([np.abs(xdot_local_desired), abs(Vmax)]) * np.sign(xdot_local_desired) # Next set the desired theta_local theta_local_desired = -np.arcsin(xdot_local_desired / Vmax) ## Next SET SPEED OF ROBOT CENTER. ## G. Cook 2011 says just use constant speed all the time, Vc = Vmax ## But, that drives farther from the path at first if it is facing away. ## This FIX causes the speed to fall to zero if the robot is more than 90 deg from the heading we want it to have. ## The parameter "angle_focus_factor" can make it even more restrictive if needed (e.g. angle_focus_factor = 2 --> 45 deg limit). ## Value of 1.0 uses a straight cosine of the angle. Vc = Vmax * np.cos(angle_focus_factor * m439rbt.fix_angle_pi_to_neg_pi(theta_local_desired - estimated_theta_local)) ## Could also limit it to only forward. Try with and without this! Vc = np.max([Vc, 0]) # Finally set the desired angular rate omega = gamma * m439rbt.fix_angle_pi_to_neg_pi( theta_local_desired - estimated_theta_local) + Vc * ( path_segment_curvature / (1 + path_segment_curvature * estimated_x_local) ) * np.cos(estimated_theta_local) # Finally, use the "robot" object to translate these into wheel speeds global robot robot.set_wheel_speeds_from_robot_velocities(Vc, omega) # Check if the path is complete # If so, stop! global path_is_complete if path_is_complete: robot.set_wheel_speeds(0., 0.) # ... and Publish it global pub_speeds # Set up the message that will go on that topic. msg_speeds = ME439WheelSpeeds() msg_speeds.v_left = robot.left_wheel_speed msg_speeds.v_right = robot.right_wheel_speed pub_speeds.publish(msg_speeds) print(estimated_segment_completion_fraction) # Finally, if the segment is complete, publish that fact if estimated_segment_completion_fraction >= 1.0: global pub_segment_complete msg_seg_complete = Bool() msg_seg_complete.data = True pub_segment_complete.publish(msg_seg_complete) # and clear the memory of angle wrapping: initialize_psi_world = True
# reinsforcement learning new_zone, new_ray, new_angle = zone_boat( zone_to_stay, psi, np.array([pos_x, pos_y])) if new_zone != prev_zone: #Chose an action thetabar, act = chose_action(epsilon, new_ray, new_angle, Q_table) # Update the Q-table reward = rewards(prev_zone, new_zone, act) Q_table[prev_angle, prev_ray, act] = Q_table[ prev_angle, prev_ray, act] + learning_rate * ( (reward + actualization_rate * (np.argmax(Q_table[new_angle, new_ray, :]))) - Q_table[prev_angle, prev_ray, act]) u = cl.control_station_keeping(thetabar, fut_x, psi) # rospy.loginfo(" Zone {}, ray de {}, angle de {}".format(new_zone,new_ray,new_angle)) prev_zone, prev_ray, prev_angle = new_zone, new_ray, new_angle u_rudder_msg.data = u[0, 0] u_sail_msg.data = u[1, 0] pub_send_u_rudder.publish(u_rudder_msg) pub_send_u_sail.publish(u_sail_msg) pub_send_reset.publish(reset_msg) if reset_msg.data == True: reset_msg.data = False rate.sleep() nbr_ite += 1 rospy.loginfo("Iteration nbr {}/{}".format(nbr_ite, nbr_episodes)) save_data(Q_table) rospy.loginfo("End of the training")
def close(self): message = Bool() message.data = True self.RS.publish(message) time.sleep(self.main_state.configs["RS_WAIT"]) super().close()
pub_Y2 = rospy.Publisher("~solved", Bool, queue_size=1) pub_busy = rospy.Publisher("~stat", Bool, queue_size=1) pub_saved = rospy.Publisher("~saved", Bool, queue_size=1) pub_loaded = rospy.Publisher("~loaded", Bool, queue_size=1) pub_score = rospy.Publisher("~score", Float32MultiArray, queue_size=1) rospy.Subscriber("~clear", Bool, cb_clear) rospy.Subscriber("~solve", Bool, cb_solve) rospy.Subscriber("~save", Bool, cb_save) rospy.Subscriber("~load", Bool, cb_load) rospy.Subscriber("~redraw", Bool, cb_master) pub_msg = rospy.Publisher("/message", String, queue_size=1) pub_err = rospy.Publisher("/error", String, queue_size=1) ###std_msgs/Bool mTrue = Bool() mTrue.data = True mFalse = Bool() mFalse.data = False ###TF tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) broadcaster = tf2_ros.StaticTransformBroadcaster() ###data Scene = [None] * len(Config["scenes"]) Model = [None] * len(Config["scenes"]) tfReg = [] tfSolve = [] rospy.Timer(rospy.Duration(5), cb_load, oneshot=True)
cro = Bool() cir = Bool() tri = Bool() l1 = Bool() r1 = Bool() l3 = Bool() r3 = Bool() opt = Bool() ps = Bool() pad = Bool() keypad = Twist() keypad.linear.x = 0 keypad.linear.y = 0 squ.data = False cro.data = False cir.data = False tri.data = False l1.data = False r1.data = False l3.data = False r3.data = False opt.data = False ps.data = False pad.data = False reverse_fac = 1.0 speeding_fac = 1.0 left_mode = 1.0
def end_experiment_pub(self, flag): msg = Bool() msg.data = flag self.pub_end_experiment.publish(msg)
#!/usr/bin/env python import rospy from std_msgs.msg import String from geometry_msgs.msg import Twist from drive_by_wire.msg import Cart_values from std_msgs.msg import Bool # import keyboard pub = rospy.Publisher('manual', Bool, queue_size=10) rospy.init_node('keyboard_input', anonymous=True) rate = rospy.Rate(30) # 10hz val = Bool() val.data = False def keyboard_input(): global val while not rospy.is_shutdown(): text = raw_input() # if 'w' in text: # if vel_msg.linear.x < vel_max_linear: # vel_msg.linear.x += vel_linear_inc # if 's' in text: # if vel_msg.linear.x > -1*vel_max_linear: # vel_msg.linear.x -= vel_linear_inc # if 'd' in text: # if vel_msg.angular.z > -1*vel_max_angular: # vel_msg.angular.z -= vel_angular_inc # if 'a' in text: # if vel_msg.angular.z < vel_max_angular:
def __init__(self): # Init attributes self.default_speed = 0.5 self.speed = self.default_speed self.steering_angle = 0 self.robot_length = 0.22 self.robot_pose = None#(-0.3, -0.1789, -0.0246) self.destination_pose = None self.stop_point = None self.waypoints = rospy.get_param('~path') #self.waypoints = [(0, 0),(2,2),(-1,1),(-3,3),(-3,0),(1,-2),(0,0)] self.current_waypoint_index = 0 self.distance_from_path = None self.lookahead_distance = rospy.get_param("~lookahead") #self.lookahead_distance_adjust = self.lookahead_distance self.threshold_proximity = 0.2 # How close the robot needs to be to the final waypoint to stop driving self.active = True self.start = True # Init subscribers and publishers self.pub_gazebo = rospy.Publisher('/cmd_vel', Twist, queue_size=1) self.pub_lookahead = rospy.Publisher('/pure_pursuit/lookahead', Point, queue_size=1) self.pub_finish = rospy.Publisher('/pure_pursuit/finished', Bool, queue_size=1) while True: rospy.wait_for_service('/gazebo/get_model_state') try: get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) model_state = get_model_state('duckiebot',"") msg = model_state #print msg except rospy.ServiceException, e: pass if not self.active: #break return finish = Bool() finish.data = False self.pub_finish.publish(finish) quaternion_msg = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w] euler = tf.transformations.euler_from_quaternion(quaternion_msg) #print euler[2] #print self.robot_pose = (msg.pose.position.x, msg.pose.position.y, euler[2]) # print self.robot_pose #print 'robot_pose:', self.robot_pose#, 'waypoints:', self.waypoints self.destination_pose = self.pure_pursuit() if self.destination_pose == None: self.active = False print "approach destination " self.gazebo_cmd(0,0) msg = Bool() msg.data = True self.pub_finish.publish(msg) else: self.publish_lookhead(self.destination_pose) self.speed = self.default_speed distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose) angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose) # self.steering_angle = np.arctan((2 * self.robot_length * np.sin(angle_to_destination)) / distance_to_destination) # print "robot_head",euler[2]*180/math.pi,"angle_to_destination", angle_to_destination*180/math.pi #print angle_to_destination w = 3*((angle_to_destination + math.pi) / (2 * math.pi) - 0.5) '''if w > 0: w = 1 else: w = -1''' #print angle_to_destination self.gazebo_cmd(self.speed,w)
cmd.acceleration.x = 0 cmd.acceleration.y = 0 cmd.acceleration.z = 0 cmd.jerk.x = 0 cmd.jerk.y = 0 cmd.jerk.z = 0 cmd.yaw = 0 cmd.yaw_dot = 0 cmd.kx = [1, 1, 1] cmd.kv = [.5, .5, .5] return cmd if __name__ == "__main__": rospy.init_node('hover_cmd') pub = rospy.Publisher('position_cmd', PositionCommand, queue_size=10) rate = rospy.Rate(20) # 10hz motorPub = rospy.Publisher('motors', Bool, queue_size=10) enable = Bool() enable.data = True rospy.sleep(1) motorPub.publish(enable) while not rospy.is_shutdown(): pub.publish(getCommand()) rate.sleep()
# when the python interpreter runs this script # this is the section that gets run as main if __name__ == '__main__': # let the master know about our name rospy.init_node("riss") # define and fill out some messages publisherDataType = rospy.get_param("~publisher_data_type") waitDuration = rospy.get_param("~wait_duration") # define a duration of 1 second myDuration = rospy.Duration(waitDuration, 0) if (publisherDataType == "bool"): msg = Bool() msg.data = 0 myPublisher = rospy.Publisher('~my_ros_topic', Bool, queue_size=5) elif (publisherDataType == "uint16"): msg = UInt16() msg.data = 10 myPublisher = rospy.Publisher('~my_ros_topic', UInt16, queue_size=5) else: msg = String() msg.data = "publisher data type was not bool or uint16." myPublisher = rospy.Publisher('~my_ros_topic', String, queue_size=5) # wait for Ctrl-C and loop until then while not rospy.is_shutdown(): # print a message for feedback print "looping..."
def send_exe_camera(self, BOOL): exe_msg = Bool() exe_msg.data = BOOL self.pub_camera.publish(exe_msg)