def execute(self, goal):
self.complete = False
rate = rospy.rate(50)
while not complete and not self.server.is_preempt_requested():
if self.server.is_new_goal_available():
pass
if self.odom is None:
continue
def callback(self, data):
wheel_left = forward_kinematics(float(data), 0.0)
print data
r = rospy.rate(10)
while not rospy.is_shutdown():
twist_msg = Twist()
twist_msg.angular.x = wheel_left[0]
self.pub.publish(twist_msg)
r.sleep()
def poly_talker():
pub = rp.Publisher('polycrier', String, queue_size=10)
rp.init_node('Crier', anonymous=True)
rate = rp.rate(21) #21Hz
while not rp.is_shutdown():
num = random.randint(0, 3)
name = ["Will", "Brett", "Henrique", "David"]
output = name[num]
rp.loginfo(output)
pub.publish(output)
rate.sleep()
def main():
rospy.init_node('pitch_dynamics')
rospy.Subscriber("",,) #("subscribed topic name",message class, callback) The callback records the subscribed data and processes it
pub = rospy.Publisher('', , queue_size=10) #("name of published topic". message class, queue size)
rate = rospy.rate(10) #frequency of publishing (optional)
while not rospy.is_shutdown():
#do stuff here
#publish
pub.publish() #argument is the variable that you want to publish, should conform with message class eg:string
rate.sleep() #sleep in order to publish at desired rate
#common for all nodes
rospy.spin() #so that ROS knows that this node should keep running
def __init__(
self,
host="10.0.0.102",
port=800,
x_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <t-X>",
y_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <t-Y>",
theta_VICON_name="GPSReceiverHelmet-goodaxes:GPSReceiverHelmet01 <a-Z>",
):
"""
Pose handler for VICON system
host (string): The ip address of VICON system (default="10.0.0.102")
port (int): The port of VICON system (default=800)
x_VICON_name (string): The name of the stream for x pose of the robot in VICON system (default="SubjectName:SegmentName <t-X>")
y_VICON_name (string): The name of the stream for y pose of the robot in VICON system (default="SubjectName:SegmentName <t-Y>")
theta_VICON_name (string): The name of the stream for orintation of the robot in VICON system (default="SubjectName:SegmentName <a-Z>")
"""
br = tf.TransformBroadcaster()
self.host = host
self.port = port
self.x = x_VICON_name
self.y = y_VICON_name
self.theta = theta_VICON_name
self.s = _pyvicon.ViconStreamer()
self.s.connect(self.host, self.port)
self.s.selectStreams(["Time", self.x, self.y, self.theta])
self.s.startStreams()
# Wait for first data to come in
# while self.s.getData() is None: pass
rate = rospy.rate(10.0)
while not rospy.is_shutdown():
print self.s.getData()
# br.sendTransform((msg.x, msg.y, 0),
# tf.transformations.quaternion_from_euler(0, 0, msg.theta),
# rospy.Time.now(),
# turtlename,
# "world")
rate.sleep()
def execute(self, goal): #Largely inspired by UF's base_controller.py but as an actionserver and only one waypoint at a time
self.complete = False
rate = rospy.rate(50)
#TODO: make this consider which tf frame the goal is in
#UF doesnt seem to worry about this being in a wierd frame so why should we
targetPosition = rosToArray(goal.targetPose.position)
targetOrientation = rosToArray(goal.targetPose.orientation)
targetLinearVelocity = rosToArray(goal.targetTwist.linear)
targetAngularVelocity = rosToArray(goal.targetTwist.angular)
while not complete:
#TODO: finish this
if self.server.is_new_goal_available():
pass
if self.odom is None: #Cant really drive the robot if we dont have odometry data
continue
if self.server.is_preempt_requested():
thrust_cmd = geometery_msgs.msg.Twist(
linear=geometry_msgs.msg.Vector3([0,0,0])
angular=geometry_msgs.msg.Vector3([0,0,0])
)
self.thrusterPublisher.publish(thrust_cmd)
self.server.set_preempted(result=sub_trajectory.msg.ExecuteWaypointResult(Ok=True))
return
#shut down if we havent had odometry in the last 3 seconds
#TODO: Finish this
#TODO: Make this use a param or something
if rospy.Time.now() - self.odom.header.stamp > 3:
pass
#TODO: consider the tf frame of odom
positionError = rosToArray(self.odom.pose.pose.position) - targetPosition
orientationError = quatErrorToAxis(rosToArray(self.odom.pose.pose.orientation), targetOrientation)
linearVelocityError = rosToArray(self.odom.twist.twist.linear) - targetLinearVelocity
angularVelocityError = rosToArray(self.odom.twist.twist.angular) - targetAngularVelocity
if np.linalg.norm(positionError) < 0.05 and np.linalg.norm(orientationError) < 0.1
and np.linalg.norm(linearVelocityError) < 0.05 and np.linalg.norm(angularVelocityError) < 0.05
and not goal.stationKeeping:
complete=True
thrust_cmd = geometery_msgs.msg.Twist(
linear=geometry_msgs.msg.Vector3([0,0,0])
angular=geometry_msgs.msg.Vector3([0,0,0])
)
self.thrusterPublisher.publish(thrust_cmd)
continue
world_linear = self.gains["kp_linear"] * positionError + self.gains["kd_linear"] * linearVelocityError
world_angular = self.gains["kp_angular"] * orientationError + self.gains["kd_angular"] * angularVelocityError
worldToBody = transformations.quaternion_matrix(rosToArray(self.odom.pose.pose.orientation))[:3,:3]
body_linear = worldToBody.dot(world_linear)
body_angular = worldToBody.dot(world_angular)
thrust_cmd = geometery_msgs.msg.Twist(
linear=geometry_msgs.msg.Vector3(*body_linear)
angular=geometry_msgs.msg.Vector3(*body_angular)
)
self.thrusterPublisher.publish(thrust_cmd)
rate.sleep()
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
rate = rospy.rate(0.1)
def callback2(data):
rospy.loginfo(rospy.get_caller_id() + "I understand that you are %s", data.data)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/qt_face/setEmotion", String, callback2)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
rate.sleep()
if __name__ == '__main__':
listener()
def taskCallback(self, goal):
r = rospy.rate(10)
success = True
ot =
# #while not rospy.is_shutdown():
# ten_message=latlonalt()
# ten_message.latitude=states_ten[0]
# ten_message.longitude=states_ten[2]
# ten_message.altitude=states_ten[4]
# #rospy.loginfo(thirty_message)
# pub.publish(ten_message)
# #rate.sleep()
if __name__=='__main__':
rospy.init_node('tilt_servo_control')
uav_data = Get_UAV_Data()
rate = rospy.rate(50) # 10hz
while not rospy.is_shutdown():
uav_data.tilt_control()
#print('Executing main while loop')
rospy.sleep(1)
# while not rospy.is_shutdown():
# #initial kalman filtering
# # uav_data.CV_Prediction(uav_data.dt, False)
# # uav_data.CV_Update()
#
# uav_data.IMM_Mixing()
#! /usr/bin/env python
# import libraries
import rospy
import math
# from geometry_msg import velocities
from geometry_msgs.msg import Twist
# from sensor_msg import ranges
from sensor_msgs.msg import LaserScan
velocity = Twist()
velocity.linear.x = 1
velocity.angular.z = 0
rospy.rate(1)
rospy.init_node('topics_quiz_node')
def callback(msg):
# print msg.ranges[360]
pub.publish(velocity)
counter = 0
while not rospy.is_shutdown() and counter > 0:
counter = counter + 1
if msg.ranges[360] <= 2:
velocity.linear.x = 0
while counter <= 1:
velocity.angular.z = -math.pi
pub.publish(velocity)
rate.sleep()
