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()