def main():
rospy.init_node("depth_hold")
#Create subscribers
pid_enable_sub = rospy.subscriber("depth_hold/pid_enable", Bool,
pid_enable_callback)
initial_depth_sub = rospy.subscriber("depth_hold/setpoint", Float64,
initial_depth_callback)
depth_sensor_sub = rospy.subscriber("depth_sensor", Float32,
depth_sensor_callback)
rospy.spin()
#Create a publisher
pub = rospy.Publisher('depth_hold/control_effort', Float64, queue_size=10)
rate = rospy.Rate(10)
#Determine Setpoint
pid.SetPoint = initial_depth
#Main Loop
while dh_enabled == True:
#Call all the functions:
pid.update(depth)
thruster_power = pid.output
pub.publish(thruster_power)
rospy.sleep(0.1)
def __init__(self):
rospy.subscriber("left_wheel_encoder",Float32,self.encoder_callback,0)
rospy.subscriber("right_wheel_encoder",Float32,self.encoder_callback,1)
pub=rospy.Publisher("dead_reckoning",Odometry,queue_size=10)
encoder_counter=np.array([0,0])
current_encoder_counter=np.array([0,0])
# x y phi
current_position= [0,0, 0 ]
while not rospy.is_shutdown():
new_encoder_counter=current_encoder_counter.copy()
dist = (new_encoder_counter-old_encoder_counter)/robo.encoder_increments*2*np.pi*robo.wheel_radius
del_s = 0.5*(dist[0]+dist[1])
del_phi = 0.5*(dist[0]-dist[1])/robo.axis_length
movement=np.array([-1*del_s*np.sin(self.current_position[2]+del_phi/2),
del_s*np.cos(self.current_position[2]+del_phi/2),
del_phi])
self.current_position=self.current_position+movement
output=Odometry()
output.pose.pose.position.x = self.current_position[0]
output.pose.pose.position.y = self.current_position[1]
output.pose.pose.orientation.z= self.current_position[2]
output.twist.twist.linear.x = np.sqrt(movement[0]**2+movement[1]**2)
output.twist.twist.angular.z = movement[2]
self.pub.publish(output)
old_encoder_counter=new_encoder_counter
def calculateHolePosition(currentPos):
global holePos
rospy.subscriber("bru_vis_calibratedPos", Int8, hole_position_callback)
## Berekent de benodige bewegingen
movePerCm = calculateMoment()
## Berekent de gewenste joint beweging
wantedPos = calculateWantedPos(currentPos, holePos, movePerCm)
return wantedPos
def init_publishers_subscribers(self):
self.lidar_sub = rospy.Subscriber(self.lidarscan_topic,
LaserScan,
self.lidar_callback,
queue_size=1)
self.odom_sub = rospy.subscriber(self.odom_topic,
Odometry,
self.odom_callback,
queue_size=1)
class LocalizationFilter(object):
def __init__(self):
'''
LocalizationFilter constructor
'''
rospy.init_node("localization_filter")
ROBOPOSE_TOPIC = rospy.get_param("topics/localization_pose", "beacon_localization_pose")
BEACON_LOST_TOPIC = rospy.get_param("topics/beacon_lost", "beacon_lost")
ROBOPOSE_TOPIC_FILTERED = rospy.get_param("topics/localization_pose_filtered", "beacon_localization_pose_filtered")
BEACON_LOST_TOPIC_FILTERED = rospy.get_param("topics/beacon_lost_filtered", "beacon_lost_filtered")
rospy.subscriber(ROBOPOSE_TOPIC, PoseStamped, self.robotpose_callback)
rospy.Subscriber(BEACON_LOST_TOPIC, Bool, self.beacon_lost_callback)
self.pose_pub = rospy.Publisher(ROBOPOSE_TOPIC_FILTERED, PoseStamped, queue_size = 10)
self.beacon_lost_pub = rospy.Publisher(BEACON_LOST_TOPIC_FILTERED, Bool, queue_size = 10)
#!/usr/bin/env python
import rospy
import Pose from turtlesim.msg
def poseCalllback(pose_message):
print ('x = %f'.%pose_message.x)
print ('y = %f' %pose_message.y)
print ('yaw = %f'.%pose_message.theta)
if __name__ == '__main__':
try:
rospy.init_node('turtlesim_motion_pose', anonymous=true)
rospy.subscriber('/turtle1/pose',turtlesim_motion_pose,poseCalllback)
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("node terminated.")
def vicon_listener():
rospy.init_node('vicon_listener', anonymous = True)
rospy.subscriber('vicon/markers', TransformedStamp, avg)
def pour_bottle():
limb.move_to_joint_positions(home)
limb.move_to_joint_positions(pre_pour_1)
limb.move_to_joint_positions(pre_pour_2)
limb.move_to_joint_positions(pour)
limb.move_to_joint_positions(pre_pour_2)
limb.move_to_joint_positions(pre_pour_1)
limb.move_to_joint_positions(home)
def main():
init_sawyer()
rospy.subscriber("camera topic", image, camera_callback)
rospy.subscriber("order topic", int, order_callback)
order = -1
shutdown = -1
while not rospy.is_shutdown():
if order is None: continue
if order==0:
recipe_1()
elif order==1:
recipe_2()
else:
rospy.shutdown()
order = None
def listener():
em = ExperienceMap()
sub_odom = rospy.subscriber("/irat_red/odom", Odometry, motion_model, em)
#! /usr/bin/env python
import rospy
from sensor_msgs.msgs import LaserScan
def callback():
print len(msg.ranges)
rospy.init_node('las_scan')
sub = rospy.subscriber('/husk_high_level_controller/laser/scan', LaserScan,
callback)
rospy.spin()
def object_publisher():
pub = rospy.Publisher('/collision_object', moveit_msgs.msg.CollisionObject, queue_size=10)
moveit_commander.roscpp_initialize(sys.argv)
rospy.subscriber("phone1", Twist, callback)
rospy.init_node('moveit_test_node', anonymous=True)
rate = rospy.Rate(30)
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
#ground_pose = geometry_msgs.msg.PoseStamped()
#ground_pose.header.frame_id = robot.get_planning_frame()
#ground_pose.pose.position.x = 0.0
#ground_pose.pose.position.y = 0.0
#ground_pose.pose.position.z = 0.0
#ground_pose.pose.orientation.w = 1.0
#scene.add_plane('ground', ground_pose, (0,0,1), 0)
y_pos = 0.0
y_itt = 0.02
while not rospy.is_shutdown():
collision_object = moveit_msgs.msg.CollisionObject()
collision_object.header.frame_id = 'world'
collision_object.id = 'test_object'
collision_object.operation = collision_object.ADD
shape = shape_msgs.msg.SolidPrimitive()
shape.type = shape.CYLINDER
shape.dimensions = [0.3, 0.05]
y_pos += y_itt
if y_pos > 1.0 or y_pos < -1.0:
y_itt = -y_itt
shape_pose = geometry_msgs.msg.Pose()
#shape_pose = geometry_msgs.msg.PoseStamped()
shape_pose.position.x = 0.5
shape_pose.position.z = 0.5
shape_pose.position.y = y_pos
shape_pose.orientation.x = xorient
shape_pose.orientation.y = yorient
shape_pose.orientation.z = zorient
shape_pose.orientation.w = 1.0
#shape_pose.header.frame_id = robot.get_planning_frame()
collision_object.primitives = [shape]
collision_object.primitive_poses = [shape_pose]
pub.publish(collision_object)
#scene.add_box('box', shape_pose, (0.5, 0.5, 0.5))
rate.sleep()
def main():
rospy.init_node('Encoder Information')
rospy.subscriber("/encoder_left", Float64, callback)
rospy.subscriber("/encoder_right", Float64, callback_two)
rospy.spin()
#!/usr/bin/env python
import rospy
from robotis.msg import mahmoud
def message_callback(message):
rospy.loginfo("new data recieved: (%d, %d)"),
message.x,message.y
rospy.init_node('sub_node', anonymous=False)
rospy.subscriber('topic_1', mahmoud, message_callback)
rospy.spin()
#!usr/bin/eng python
import rospy
from std_msgs.msg import String
from AStar import astar
"""
publishers-> two strings action and value
"""
""" create node"""
rospy.init_node('pythonNode', anonymous=True)
""" publishers for (action, value) as string"""
command_pub = rospy.Publisher('commands', String, queue_size=10)
value_pub = rospy.Publisher('values', String, queue_size=10)
""" create subscribers for arduino requests"""
route_request_sub = rospy.subscriber('path_finder', String, route_request_callback)
def route_request_callback(request):
"""this function runs the a* algorithm"""
position_array = request.msg.split(',')
currPosX = position_array[0]
currPosY = position_array[1]
targetPosX = position_array[2]
targetPosY = position array[3]
# perform a* search with these parameters
if __name__ == "__main__":
print("running node...")
rospy.spin()