Do My Homework / Homework Help Answers / Python Homework Help / Computer Science - Intro to robotincs

# Computer Science - Intro to robotincs

Need help with this question or any other Python assignment help task?

#!/usr/bin/env python # license removed for brevity # # Example program that # spins in place # dml 2016 # # import sys import math import random import rospy # needed for ROS from geometry_msgs.msg import Twist      # ROS Twist message from sensor_msgs.msg import LaserScan    # ROS laser scan message from nav_msgs.msg import Odometry        # ROS pose message from tf.transformations import euler_from_quaternion # rotations are quaternions # ROS topic names for turtlebot_gazebo motionTopic ='/cmd_vel'  poseTopic   = '/odom' laserTopic  = '/scan' # Global variables gFrontDistance=0      # average laser range distance in front of robot gPose = [0,0,0] # callback to accept pose information # orientation is stored as a quaterion # and needs to be translated to angles # def callback_pose(msg):     global gPose       orient = msg.pose.pose.orientation     quat = [axis for axis in [orient.x, orient.y, orient.z, orient.w]]     (roll,pitch,yaw)=euler_from_quaternion(quat)     gPose[0]= msg.pose.pose.position.x     gPose[1]= msg.pose.pose.position.y     gPose[2]= yaw     return # callback for the laser range data # calculates a front distance as an average # laser reading over a few measurements in front # def callback_laser(msg):     '''Call back function for laser range data'''     global gFrontDistance     center = 0     width = 10 # region in front to average over     dist=0.0     count=0     for i in range(center-width,center+width):         if not math.isnan( msg.ranges[i] ) and \            not math.isinf( msg.ranges[i] ) and \            msg.ranges[i]>1.0:             dist += msg.ranges[i]             count += 1     if (count>0) :         gFrontDistance = dist/float(count)     else:         gFrontDistance=1000 # a big number, nothing in view     return # spin_node # will spin the robot aroud to reach the angle given as argument # at speed given as second argument in rps def spin_node(target_angle,vgain):     '''Spins the robot to target_angle with factor vgain'''     rospy.init_node('Spin_Node', anonymous=True)     # register publisher and two subscribers      vel_pub = rospy.Publisher(motionTopic, Twist, queue_size=0)     scan_sub = rospy.Subscriber(laserTopic, LaserScan, callback_laser)     pose_sub = rospy.Subscriber(poseTopic, Odometry, callback_pose)     rospy.sleep(1) # allow callbacks to happen before proceeding     rate = rospy.Rate(10) # Hz for main loop frequency     msg = Twist() # empty new velocity message       accuracy = 0.01 # radians     adist = abs(gPose[2]-target_angle)     direction2go = 1          if target_angleaccuracy:         adist = abs(gPose[2]-target_angle)         msg.linear.x,msg.angular.z=0.0,direction2go*vgain*adist         vel_pub.publish(msg)         rate.sleep()     print ("Spin complete at ",gPose[2])     msg.linear.x,msg.angular.z=0.0,0.1 # stop now     vel_pub.publish(msg)     return # # Callback function for a controlled shutdown # def callback_shutdown():     print("Shutting down")     vpub = rospy.Publisher(motionTopic, Twist, queue_size=10)     msg = Twist()     msg.angular.z=0.0     msg.linear.x=0.0     vpub.publish(msg)        return #-------------------------------MAIN  program---------------------- if __name__ == '__main__':     try:         rospy.on_shutdown(callback_shutdown)         spin_node(float(sys.argv[1]),float(sys.argv[2]))     except rospy.ROSInterruptException:         pass
There are no answers to this question.