def measure(self, initv, interval, repeat): da_all = [] speed = 5000 msg = Int64() msg.data = speed self.pub_speed.publish(speed) for i in range(repeat + 1): da = [] vol = initv + interval * i msg = Float64() msg.data = vol self.pub_vol.publish(vol) time.sleep(3) ret = reader.piv_reader() da.append(ret[0]) da.append(ret[1]) da.append(ret[2]) print(da) da_all.append(da) np.savetxt("yfactor_{0}.txt".format(self.ut), np.array(da_all), delimiter=" ") speed = 0 msg = Int64() msg.data = speed self.pub_speed.publish(speed) yf.pv_iv_plot()
def talker(name="talker", anon=False): rospy.init_node(name, anonymous=anon) time.sleep(0.5) print("Started node %s" % rospy.get_name()) chatter_topic = "/chatter" counter_topic = "/counter" print("Publishing to %s" % chatter_topic) pub1 = rospy.Publisher(chatter_topic, String, queue_size=10) time.sleep(0.5) print("Publishing to %s" % counter_topic) pub2 = rospy.Publisher(counter_topic, Int64, queue_size=10) cnt = 0 rate = rospy.Rate(1.) # 1hz while not rospy.is_shutdown(): chatter_msg = "hello world %d" % (cnt) counter_msg = Int64(data=cnt) if cnt % 10 == 1: rospy.loginfo("%d. %s <- %s" % (cnt, chatter_topic, chatter_msg)) pub1.publish(chatter_msg) rate.sleep() if cnt % 10 == 1: rospy.loginfo("%d. %s <- %s" % (cnt, counter_topic, counter_msg)) pub2.publish(Int64(data=cnt)) rate.sleep() cnt = cnt + 1 print("Finished")
def step(self): if self.playing: # don't use ros time because we don't want to rely on simulation time sleep(self.period) else: while not self.should_step and not self.playing and not self.done: sleep(0.01) if self.fwd: if self.idx < self.max_idx - 1: self.idx += 1 else: if self.auto_play: self.done = True elif self.loop: self.idx = 0 else: self.playing = False else: if self.idx > 0: self.idx -= 1 t_msg = Int64() t_msg.data = self.time_steps[self.idx] self.time_pub.publish(t_msg) self.should_step = False max_t_msg = Int64() max_t_msg.data = self.time_steps[-1] self.max_time_pub.publish(max_t_msg) return self.done
def initializeDefaultParam(self): """Initialise the grasp planner with default values """ camera_pose = Pose() camera_pose.orientation.w = 1 num_samples = Int64(400) num_threads = Int64(1) tabin_radius = Float64(0.03) hand_radius = Float64(0.08) self._graspPlanner.configureAgilePlannerLocalizer( camera_pose, num_samples, num_threads, tabin_radius, hand_radius) finger_width = Float64(0.01) hand_outer_diameter = Float64(0.09) hand_depth = Float64(0.06) init_bite = Float64(0.01) hand_height = Float64(0.02) self._graspPlanner.configureAgilePlannerHand(finger_width, hand_outer_diameter, hand_depth, init_bite, hand_height) pass
def MotionCallback(self, data): v1 = data.linear.x theta1 = data.angular.z max_speed = self.get_parameter( "max_speed").get_parameter_value().double_value turn_rate = self.get_parameter( "turn_rate").get_parameter_value().double_value self.get_logger().info("turnRate: " + str(turn_rate) + " max_speed: " + str(max_speed)) # Calculating speed of each of the motors leftSpeed = max_speed * v1 - turn_rate * theta1 rightSpeed = max_speed * v1 + turn_rate * theta1 self.get_logger().info("leftSpeed: " + str(leftSpeed) + " rightSpeed: " + str(rightSpeed)) #Sending messages to the motor nodes. lmsg = Int64() lmsg.data = int(clip(leftSpeed, -98, 98)) self.leftWheelPub.publish(lmsg) rmsg = Int64() rmsg.data = int(clip(rightSpeed, -98, 98)) self.rightWheelPub.publish(rmsg) self.get_logger().info("Updated Driving Motors- Right Speed:" + str(rightSpeed) + " Left Speed:" + str(leftSpeed))
def camera_callback(self, data): rospy.loginfo("April tag detector loginfo") # We select bgr8 because its the OpneCV encoding by default cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding="bgr8") frame = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) # April tag detector tag_detected = False detector = apriltag.Detector() result = detector.detect(frame) tagID = 0 global mission_stage_msg if len(result) == 1: tag_detected = True tag = result[0].tag_family tagID = result[0].tag_id cx = result[0].center[0] cy = result[0].center[1] corner_points = result[0].corners if len(result) > 1: print("Multiple tags detected") pub2 = rospy.Publisher("/current_tag", Int64, queue_size=1) msg2 = Int64() msg2.data = 0 if tag_detected: print("Tag detected") print('tag ID = ', tagID) #print(result) # publish current detected tag if any msg2.data = tagID if tag_detected == False: print('No tag detected') print('\n') # publish mission stage if mission_stage_msg == 0 and tagID == 8: mission_stage_msg = 1 if mission_stage_msg == 1 and tagID == 9: mission_stage_msg = 2 if mission_stage_msg == 2 and tagID == 6: mission_stage_msg = 3 if mission_stage_msg == 3 and tagID == 4: mission_stage_msg = 4 pub = rospy.Publisher("/mission_stage", Int64, queue_size=1) msg = Int64() msg.data = mission_stage_msg pub.publish(msg) pub2.publish(msg2)
def pubMsg(vr, vl): rospy.loginfo('right: %d', vr) rospy.loginfo('left: %d', vl) msg_vr = Int64() msg_vr.data = vr listener_R(msg_vr) msg_vl = Int64() msg_vl.data = vl listener_L(msg_vl)
def callback_raw_image(self, msg): """ Callback: new image came is -> Process it After detecting the ball, three topics are published: camera1/image_processed camera1/ball_visible camera1/ball_center_radius """ self.image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') greenLower = (50, 50, 20) greenUpper = (70, 255, 255) blurred = cv2.GaussianBlur(self.image, (11, 11), 0) hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, greenLower, greenUpper) mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) #cv2.imshow('mask', mask) cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) center = [0, 0] radius = 0 visible = False # 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"])) # only proceed if the radius meets a minimum size if radius > 10: # draw the circle and centroid on the frame, # then update the list of tracked points visible = True cv2.circle(self.image, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(self.image, center, 5, (0, 0, 255), -1) self.pub_img.publish((self.bridge.cv2_to_imgmsg(self.image, 'bgr8'))) self.pub_vis.publish(visible) cr = BallCenterRadius() cr.visible = Bool(visible) cr.center_x = Int64(center[0]) cr.center_y = Int64(center[1]) cr.radius = Int64(int(radius)) self.pub_cr.publish(cr)
def __init__(self): self.command_left = Int64() self.command_right = Int64() self.received_twist = None rospy.init_node('Twist2int64') # rospy.Subscriber('motor/twist/cmd_vel', Twist, self.callback) rospy.Subscriber('cmd_vel', Twist, self.callback) self.pub_right = rospy.Publisher( 'right_motor/cmd_vel', Int64, queue_size=10) self.pub_left = rospy.Publisher( 'left_motor/cmd_vel', Int64, queue_size=10)
def plan(self, object): """Compute a grasp candidate. """ pcd_name = "../../data/models/pointclouds/" + object.name + ".pcd" self._graspPlanner.computeAgileGraspFromPointCloud( String(pcd_name), String(self._svm_name), Int64(self._min_inliers)) axis_vec = self._graspPlanner.getAgileGraspCandidateAxisAt(Int64(0)) approach_vec = self._getAgileGraspCandidateApproachAt(Int64(0)) #other???? this is what I can provide: the center of the grasp (grasp position between the end of the finger tips), #the width of the object contained in the grasp, the center of the grasp projected onto the back of the hand. return (approach_vec, axis_vec)
def test_ros_message_with_int64(self): from std_msgs.msg import Int64 expected_dictionary = { 'data': -0x7FFFFFFFFFFFFFFF } message = Int64(data=expected_dictionary['data']) message = serialize_deserialize(message) dictionary = message_converter.convert_ros_message_to_dictionary(message) self.assertEqual(dictionary, expected_dictionary)
def publish(self): # pin_msg is of type Int64 pins_msg = Int64() # pack pins to pin_msg pins_msg.data = self.pins # pub self.pub.publish(pins_msg)
def low_pri_work(self): msg = Int64() msg.data = 29481 while True: self.publisher_2.publish(msg) time.sleep(0.2)
def face_detection_cb(msg): pub = rospy.Publisher('motor1/command', Int64, queue_size=1) face_msg = FaceArrayStamped() motor_command_msg = Int64() face_pos = [0, 0] global motor_angle # face check if len(msg.faces): face = msg.faces[0].face face_pos[0] = face.x face_pos[1] = face.y # check face position. left or right if face_pos[0] <= image_center[0]: motor_angle -= 1 else: motor_angle += 1 motor_command_msg.data = motor_angle # print print "face_pos(x, y): ({} {})".format(face_pos[0], face_pos[1]) print "/motor1/command: {}\n".format(motor_command_msg.data) # publish pub.publish(motor_command_msg) else: print "no faces"
def callback_number(msg): global counter counter += msg.data new_msg = Int64() new_msg.data = counter pub.publish(new_msg) rospy.loginfo("I Publish the counter value: %s", counter)
def main(): pwm_msg = Int64() pwm_msg.data = 30 pubPWM.publish(pwm_msg) subGoal = rospy.Subscriber('goal_rig', Point, get_goal) sub_pose = rospy.Subscriber('zed/zed_node/pose_twist', Twist, get_pose) sub_xboxTakeover = rospy.Subscriber('xbox_takeover', Int64, xboxTakeover) sub_pose_for_test= rospy.Subscriber('zed/zed_node/pose', PoseStamped, save_pose) sub_path_for_test= rospy.Subscriber('Astar_path', Path, save_path) reachedGoal = Int64() reachedGoal.data = 1 pubReachedGoal.publish(reachedGoal) while not rospy.is_shutdown(): doStuff() rate.sleep()
def __init__(self): self.bridge = CvBridge() self.image_received = False self.detector = apriltag.Detector() self.apriltag_detection_status = Bool() self.apriltagID = Int64() rospy.logwarn("AprilTag Detection PC Node [ONLINE]...") # rospy shutdown rospy.on_shutdown(self.cbShutdown) # Subscribe to CompressedImage msg self.image_topic = "/pc_cv_camera/image_raw" self.image_sub = rospy.Subscriber(self.image_topic, Image, self.cbImage) # Publish to Bool msg self.apriltagStatus_topic = "/pc_apriltag_detection_status" self.apriltagStatus_pub = rospy.Publisher(self.apriltagStatus_topic, Bool, queue_size=10) # Publish to Int64 msg self.apriltagID_topic = "/pc_apriltag_detection_ID" self.apriltagID_pub = rospy.Publisher(self.apriltagID_topic, Int64, queue_size=10) # Allow up to one second to connection rospy.sleep(1)
def __init__(self): # Properties self.filename = OUT_DATA_FILENAME self._data = [] self._data_size = 0 # Subscriptions self._encoder_msg = Int64() self._encoder_sub = rospy.Subscriber('/encoder', Int64, self.encoder_callback) self._cmd_vel_msg = Twist() self._cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel_callback) # Initialise servo driver self._servo_driver = curio_base.lx16a_driver.LX16ADriver() self._servo_driver.set_port(SERVO_SERIAL_PORT) self._servo_driver.set_baudrate(SERVO_BAUDRATE) self._servo_driver.set_timeout(SERVO_TIMEOUT) self._servo_driver.open() rospy.loginfo('Open connection to servo bus board') rospy.loginfo('is_open: {}'.format(self._servo_driver.is_open())) rospy.loginfo('port: {}'.format(self._servo_driver.get_port())) rospy.loginfo('baudrate: {}'.format(self._servo_driver.get_baudrate())) rospy.loginfo('timeout: {}'.format(self._servo_driver.get_timeout()))
def publish(self): # action_msg is of type Int64 action_msg = Int64() # pack action action_msg.data = self.action # pub self.pub1.publish(action_msg)
def callback_number(msg): global counter counter += msg.data new_msg = Int64() new_msg.data = counter pub.publish(new_msg) rospy.loginfo(counter)
def __init__(self): # Publisher self._encoder_msg = Int64() self._encoder_pub = rospy.Publisher('/encoder', Int64, queue_size=10) # Subscriber self._cmd_vel_msg = Twist() self._cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.cmd_vel_callback) # Initialise encoder filter self._encoder_filter = LX16AEncoderFilter(MODEL_FILENAME, WINDOW) # Initialise servo driver self._servo_driver = LX16ADriver() self._servo_driver.set_port(SERVO_SERIAL_PORT) self._servo_driver.set_baudrate(SERVO_BAUDRATE) self._servo_driver.set_timeout(SERVO_TIMEOUT) self._servo_driver.open() rospy.loginfo('Open connection to servo bus board') rospy.loginfo('is_open: {}'.format(self._servo_driver.is_open())) rospy.loginfo('port: {}'.format(self._servo_driver.get_port())) rospy.loginfo('baudrate: {}'.format(self._servo_driver.get_baudrate())) rospy.loginfo('timeout: {}'.format(self._servo_driver.get_timeout()))
def test_dictionary_with_int64(self): from std_msgs.msg import Int64 expected_message = Int64(data = -0x7FFFFFFFFFFFFFFF) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/Int64', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def publish_pendulum(self): #action_msg is of type Int64 pendulum_msg = Int64() #pack action pendulum_msg.data = self.pendulumAction # pub self.pubPendulum.publish(pendulum_msg)
def static_data_cb(data): global A, pub, tmp ipdb.set_trace() A = sum(data.taxels[0].values) + sum(data.taxels[1].values) msg = Int64() msg.data = A - tmp pub.publish(msg)
def callback_number_counter(msg): global counter counter += msg.data new_msg = Int64() new_msg.data = counter pub.publish(new_msg)
def decideOnAction(): global closeEnough global angleDev global xbox if (xbox == 0): pwm_msg = Int64() pwm_msg.data = 30 pubPWM.publish(pwm_msg) action = 4 distance, angle = calculateAngleAndDistance() if (xbox == 0) : if ((angle > 3 or angle < -3) and distance < reverseDist and distance >= closeEnough): ## if not reached goal and point not within acceptable angles, try backing action = 1 elif angle <= angleDev and angle >= -angleDev and distance >= closeEnough: ##if kinda infront and at least 'closeEnough' meters to the point action = 7 elif angle > angleDev and distance >= closeEnough: ##if to the left and at least selected distance to the point action = 6 elif angle < -angleDev and distance >= closeEnough: ##if to the right and at least selected distance to the point action = 8 else: action = 4 #stand still if no conditions are fulfilled return action
def do_joint_cmd(self, data=None): if data is not None: cmd = Int64() cmd.data = data self.JointCmdPub.publish(cmd) else: self.JointCmdPub.publish(self.JointCmd) pass
def callback_function(self, msg): """ This function is a callback function which sums the numbers. """ self.counter += msg.data new_msg = Int64() new_msg.data = self.counter self.pub.publish(new_msg)
def publish_timer_cb(self): # Pulls data and publishes them over the autogenerated ROS2 topic self.reading_data_lock = True all_device_chrc_data = self.ble_adapter.pull_device_data() self.reading_data_lock = False for addr, chrc_data in all_device_chrc_data.items(): for chrc_handle, chrc_value in chrc_data.items(): chrc_type = self.yaml_config['characteristics'][chrc_handle][ 'msg_type'] # Generate a unique topic_id for this device/chrc combo topic_id = self._generate_topic_id(addr, chrc_handle) # Create new publisher and subscriber if new device is found if topic_id not in self.pubs.keys(): try: self._generate_publisher(addr, chrc_handle, chrc_type) except ValueError: Utils.logger.error( 'An error occurred generating publishers.') return if topic_id not in self.subs.keys(): try: self._generate_subscriber(addr, chrc_handle, chrc_type) except ValueError: Utils.logger.error( 'An error occurred generating subscribers.') return pub = self.pubs[topic_id] if chrc_type == 'byte': # Hacky way to display raw bytes in ROS2 topics msg = String() msg.data = str(chrc_value) elif chrc_type == 'string': msg = String() msg.data = str(chrc_value) elif chrc_type == 'int': msg = Int64() msg.data = int(chrc_value) elif chrc_type == 'float': msg = Float64() msg.data = float(chrc_value) elif chrc_type == 'bool': msg = Bool() msg.data = bool(chrc_value) else: Utils.logger.error(f'{chrc_type} is invalid.') return Utils.logger.info(f'Published data: {topic_id}:{msg}') pub.publish(msg)
def generate_cloud_indexed_msg(self): np_cloud, idx = self.extract_indices() msg = CloudIndexed() header = Header() # header.frame_id = "xtion_rgb_optical_frame" header.frame_id = "camera_depth_optical_frame" header.stamp = rospy.Time.now() msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(header, np_cloud.tolist()) msg.cloud_sources.view_points.append(Point(0, 0, 0)) for i in xrange(np_cloud.shape[0]): msg.cloud_sources.camera_source.append(Int64(0)) for i in idx[0]: msg.indices.append(Int64(i)) return msg