# Computer Science - Intro To Robotincs

Posted Under: Python

Order Now

#### Related Study Services

DESCRIPTION
Posted
Modified
Viewed 16
Write a python ROS node that will behave as follows: a) Will rotate 360 degrees in place when started, with fixed angular velocity w=0.1 rps, and then stop b) At each step during the rotation, it will add gFrontDistance to any array (list) H, indexed by the angle rotated. You will need to add  to the gPose[2] orientation so that it is always positive, and then map that floating point number to an integer between 0 and a variable Pmax so that you can use it as an index into the array. Initially pMax can be 8. c) After one rotation, and the update of H, show a bar graph of the array. d) Repeat the process at a rate 0.1 Hz (how often will this be repeated in seconds?) Write a short explanation of how you implemented each of these three. Step 2: Using the “enclosed.world” simulation world, move the robot using the Gazebo menu and the mouse to the following points, and generate and record H for each one: a) A short distance from reach of the four corners, b) And approximately centered, just above the middle wall Explain the appearance of each graph based on the location at which it was measured. Step 3: Repeat the process for pMax=16. Explain any difference you see and comment on what effect this might have on any future planned motions. Step 4 Modify your ROS node by adding a function that when given H as an argument, will return the index into H that reflects the furthest open space around the robot. Add code to rotate the robot to this heading and move forward for 0.5 seconds at 0.5 m/s. (Remember you added .) Place the robot in one of the corners and allow your ROS node to repeat at least 3 times. Explain what happens using the bar charts you generated The current code; #!/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_angle
Attachments
#!/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_angle<gPose[2]: # turn +ve/CCW or -ve/CW         direction2go = -1      print ("Spinning to ",target_angle, " from ",gPose[2]," vgain= ",vgain)       while not rospy.is_shutdown() and adist>accuracy:         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