def __init__(self): rospy.init_node('Robot Tracking') # TCP_IP = rospy.get_param('~IP', 'localhost') # TCP_PORT = rospy.get_param('~PORT', 6666) # self.BUFFER_SIZE = 1024 # self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # self.s.connect((TCP_IP, TCP_PORT)) self.validId = Int8MultiArray() self.f = open( '/home/abhay/ROS_Projects/test/src/testpkg/scripts/data.txt', 'r') # Subsciber_generator.subscribe('/CT0/validity', Int8MultiArray, self.ct0_valid_callback) rospy.Subscriber('/CT0/validity', Int8MultiArray, self.ct0_valid_callback) self.location_pub = rospy.Publisher('~poses', String, queue_size=10) self.valid_id_pub = rospy.Publisher('~validity', Int8MultiArray, queue_size=10) self.rate = rospy.get_param('~rate', 10) self.test = Int8MultiArray() self.validity_ct0 = [] self.valid_id_count_ct0 = 0
def publish_arming_Message(self): if (self.arming_flag): vehicle_status_array = Int8MultiArray() vehicle_status_array.data = [1] self.confirm_flag_pub.publish(vehicle_status_array) else: vehicle_status_array = Int8MultiArray() vehicle_status_array.data = [0] self.confirm_flag_pub.publish(vehicle_status_array)
def main(): game_pad = GamePad(0) game_pad.button_mode_[0] = 1 rospy.init_node('GamePad', anonymous=True, disable_signals=True) pub = rospy.Publisher('GamePad', Int8MultiArray, queue_size=30) rospy.on_shutdown(shutdown) rate = rospy.Rate(5) # button_frag = False while True: game_pad.Update() # if game_pad.buttons_[0] == True: # button_frag = not button_frag game_pad_state = Int8MultiArray() game_pad_state.data = np.zeros(len(game_pad.buttons_)) print game_pad_state.data # game_pad_state.data = [int(button_frag)] + game_pad.axes_[:3] + list(game_pad.hats_[0]) # print game_pad_state.data for i in range(len(game_pad.buttons_)): game_pad_state.data[i] = int(game_pad.buttons_[i]) print game_pad_state pub.publish(game_pad_state) rate.sleep() rospy.spin()
def image_process(self): print("Processing Image") # Resize the photo and apply Canny Edge Detector # Generate edgemap and send to display edges = self.resize_edge(self.face, 80) # Save locally cv2.imwrite("/home/ethan/Pictures/face.jpg", self.face) cv2.imwrite("/home/ethan/Pictures/edge.jpg", edges) # Resize the edgemap image edges_resize = self.resize(edges, 600) edges_rgb = cv2.cvtColor(edges_resize, cv2.COLOR_GRAY2RGB) # Add Jarvis as background self.jarvis[0:edges_rgb.shape[0], 0:edges_rgb.shape[1]] = edges_rgb edges_ros = self.bridge.cv2_to_imgmsg(self.jarvis, encoding="passthrough") self.image_pub.publish(edges_ros) print("Sending edges to display") # Apply DFS to transfer binary image to trajectory points pathlist = getPointsFromEdges(edges) # (-1,-1) to lift up the end effector pathlist.append((-1, -1)) print("Path length: ", len(pathlist)) # Publish to drawing node path = Int8MultiArray() pathlist = np.array(pathlist).reshape(2 * len(pathlist)) path.data = pathlist self.trajectory_pub.publish(path) print("Publishing Trajectory")
def player(): pub = rospy.Publisher('mecanum_motors', Int8MultiArray, queue_size=10) rospy.init_node('player', anonymous=True) r = rospy.Rate(10) # 10hz loadFile(fileName) global i global lines # print lines print len(lines) print i while not rospy.is_shutdown(): if lines: if i < len(lines): print 'odom_run: ', line = lines[i] print line arr = [] for j in range(0, 4): arr.append(int(line[j])) pub.publish(Int8MultiArray(data=arr)) i += 1 elif i == len(lines): #finish task_pub.publish(Int8(Tasks.ODOMRUN_END)) i += 1 else: pass #str = "hello world %s"%rospy.get_time() #rospy.loginfo(str) #pub.publish(str) r.sleep()
def pubby(outlist): # while not rospy.is_shutdown(): stripe_obs_data = Int8MultiArray() #Need Function that runs algorithm and returns outlist stripe_obs_data.data = outlist #rospy.loginfo(stripe_obs_data) pub.publish(stripe_obs_data)
def _record_demonstration(self): observations = [] while True: robot = self.environment.robot if robot._gripper: if robot._cuff.upper_button(): robot._gripper.open() elif robot._cuff.lower_button(): robot._gripper.close() data = { "time": self._time_stamp(), "robot": self.environment.get_robot_state(), "items": self.environment.get_item_state(), "triggered_constraints": self.environment.check_constraint_triggers() } observation = Observation(data) if self.publish_constraint_validity: valid_constraints = check_constraint_validity(self.environment, self.environment.constraints, observation)[1] pub = rospy.Publisher('cairo_lfd/valid_constraints', Int8MultiArray, queue_size=10) msg = Int8MultiArray(data=valid_constraints) pub.publish(msg) observations.append(observation) if self.command == "discard": rospy.loginfo("~~~DISCARDED~~~") self.interaction_publisher.send_position_mode_cmd() self._clear_command() return [] if self.command == "capture": rospy.loginfo("~~~CAPTURED~~~") self.interaction_publisher.send_position_mode_cmd() self._clear_command() rospy.loginfo("{} observations captured.".format(len(observations))) return observations self._rate.sleep()
def __init__(self): self._nodeCost = rclpy.create_node('costmap') self.pubCost = self._nodeCost.create_publisher(Int8MultiArray, '/yf_camera/costmap', 1) self.pubCost # prevent unused variable warning self._cost = Int8MultiArray()
def somethingCool(): global mapdata mapdata = Int8MultiArray() rospy.init_node('test_name', anonymous=False) rospy.Subscriber("map", OccupancyGrid, callback) rospy.loginfo(mapdata) rospy.spin()
def talker(): data_list = Int8MultiArray() pub = rospy.Publisher('Testmessage', Int8MultiArray, queue_size=1) rospy.init_node('talker', anonymous=True) while not rospy.is_shutdown(): data_list.data = [50, 50] pub.publish(data_list) time.sleep(1)
def callback(self, heat_array): coordinates = get_bright_loc(heat_array.data) self.get_logger().info(f'Coordinates: X:{coordinates[0]}, Y:{coordinates[1]}') comm_array = Int8MultiArray() comm_array.data = command(coordinates) self.publisher_.publish(comm_array) self.get_logger().info( f'Direction: X:{comm_array.data[0]}, Y:{comm_array.data[1]}, Active:{comm_array.data[2]}')
def talker(): data_list = Int8MultiArray() pub = rospy.Publisher('contl_data', Int8MultiArray, queue_size=2) rospy.init_node('talker', anonymous=True) while not rospy.is_shutdown(): data_list.data = [5, 5] pub.publish(data_list) time.sleep(1)
def pubLabelMsg(self, known_labels): msg = Int8MultiArray() dim = MultiArrayDimension() dim.label = "length" dim.size = len(known_labels) dim.stride = len(known_labels) msg.layout.dim.append(dim) msg.layout.data_offset = 0 msg.data = known_labels self.labels_pub.publish(msg)
def on_move_emitted(client, userdata, message): UNUSED(client) UNUSED(userdata) msg = str(message.payload.decode("utf-8")) values = np.array(msg.split(','), dtype=np.int8) assert values.shape == (3, ) state = Int8MultiArray() state.data = list(values) MOVE_ROS_PUBLISHER.publish(state) print('MOVE with shape {} has values: {}'.format(values.shape, msg))
def on_board_state_changed(client, userdata, message): UNUSED(client) UNUSED(userdata) msg = str(message.payload.decode("utf-8")) values = np.array(msg.split(','), dtype=np.int8) print('RECEIVED: ', msg) assert values.shape == (9, ) state = Int8MultiArray() state.data = list(values) BOARD_STATE_ROS_PUBLISHER.publish(state) print('BOARD STATE with shape {} has values: {}'.format(values.shape, msg))
def listener(): pub = rospy.Publisher('contl_data',Int8MultiArray,queue_size=1) rospy.init_node('listener', anonymous=True) data_list = Int8MultiArray() global stealing global throttle while not rospy.is_shutdown(): data_list.data = [int(throttle),int(stealing)] print(data_list.data) pub.publish(data_list) time.sleep(0.01) rospy.Subscriber("joy",Joy,callback)
def talker(): ############################################# # pub=rospy.Publisher('/servo',UInt16, queue_size=10) pub2=rospy.Publisher('/servo2',UInt16, queue_size=10) # values between 30 and 150 # 30 50 70 90 110 130 150 selected_column=2 arm_position = UInt16() arm_position.data = 30+20*selected_column pub.publish(arm_position) open_hand = UInt16() open_hand.data= 50 pub2.publish(open_hand) ############################################# pub = rospy.Publisher('new_action', Int8MultiArray, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(0.2) # publish every 1/rate seconds while not rospy.is_shutdown(): new_action = Int8MultiArray() new_action.layout.dim = [] new_action.data=[1,1,2,3] # 1st cell: 0 -> cv; 1 -> talk; 2 -> logic # 2nd cell: 1 -> player; 2 -> Roboy # 3rd cell: row-coordinate # 4th cell: column-coordinate pub.publish(new_action) rate.sleep()
def talker(size): '''This is the main function of the whole script. Here we set up all of publishers and subscribers. In short, subscribe to the map and attach it to genTunnel (via callback()). explorer is updated in the process and we can then call the navigation engine functions on it. A path is returned and the next waypoint is published on the /next_wp topic''' global pubway, rtl_pub mapdata = Int8MultiArray() rospy.init_node('talker', anonymous=True) pubway = rospy.Publisher('next_wps', Waypoints, queue_size=10) rtl_pub = rospy.Publisher('return_to_launch', Bool, queue_size=10) rtl_pub.publish(False) rospy.Subscriber("nav_map", OccupancyGrid, occ_grid_cb) rospy.spin()
def main(self): rate = rospy.Rate(40) # Hz while not rospy.is_shutdown(): #if self.state is not None: # msg = Bool() # msg.data=self.state # self.publisher.publish(msg) msg = Int8MultiArray() msg.data = [ self.state[0], self.state[1], self.state[2], self.state[3] ] #,self.state[4],self.state[5]] self.publisher.publish(msg) rate.sleep()
def joy_cb(msg): throttle = -1 if msg.axes[axes['LY']] > 0: throttle += msg.axes[axes['LY']] * 2 rudder = msg.axes[axes['LX']] elevator = msg.axes[axes['RY']] aileron = msg.axes[axes['RX']] pub_msg = Int8MultiArray() factor = 127 pub_msg.data.append(throttle * factor) pub_msg.data.append(rudder * factor) pub_msg.data.append(elevator * factor) pub_msg.data.append(aileron * factor) pub_msg.data.append(-aileron * factor) rc_plane_cmd_Publisher.publish(pub_msg)
def callback(self, data, joy_data): global stealing global throttle global flag data_list = Int8MultiArray() if (data.buttons[3] == 1): #start=>linetrace flag = 1 elif (data.buttons[0] == 1): #select=>ps3control flag = 0 if (flag == 0): data_ste = [round((data.axes[0] - 1) * -50, 1)] #round is 四捨五入 stealing = np.clip(data_ste, 35, 65)[0] data_thr = [round((data.axes[1] - 1) * -50, 1)] throttle = np.clip(data_thr, 35, 65)[0] elif (flag == 1): throttle = 45 try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) gray_image = cv2.cvtColor(cv_image[105:119, 0:159], cv2.COLOR_BGR2GRAY) ret, cv_image2 = cv2.threshold(gray_image, 150, 255, cv2.THRESH_BINARY_INV) thresh = 210 max_pixel = 255 ret, img_dst = cv2.threshold(gray_image, thresh, max_pixel, cv2.THRESH_BINARY) img_binary = img_dst / 255 line_data = np.sum(img_binary, axis=0) line_data_left = line_data[0:((line_data.shape[0] / 2) - 1)].sum() line_data_right = line_data[(line_data.shape[0] / 2):(line_data.shape[0] - 1)].sum() cont_data = [float(line_data_left) - float(line_data_right)] cont_data = np.clip(cont_data, -50, 50)[0] cont_data = (cont_data - 50) * -1 print(cont_data) stealing = cont_data cv2.imshow("img_dst", img_dst) cv2.waitKey(2) data_list.data = [int(throttle), int(stealing)] self.image_pub.publish(data_list)
def parking_possiblity( ros_data ): ### using web_cam, check white_blob and line and enough distance, if all exist, it send Availablity global white_detected global park_enable global d if stage is 1: np_arr = np.fromstring(ros_data.data, np.uint8) ### image process frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) mask_white = cv2.inRange(hsv, lower_white, upper_white) res1 = cv2.bitwise_and(frame, frame, mask=mask_white) line = turtle_video_siljun.find_line( frame) ### Checking if there are lines keypoints = turtle_video_siljun.find_white( frame, lower_white, upper_white) ### Checking if there are white if line > 0 and keypoints: white_detected = 1 ### check white line else: white_detected = 0 n = 1 for i in d: if i > 0.12 and i < 0.6: n = 0 ### check parking space if n == 0: park_enable = 0 else: park_enable = 1 cv2.imshow('frac', frame) ### show processd image cv2.imshow('white', res1) cv2.waitKey(1) & 0xFF print(white_detected) state = Int8MultiArray() state.data = [white_detected, park_enable] pub1.publish(state) ### pub white_detected and park_enable to 'main'
def talker(size): '''This is the main function of the whole script. Here we set up all of publishers and subscribers. In short, subscribe to the map and attach it to genTunnel (via callback()). explorer is updated in the process and we can then call the navigation engine functions on it. A path is returned and the next waypoint is published on the /next_wp topic''' global pubway mapdata = Int8MultiArray() rospy.init_node('talker', anonymous=True) pub = rospy.Publisher('mapprob', Int8MultiArray, queue_size=10) pubway = rospy.Publisher('next_wps', Waypoints, queue_size=10) rospy.Subscriber("nav_map", OccupancyGrid, occ_grid_cb) rospy.spin() # time.sleep(10) #shortest distance to consider and maximum range to use for navigation #these work for a 30 x 30 but for a larger obstacle map may need to be #larger. Some experimentation will be needed to find a good balance of #performance # shortestdistance = 3 # maxrange = 40 #get first move # p = explorer.findClosestFrontier(size/2,size/2,maxrange,shortestdistance) # last = False #while we still have moves and have not returned to home base navigate #around # while p is not None: # for pp in p: # #jva.awt.point uses floats so we cast back to integers here # way = Point() # x = int(pp.getX()) # y = int(pp.getY()) # way.x = x # way.y = y # n = explorer.getNode(x,y) # while not rospy.is_shutdown(): # pubway.publish(way) #x_path,y_path = zip(*[(pp.getX(),pp,getY()) for pp in p]) '''ways = PoseArray()
def callback(self, data): global stealing global throttle data_list = Int8MultiArray() try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) #RGB表色系からHSV表色系に変換 #hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) #hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) #color_min = np.array([150,100,150]) #color_max = np.array([180,255,255]) #color_mask = cv2.inRange(hsv_image, color_min, color_max) #cv_image2 = cv2.bitwise_and(cv_image, cv_image, mask = color_mask) gray_image = cv2.cvtColor(cv_image[105:119, 0:159], cv2.COLOR_BGR2GRAY) #histr = cv2.calcHist([gray_image],[0],None,[256],[0,256]) #plt.plot(histr,color = 'k') #plt.show() ret, cv_image2 = cv2.threshold(gray_image, 150, 255, cv2.THRESH_BINARY_INV) thresh = 210 max_pixel = 255 ret, img_dst = cv2.threshold(gray_image, thresh, max_pixel, cv2.THRESH_BINARY) img_binary = img_dst / 255 line_data = np.sum(img_binary, axis=0) line_data_left = line_data[0:((line_data.shape[0] / 2) - 1)].sum() line_data_right = line_data[(line_data.shape[0] / 2):(line_data.shape[0] - 1)].sum() #print("left :{} right:{}".format(line_data_left,line_data_right)) cont_data = [float(line_data_left) - float(line_data_right)] #print(cont_data) cont_data = np.clip(cont_data, -50, 50)[0] cont_data = (cont_data - 50) * -1 print(cont_data) stealing = cont_data data_list.data = [int(throttle), int(stealing)] self.image_pub.publish(data_list) cv2.imshow("img_dst", img_dst) cv2.waitKey(2)
def talker(): pub = rospy.Publisher('new_action', Int8MultiArray, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(0.2) # publish every 1/rate seconds while not rospy.is_shutdown(): new_action = Int8MultiArray() new_action.layout.dim = [] new_action.data = [1, 1, 2, 3] # 1st cell: 0 -> cv; 1 -> talk; 2 -> logic # 2nd cell: 1 -> player; 2 -> Roboy # 3rd cell: row-coordinate # 4th cell: column-coordinate pub.publish(new_action) rate.sleep()
def update_temperature(self): t0 = time.time() temperatures = [] for i in [ (self.thermometer_range_m, 0), (0, -self.thermometer_range_m), (-self.thermometer_range_m, 0), (0, self.thermometer_range_m) ]: robot_angle = getHeading(self.pose.orientation) sensor_pos_x = self.pose.position.x + i[0] * math.cos( robot_angle) - i[1] * math.sin(robot_angle) sensor_pos_y = self.pose.position.y + i[1] * math.cos( robot_angle) + i[0] * math.sin(robot_angle) pixel_nb = self.converter.RealToOccupancyGrid( sensor_pos_x, sensor_pos_y) temperatures.append(self.firemap.data[pixel_nb]) array = Int8MultiArray() array.data = temperatures self.pub.publish(array) print(temperatures)
def shinho(blob_ROI, stage, angular): ###Function that run when stage=0 global f_r global s_g global sinho_state sinho_state = Int8MultiArray() if f_g == 1 and f_r == 0 and s_g == 0: keypoints_red = turtle_video_siljun.find_color(blob_ROI, lower_red, upper_red, stage) print('first green signal detected.') if keypoints_red: f_r = 1 sinho_state.data = np.array([1, 0, 1]) pub_sinho.publish(sinho_state) else: sinho_state.data = np.array([0, 0, 0]) pub_sinho.publish(sinho_state) return 0 if f_g == 1 and f_r == 1 and s_g == 0: keypoints_green = turtle_video_siljun.find_color( blob_ROI, lower_green, upper_green, stage) print('red signal detected. waiting secondary green signal.') sinho_state.data = np.array([1, 0, 0]) pub_sinho.publish(sinho_state) if keypoints_green: s_g = 1 sinho_state.data = np.array([1, 1, 1]) pub_sinho.publish(sinho_state) return 0 if f_g == 1 and f_r == 1 and s_g == 1: print('second green signal detected.') s_g = 2 sinho_state.data = np.array([1, 2, 0]) pub_sinho.publish(sinho_state) return 100
def track_motion_during_duration(counter_in): State_pub = rospy.Publisher('SM/current_state', Int8MultiArray, queue_size=10) State_msg = Int8MultiArray() cmd_idx = counter_in original_cmd_idx = cmd_idx print "cmd_idx", cmd_idx start_time = rospy.get_time() print "start_time", start_time duration = 0 iterator = 0 while duration < 10.0: iterator = iterator + 1 action_state = generate_send_goal(cmd_idx) curr_time = rospy.get_time() # print "curr_time", curr_time duration = curr_time - start_time # print duration if iterator % 100000 == 1: print "duration time: %s, action_state %s," % (duration, action_state) if action_state == GoalStatus.SUCCEEDED: cmd_idx = -1 iterator = 0 battery_msg = rospy.wait_for_message('/hsrb/battery_state', BatteryState) State_msg.data.append(battery_msg.power) State_msg.data.append(int(desired_states[original_cmd_idx])) State_pub.publish(State_msg) return action_state
def __init__(self): rospy.init_node("controller", anonymous=False) self._sub = rospy.Subscriber("joy", Joy, self.callback) self._rate = rospy.Rate(50) self._pub = rospy.Publisher("cmd_vel", Twist, queue_size=10) self._velocity = Twist() self._left_stick_y = 0.0 self._left_stick_x = 0.0 self._right_stick_x = 0.0 self._r2 = 0.0 self._l2 = 0.0 self._r22 = 0.0 self._l22 = 0.0 self._profile_2_linear_axis = 0.0 self._linear_speed_limit = rospy.get_param("teleop/linear_speed_limit", 1.50) self._linear_speed_step = rospy.get_param("teleop/linear_speed_step", 0.10) self._angular_speed_limit = rospy.get_param("teleop/angular_speed_limit", 1.00) self._angular_speed_step = rospy.get_param("teleop/angular_speed_step", 0.05) self._max_linear_speed = 1.0 self._max_angular_speed = 1.0 self._dpad_y = 0.0 self._dpad_x = 0.0 self._inverted_right_stick_x = 1 # for profile 1 self._inverted_left_stick_x = 1 # for profile 2 self._r3 = 1 self._l3 = 1 self._share_button = 0 self._cross = 0 self._profile = 1 self._linear_lock = False self._inverted_angular_lock = False self._angular_lock = False self._share_lock = False self._profile_2_linear_lock = True # Remove in case of Quadrature encoder self._sign_pub = rospy.Publisher("sign", Int8MultiArray, queue_size=1) self._l_sign = 1 self._r_sign = 1 self._signs = Int8MultiArray()
#!/usr/bin/env python import rospy from sensor_msgs.msg import Joy from geometry_msgs.msg import Twist from std_msgs.msg import Int8MultiArray from std_msgs.msg import MultiArrayDimension BUTTONS_NUM = 11 vel_msg = Twist() button = Int8MultiArray() button.data = [0 for _ in range(BUTTONS_NUM)] rospy.init_node('joy_converter') x_gain = rospy.get_param('~x', 1) y_gain = rospy.get_param('~y', 1) z_gain = rospy.get_param('~z', 1) def callback(_data): vel_msg.linear.x = _data.axes[1] * x_gain vel_msg.linear.y = _data.axes[0] * y_gain vel_msg.angular.z = _data.axes[3] * z_gain for i in range(BUTTONS_NUM): button.data[i] = _data.buttons[i] def main(): pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10) pub_buttons = rospy.Publisher('/buttons', Int8MultiArray, queue_size=10) r = rospy.Rate(10)