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:
