Skip to content

Instantly share code, notes, and snippets.

@Kdoje Kdoje/robot.py
Created Nov 10, 2018

Embed
What would you like to do?
Lab 2 code
#!/usr/bin/env python
import math
import copy
import numpy
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped, Twist, Pose, PoseWithCovariance
from geometry_msgs.msg import Quaternion
import tf
from tf.transformations import euler_from_quaternion
class Robot:
px = 0
py = 0
zRot = 0
vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
vel_msg=Twist()
def __init__(self):
""""
Set up the node here
"""
rospy.loginfo('hello')
sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
goalSub = rospy.Subscriber("/move_base/goal", PoseStamped, self.nav_to_pose)
def nav_to_pose(self, goal):
# type: (PoseStamped) -> None
"""
This is a callback function. It should extract data from goal, drive in a striaght line to reach the goal and
then spin to match the goal orientation.
:param goal: PoseStamped
:return:
"""
rospy.loginfo('we movin')
to_move_x = goal.pose.position.x
to_move_y = goal.pose.position.y
disp_x = to_move_x-self.px
disp_y = to_move_y-self.py
total_dist = math.sqrt(math.pow(disp_x, 2)+math.pow(disp_y, 2))
# if disp_x<0:
# total_dist=-total_dist # make the distance negative
init_rotation = math.atan2(disp_y, disp_x)
# get the robot in position to move
self.rotate(init_rotation)
# actually move
rospy.loginfo("actually moving straight for {}".format(total_dist))
self.drive_straight(.5, total_dist)
# do the last rotation
quat = goal.pose.orientation
q = [quat.x, quat.y, quat.z, quat.w]
roll, pitch, to_rotate = euler_from_quaternion(q)
self.rotate(to_rotate)
# rospy.loginfo('the setpoint is x: {} y: {}'.format())
# to_move=goal.pose.position.x
# quat = goal.pose.orientation
# q = [quat.x, quat.y, quat.z, quat.w]
# roll, pitch, to_rotate = euler_from_quaternion(q)
# self.rotate(to_rotate)
# self.drive_straight(.5, to_move)
# now = rospy.Time.now()
# self._odom_list.waitForTransform('base_link', 'odom', now, rospy.Duration(1))
# transGoal = self._odom_list.transformPose('base_link', goal)
#
# (goal_angle_roll, goal_angle_pitch, goal_angle_yaw) = tf.transformations.euler_from_quaternion(
# [transGoal.pose.orientation.x,
# transGoal.pose.orientation.y,
# transGoal.pose.orientation.z,
# transGoal.pose.orientation.w])
def drive_straight(self, speed, distance):
"""
Make the turtlebot drive straight
:type speed: float
:type distance: float
:param speed: speed to drive
:param distance: distance to drive
:return:
"""
drive_rate = rospy.Rate(10) # 10hz
self.vel_msg.linear.y = 0
self.vel_msg.linear.z = 0
start_px = self.px
start_py = self.py
dist_traveled=math.sqrt(math.pow(start_px-self.px, 2) + math.pow(start_py-self.py, 2))
while dist_traveled < distance: # and move it forward
dist_traveled = math.sqrt(math.pow(start_px - self.px, 2) + math.pow(start_py - self.py, 2))
self.vel_msg.linear.x = speed
self.vel_pub.publish(self.vel_msg)
drive_rate.sleep()
self.vel_msg.linear.x = 0
self.vel_pub.publish(self.vel_msg)
def rotate(self, angle):
"""
Rotate in place
:param angle: angle to rotate
:return:
"""
self.vel_msg.angular.x = 0
self.vel_msg.angular.y = 0
rospy.loginfo("my roatation is {}".format(self.zRot-angle))
while (self.zRot-angle) > .01:
self.vel_msg.angular.z = -1
self.vel_pub.publish(self.vel_msg)
#rospy.loginfo("my roatation is {}".format(self.zRot-angle))
while (self.zRot-angle) < -.01:
self.vel_msg.angular.z = 1
self.vel_pub.publish(self.vel_msg)
#rospy.loginfo("my roatation is {}".format(self.zRot-angle))
rospy.loginfo("exited loop at {}".format(self.zRot - angle))
self.vel_msg.angular.z = 0
self.vel_pub.publish(self.vel_msg)
def odom_callback(self, msg):
"""
update the state of the robot
:type msg: Odom
:return:
"""
self.px = msg.pose.pose.position.x
self.py = msg.pose.pose.position.y
quat = msg.pose.pose.orientation
q = [quat.x, quat.y, quat.z, quat.w]
roll, pitch, self.zRot = euler_from_quaternion(q)
# rospy.loginfo("-----------------------------")
# rospy.loginfo("hello from odom")
# rospy.loginfo("my roll is {}".format(self.px))
if __name__ == '__main__':
rospy.init_node('Robot')
ne = Robot()
# ne.drive_straight(.2, 2)
# ne.rotate(-math.pi)
# ne.rotate(0)
while not rospy.is_shutdown():
rate = rospy.Rate(10) # 10hz
rate.sleep()
pass
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.