def axis_level_control(twist): pub_left = rospy.publisher("left_wheel_speed", Float32, queue_size=1) pub_right = rospy.publisher("right_wheel_speed", Float32, queue_size=1) v_x = twist.linear.x omega = twist.angular.z v_1 = Float32() v_2 = Float32() v_1 = v_x + (omega * robo.axis_length) / 4 v_2 = v_x - (omega * robo.axis_length) / 4 pub_left.publish(v_1) pub_right.publish(v_2)
def Wrist_mapping(value): pubwrist = rospy.publisher('WristAng', Float32, queue_size=10) #loop while not rospy.is_shutdown(): angle_wrist = numpy.interp(value, [-25, 155], [0, 180]) pubwrist.publish(angle_wrist) rate.sleep()
def Bicebs_mapping(value): pubbicebs = rospy.publisher('lBicebsAng', Float32, queue_size=10) #loop while not rospy.is_shutdown(): angle_bicebs = value pubbicebs.publish(angle_bicebs) rate.sleep()
def Rotate_mapping(value): pubrotate = rospy.publisher('lRotateAng', Float32, queue_size=10) #loop while not rospy.is_shutdown(): angle_rotate = numpy.interp(value, [-50, 90], [40, 180]) pubrotate.publish(angle_rotate) rate.sleep()
def Shoulder_mapping(value): pubshoulder = rospy.publisher('lShoulderAng', Float32, queue_size=10) #loop while not rospy.is_shutdown(): angle_shoulder = numpy.interp(value, [-25, 155], [0, 180]) pubshoulder.publish(angle_shoulder) rate.sleep()
def sender(): pub = rospy.publisher("/camera/depth_registered/points", PointCloud2); rospy.init_node('sender', anonymous=True) r = rospz.Rate(10) while not rospy.is_shutdown(): rospy.loginfo("Sender -----") pub.publish(pc2) r.sleep()
def Omoplate_mapping(value): # name of publisher, and topic and type of msg pubomoplate = rospy.publisher('lOmoplateAng', Float32, queue_size=10) #loop while not rospy.is_shutdown(): angle_omoplate = value pubomoplate.publish(angle_omoplate) rate.sleep()
def talker(): rospy.init_node("talker", anonymous=True) pub = rospy.publisher("chatter", String, queue_size=10) rate = rospy.Rate(10) # 10Hz while not rospy.is_shutdown(): hello_str = "hello world %s" % rospy.get_time() rospy.loginfo(hello_str) pub.publish(hello_str) rate.sleep()
def __init__(self, args): super(Template, self).__init__() self.args = args self.subscriber = rospy.Subscriber("/duckiebot/camera_node/image/rect", Image, self.callback) self.publisher = rospy.Publisher("instagram", Image, queue_size=10) self.pub_equalizada = rospy.Publisher("equalizada", Image, queue_size=10) self.pub_imagen = rospy.Publisher("imagen", Image, queue_size=10) self.pub_point = rospy.publisher("Point", Point, queue_size=10)
def getImage2(IP): TopCameraAddress = IP #SideCameraAddress = 'http://192.168.x.xxx/video.cgi' cap0 = cv2.VideoCapture(TopCameraAddress) #VideoCapture from source 0 while 1: ret0, frame0 = cap0.read() #Read next frame cv2.imshow('Top', frame0) #Show the frame pub = rospy.publisher('TopicName', String, queue_size=10) rospy.init_node('talker', anonymous=True) #anonymous sikre at navnet er unikt rate = rospy.Rate(1)
def callback(ros_data): goal_publisher = rospy.publisher('/goal_dynamixel_position', Pose, queue_size=10) goal_msg = Pose() if (ros_data.buttons[6] != 0 or ros_data.buttons[7] != 0): goal_msg = 2048 print("scan") else: goal_msg = 0 print("scan is done")
def move_cb(userdata): rospy.loginfo('Moveing') cmd_topic = rospy.publisher('/diff_drive_controller/cmd_vel',Twist,queue_size=1) rospy.sleep(1) vel = Twist() vel.liniear.x = user_data.lspeed result = cmd_topic.publish(vel) rospy.sleep(2) if result == None: return 'finished' else: return 'failed'
def Mesagger(): geometrypublishe = rospy.publisher('cmd_vel',geometrymsgsTwist,queue_size=10) rospy.init_node('MazePublisher', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): if((right.angle_max - right.angle_min) < (left.angle_max - left.angle_min)): msg.angular.z = -0.3333333 if((right.angle_max - right.angle_min) > (left.angle_max - left.angle_min)): mag.angular.z = -0.3333333 if((right.angle_max - right.angle_min) = (left.angle_max - left.angle_min)): msg.linear.x = 0.1
def test_put_value(self): pub = rospy.publisher('/buzzer', UInt16) for i in range(10): pub.publish(1234) time.sleep(0.1) with open("/dev/rtbuzzer0","r") as f: data = f.readline() self.assertEqual(data,"1234\n","value does not written to buzzer 0") if __name__ == '__main__': time.sleep(3) rospy.init_node('travis_test_buzzer') rostest.rosrun('pimouse_ros','travis_test_buzzer',BuzzerTest)
def __init__(self, R=np.eye(6) * 0.005, Q=np.diag([ .1, .1, (np.pi / 18)**2, 0.1, (np.pi / 18)**2, .0, .0, .0, .0, .0, .0 ]), P=np.diag([0, 0, 1, 0.1, 0.05, 1, 1, 1, 1, 1, 1])): self.R = R self.Q = Q self.P = P self.x = np.zeros(11) self.t0 = rospy.Time.now() self.posePub = rospy.publisher('/pose', PoseWithCovariance, queue_size=1) self.landmarkPub = rospy.publisher('/predicted_landmarks', PointCloud2, queue_size=1) self.scanSub = rospy.Subscriber('/measured_landmarks', PointCloud2, self.kalmanCb)
def stateMachine(self): publisher = rospy.publisher('landmarkDetection',Movement) moveMessage = None if (self.state == 0): #Look for the landmark moveMessage = self.lookForLandmark(self.landmark1) elif (self.state == 1): #Move towards landmark moveMessage = self.moveTowardsLandmark else: #Reset to initial state moveMessage = self.move() self.currentLandmark = -2 self.currentVisibleLMCode = -2 self.isLandmarkInView = False self.landmarkLocation = None self.distanceToLandmark = 1000 self.targetReached = False self.landMarkX = None self.state = 0 donePublisher = rospy.publisher('currentBallState', DetectionSwitcher) #Reuse of existing message type dState = DetectionSwitcher() dState.state = 1 donePublisher.publish(dState) publisher.publish(moveMessage)
def rviz(): rospy.init_node('rvizmark') pub = rospy.publisher("marker",visualization_msgs,queue_size=10) rate = rospy.Rate(10) while not rospy.is_shutdown(): marker= Marker() marker.header.frame_id="/base_link" marker.header.stamp=rospy.Time.now() marker.ns="rvizmark" marker.action=Marker.ADD marker.pose.orientation.w=1.0 marker.id=0 marker.type = Marker.LINE_STRIP marker.scale.x=0.1 marker.color.b=1.0 startpt=(1.0,2.0) endpt=(5.0,7.0) marker.points.append(startpt) marker.points.append(endpt) pub.publish(marker) rate.sleep() rospy.spin()
#publish the robots positions for tracking in cm: Robot1_current = rospy.Publisher('rob1_CurrentPose', Int32MultiArray, queue_size=10) Robot2_current = rospy.Publisher('rob2_CurrentPose', Int32MultiArray, queue_size=10) Robot3_current = rospy.Publisher('rob3_CurrentPose', Int32MultiArray, queue_size=10) Robot4_Current = rospy.Publisher('rob4_CurrentPose', Int32MultiArray, queue_size=10) #publish all robot poses in single array: pub_robots_current_poses = rospy.publisher('robots_current_poses', Int32MultiArray, queue_size=10) x = 0 index = 0 x_ref = 0 y_ref = 0 X_r1 = 0 Robot = Int32MultiArray() get_quaternion_x = get_quaternion_y = get_quaternion_z = get_quaternion_w = 0 def callback(data): try: global Robot, index, x, x_ref, y_ref, x_1, y_1, x_2, y_2, get_quaternion_x, get_quaternion_y, get_quaternion_z, get_quaternion_w Robot = [0, 0, 0, 0, 0, 0, 0, 0]
#!/usr/bin/env python from std_msgs.msg import Int64 import rospy import math from geometry_msgs.msg import Twist from geometry_msgs.msg import Pose from time import sleep pub_vel = rospy.publisher('/mobile_base/commands/velocity',Twist,queue_size=10) pose = pose() t = Twist() def odomCb(data): global pose print('In odomCb') pose = data.pose.pose print('pose in odomCb: is' str(% pose)) def turnToTheta(theta): print('In turnToTHeata') def moveDist(d, speed): print('In moveDist') #the position before we move p_start = pose.position dRelative = 0 t = Twist() t.linear.x = speed
def callback_vel(data): rospy.loginfo(rospy.get_caller_id() + "vel : I heard %s", data.velocity.x) rospy.loginfo(rospy.get_caller_id() + "vel : I heard %s", data.velocity.y) rospy.loginfo(rospy.get_caller_id() + "vel : I heard %s", data.velocity.z) msg_Twist.linear = data.velocity def callback_ang_vel(data): rospy.loginfo(rospy.get_caller_id() + "vel : I heard %s", data.gyro.x) rospy.loginfo(rospy.get_caller_id() + "vel : I heard %s", data.gyro.y) rospy.loginfo(rospy.get_caller_id() + "vel : I heard %s", data.gyro.z) msg_Twist.angular = data.gyro rospy.init_node('listener') #, anonymous=True) GPS = rospy.publisher('Pose_GPS_cart', Pose, queue_size=10) IMU = rospy.publisher('Twist_GPS_cart', Twist, queue_size=10) msg_Pose = Pose() msg_Twist = Twist() rospy.Subscriber("ekf_euler", SbgEkfEuler, callback_IMU) rospy.Subscriber("gps_pos", SbgGpsPos, callback_GPS) rospy.Subscriber("ekf_nav", SbgEkfNav, callback_vel) rospy.Subscriber("imu_data", SbgImuData, callback_ang_vel) r = rospy.Rate(10) x, y = 0, 0 while not rospy.is_shutdown():
relativeAngle = math.atan(yVel/xVel) if(relativeAngle => 0 and relativeAngle <= 135): print('the angle is between 0 and 135 degrees') mappedAngle = (-1)*relativeAngle + 135 else if(relativeAngle > 135 and relativeAngle <= 315): print('the angle is between 135 and 315 degrees') mappedAngle = (-1)*relativeAngle + 495 else: print('unknown angle') print(mappedAngle) getCompass(mappedAngle) velMsg = String() velMsg.data = mappedAngle pub.publish(velMsg) time.sleep(1) if __name__ == '__main__': vel = velData() rospy.Subscriber('/vel', TwistStamped, velCallback, vel) pub = rospy.publisher('/gps/heading', String, queue_size=10) t = threading.Thread(target=computeHeading, args=(vel,pub)) t.daemon = True t.start() rospy.spin()
import rospy from std_msgs.msg import Bool, String from datetime import datetime num_objects = 0 most_likely_objects = ['bottle','cup','wine glass','laptop','mouse'] weight_increase = 1.2 object_dict = {} change_publisher = rospy.publisher('change_detected', Bool, queue_size=100) def detectChange(detecting_object): #global num_objects global detecting change_publisher.publish(detecting == detecting_object) #seperate the string into objects using deliminator "|" """seperated = objects.split("|") if not(len(seperated) == num_objects): change_publisher.publish(True) else: change_publisher.publish(False) num_objects = len(seperated) """ def probabalistic_calculations(objects): #a function to give higher weighting to objects that are likely to be in the frame seperated = objects.split("|") for ob in seperated: