def reverse_robot():
print "Reversing Robot\n"
import time
global vel_lin_buffer, vel_ang_buffer, reversing
global cmd_vel_pub, restored_bump_pub
reversing = True
for l, a in zip(reversed(vel_lin_buffer), reversed(vel_ang_buffer)):
lin_vec = Vector3(-l[0], -l[1], -l[2])
ang_vec = Vector3(-a[0], -a[1], -a[2])
time.sleep(0.1)
twist_msg = Twist()
twist_msg.linear = lin_vec
twist_msg.angular = ang_vec
cmd_vel_pub.publish(twist_msg)
for _ in range(10):
time.sleep(0.05)
twist_msg = Twist()
twist_msg.linear = Vector3(0, 0, 0)
twist_msg.angular = Vector3(0, 0, 0)
cmd_vel_pub.publish(twist_msg)
reversing = False
restored_bump_pub.publish(True)
def square():
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
rospy.init_node('square', anonymous=True)
pause = Twist()
pause.linear = Vector3(0, 0, 0)
pause.angular = Vector3(0, 0, 0)
forward = Twist()
forward.linear = Vector3(forward_velocity, 0, 0)
forward.angular = Vector3(0, 0, 0)
rotate = Twist()
rotate.linear = Vector3(0, 0, 0)
rotate.angular = Vector3(0, 0, rotate_velocity)
cmds = [pause, forward, pause, rotate] * 4 + [pause]
cmd_step = 0
sleep_times = {pause: 0.5, forward: 2, rotate: 0.5}
while not rospy.is_shutdown() and cmd_step < len(cmds):
cmd = cmds[cmd_step]
sleep_time = sleep_times[cmd]
# rospy.loginfo(cmd, sleep_time)
pub.publish(cmd)
rospy.sleep(sleep_time)
cmd_step += 1
def publishVelocities(self, listOfVelocities):
"""
Input: takes a list of Velocities, alternating angular and linear.
Output: publishes velocities for a certain amount of time to the rostopic
"""
r = rospy.Rate(20)
r.sleep()
for i in range(len(listOfVelocities)):
print(listOfVelocities[i])
if (i % 2 == 1):
output = Twist()
output.linear = Vector3(listOfVelocities[i], 0, 0)
output.angular = Vector3(0, 0, 0)
now = rospy.get_time()
while (now + 1.0 >
rospy.get_time()) and (not rospy.is_shutdown()):
self.velpub.publish(output)
r.sleep()
else:
if (listOfVelocities[i] != 0):
output = Twist()
if (listOfVelocities[i] > 3.14):
listOfVelocities[i] = listOfVelocities[i] - 6.28
output.linear = Vector3(0, 0, 0)
output.angular = Vector3(0, 0, -listOfVelocities[i])
now = rospy.get_time()
while (now + 1.0 >
rospy.get_time()) and (not rospy.is_shutdown()):
self.velpub.publish(output)
r.sleep()
def turtle(cmd):
print("cmd: ", cmd)
# rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, -1.8]'
# $ rostopic type /turtle1/cmd_vel
# geometry_msgs/Twist
# $ rosmsg show geometry_msgs/Twist
# geometry_msgs/Vector3 linear
# float64 x
# float64 y
# float64 z
# geometry_msgs/Vector3 angular
# float64 x
# float64 y
# float64 z
pub_twist = Twist()
pub_twist.linear = Vector3(x=0.0, y=0.0, z=0.0)
pub_twist.angular = Vector3(x=0.0, y=0.0, z=0.0)
if cmd == "顺时针旋转。":
pub_twist.linear = Vector3(x=2.0, y=0.0, z=0.0)
pub_twist.angular = Vector3(x=0.0, y=0.0, z=-1.8)
if cmd == "逆时针旋转。":
pub_twist.linear = Vector3(x=2.0, y=0.0, z=0.0)
pub_twist.angular = Vector3(x=0.0, y=0.0, z=1.8)
pub_turtle.publish(pub_twist)
def robotDoLoop():
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
rospy.init_node('robotDoLoop', anonymous=True)
do_nothing = Twist()
do_nothing.linear = Vector3(0, 0, 0)
do_nothing.angular = Vector3(0, 0, 0)
go_forward = Twist()
go_forward.linear = Vector3(forward_velocity, 0, 0)
go_forward.angular = Vector3(0, 0, 0)
do_rotation = Twist()
do_rotation.linear = Vector3(0, 0, 0)
do_rotation.angular = Vector3(0, 0, rotate_velocity)
cmds = [do_nothing, go_forward, do_nothing, do_rotation] * 4 + [do_nothing]
cmd_step = 0
sleep_times = {do_nothing: 0.5, go_forward: 2.0, do_rotation: 0.5}
# Wait 5 seconds, then run the loop
rospy.sleep(5)
while not rospy.is_shutdown() and cmd_step < len(cmds):
cmd = cmds[cmd_step]
sleep_time = sleep_times[cmd]
# rospy.loginfo(cmd, sleep_time)
pub.publish(cmd)
rospy.sleep(sleep_time)
cmd_step += 1
def goalPub():
global currentGoal
goalCount = 0
goalinworld = rospy.Publisher('robot_2/goal',Twist,queue_size=10)
goal = Twist()
goal.linear = Vector3(currentGoal.x,currentGoal.y,0.0)
goal.angular = Vector3(0.0,0.0,0.0)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
result = goal2_client()
if result.robot2_thereOrNot == 1:
goalCount = goalCount + 1
if goalCount>=1:
currentGoal.x = -13.0
currentGoal.y = -10.0
else:
currentGoal.x = -13.0
currentGoal.y = -5.0
goal.linear = Vector3(currentGoal.x,currentGoal.y,0.0)
goal.angular = Vector3(0.0,0.0,0.0)
rospy.loginfo(goalCount)
rospy.loginfo(goal.linear)
goalinworld.publish(goal)
rate.sleep()
def pub_movement(movement):
if not isinstance(movement, type("")):
return False
move_msg = Twist()
if movement == "stop":
move_msg.angular = 0
move_msg.linear = 0
elif movement == "left":
move_msg.angular = -robot_speed * 0.5
move_msg.linear = 0
elif movement == "right":
move_msg.angular = robot_speed * 0.5
move_msg.linear = 0
elif movement == "forward":
move_msg.angular = 0
move_msg.linear = robot_speed
elif movement == "backward":
move_msg.angular = 0
move_msg.linear = -robot_speed
movement_pub.publish()
def turn180(self):
print "Doing turn now!"
trans, rot = self.tfFrames.lookupTransform('/odom','/base_link',rospy.Time(0))
Angle = euler_from_quaternion(rot)
origAng = Angle[2]
output = Twist()
#print(euler_from_quaternion(rot))
output.linear = Vector3(0,0,0)
output.angular = Vector3(0,0,self.roomba_turn_speed)
self.pub.publish(output)
trans, rot = self.tfFrames.lookupTransform('/odom','/base_link',rospy.Time(0))
Angle = euler_from_quaternion(rot)
newAng = Angle[2]
r = rospy.Rate(50)
while(self.angle_diff(newAng,origAng) > -2.6) and not rospy.is_shutdown():
self.pub.publish(output)
trans, rot = self.tfFrames.lookupTransform('/odom','/base_link',rospy.Time(0))
Angle = euler_from_quaternion(rot)
newAng = Angle[2]
r.sleep()
#print(self.angle_diff(newAng,origAng))
output = Twist()
output.linear = Vector3(self.roomba_speed,0,0)
output.angular = Vector3(0,0,0)
self.pub.publish(output)
def callback(data):
rospy.loginfo("Recieved {0}".format(data.data))
twist = Twist()
if data.data # If data present an object was detected, move forward
twist.linear.x = .10; twist.linear.y = 0.0; twist.linear.z = 0.0;
twist.angular. = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0;
else #if not present, no object detected, spin
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0;
twist.angular. = 0.0; twist.angular.y = 0.0; twist.angular.z = .5;
pub.publish(twist)
def roll(self, color, speed, heading, time=5):
t = Twist()
t.linear = Vector3(speed, 0, 0)
t.angular = Vector3(0, 0, heading)
self.pub_sphero_cmd_vel[color].publish(t)
rospy.sleep(time)
t = Twist()
t.linear = Vector3(0, 0, 0)
t.angular = Vector3(0, 0, 0)
self.pub_sphero_cmd_vel[color].publish(t)
def _parse_joy(self, joy=None):
if self._msg_type == 'twist':
cmd = Twist()
else:
cmd = Accel()
if joy is not None:
# Linear velocities:
l = Vector3(0, 0, 0)
if self._axes['x'] > -1 and abs(joy.axes[self._axes['x']]) > self._deadzone:
l.x += self._axes_gain['x'] * joy.axes[self._axes['x']]
if self._axes['y'] > -1 and abs(joy.axes[self._axes['y']]) > self._deadzone:
l.y += self._axes_gain['y'] * joy.axes[self._axes['y']]
if self._axes['z'] > -1 and abs(joy.axes[self._axes['z']]) > self._deadzone:
l.z += self._axes_gain['z'] * joy.axes[self._axes['z']]
if self._axes['xfast'] > -1 and abs(joy.axes[self._axes['xfast']]) > self._deadzone:
l.x += self._axes_gain['xfast'] * joy.axes[self._axes['xfast']]
if self._axes['yfast'] > -1 and abs(joy.axes[self._axes['yfast']]) > self._deadzone:
l.y += self._axes_gain['yfast'] * joy.axes[self._axes['yfast']]
if self._axes['zfast'] > -1 and abs(joy.axes[self._axes['zfast']]) > self._deadzone:
l.z += self._axes_gain['zfast'] * joy.axes[self._axes['zfast']]
# Angular velocities:
a = Vector3(0, 0, 0)
if self._axes['roll'] > -1 and abs(joy.axes[self._axes['roll']]) > self._deadzone:
a.x += self._axes_gain['roll'] * joy.axes[self._axes['roll']]
if self._axes['rollfast'] > -1 and abs(joy.axes[self._axes['rollfast']]) > self._deadzone:
a.x += self._axes_gain['rollfast'] * joy.axes[self._axes['rollfast']]
if self._axes['pitch'] > -1 and abs(joy.axes[self._axes['pitch']]) > self._deadzone:
a.y += self._axes_gain['pitch'] * joy.axes[self._axes['pitch']]
if self._axes['pitchfast'] > -1 and abs(joy.axes[self._axes['pitchfast']]) > self._deadzone:
a.y += self._axes_gain['pitchfast'] * joy.axes[self._axes['pitchfast']]
if self._axes['yaw'] > -1 and abs(joy.axes[self._axes['yaw']]) > self._deadzone:
a.z += self._axes_gain['yaw'] * joy.axes[self._axes['yaw']]
if self._axes['yawfast'] > -1 and abs(joy.axes[self._axes['yawfast']]) > self._deadzone:
a.z += self._axes_gain['yawfast'] * joy.axes[self._axes['yawfast']]
cmd.linear = l
cmd.angular = a
else:
cmd.linear = Vector3(0, 0, 0)
cmd.angular = Vector3(0, 0, 0)
return cmd
def reverse_robot():
global logger
logger.info("Reversing Robot ...\n")
import time
global vel_lin_buffer, vel_ang_buffer, reversing
global cmd_vel_pub, restored_bump_pub
global curr_cam_data, save_pose_data, image_dir, save_img_seq
reversing = True
logger.debug("Posting cmd_vel messages backwards with a %.2f delay",
utils.REVERSE_PUBLISH_DELAY)
for l, a in zip(reversed(vel_lin_buffer), reversed(vel_ang_buffer)):
lin_vec = Vector3(-l[0], -l[1], -l[2])
ang_vec = Vector3(-a[0], -a[1], -a[2])
twist_msg = Twist()
twist_msg.linear = lin_vec
twist_msg.angular = ang_vec
rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
cmd_vel_pub.publish(twist_msg)
# publish last twist message so the robot reverse a bit more
for _ in range(5):
rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
cmd_vel_pub.publish(twist_msg)
logger.debug("Finished posting cmd_vel messages backwards ...\n")
rospy.sleep(0.5)
logger.debug("Posting zero cmd_vel messages with %.2f delay",
utils.ZERO_VEL_PUBLISH_DELAY)
for _ in range(100):
rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
twist_msg = Twist()
twist_msg.linear = Vector3(0, 0, 0)
twist_msg.angular = Vector3(0, 0, 0)
cmd_vel_pub.publish(twist_msg)
logger.debug("Finished posting zero cmd_vel messages ...\n")
reversing = False
restored_bump_pub.publish(True)
if image_dir is not None:
if save_img_seq:
pickle.dump(
curr_cam_data,
open(image_dir + os.sep + 'images_' + str(episode) + '.pkl',
'wb'))
curr_cam_data = []
pickle.dump(
save_pose_data,
open(image_dir + os.sep + 'pose_' + str(episode) + '.pkl', 'wb'))
save_pose_data = []
logger.info("Reverse finished ...\n")
def control_backup(self):
v_pallet = self.pallet_position - self.robot_position
distance = np.linalg.norm(v_pallet)
if (distance < BACKUP_DISTANCE):
cmd = Twist()
cmd.linear = Vector3(-BACKUP_SPEED, 0, 0)
cmd.angular = Vector3(0, 0, 0)
self.wheel_cmd_publisher.publish(cmd)
else:
cmd = Twist()
cmd.linear = Vector3(0, 0, 0)
cmd.angular = Vector3(0, 0, 0)
self.wheel_cmd_publisher.publish(cmd)
self.state = CONTROL_STATE_DONE
def coordinate_to_action(self, msg):
x = msg.x
y = msg.y
r = msg.z
depth_proportion = -0.025
depth_intercept = 1.35
depth = r*depth_proportion + depth_intercept
# print depth
y_transform = self.frame_height/2 - y
x_transform = x-self.frame_width/2
angle_diff = math.tan(x_transform/depth)
print "x: ", x_transform
print "y: ",y
print "d: ", depth
print "a: ", angle_diff
twist = Twist()
lin_proportion = 0#-(0.5-depth)*0.1
twist.linear = Vector3(lin_proportion, 0, 0)
turn_proportion = 0*(angle_diff)
twist.angular = Vector3(0, 0, turn_proportion)
self.move_pub.publish(twist.linear, twist.angular)
def _move_tick(self, linear_vector=Vector3(0.0, 0.0, 0.0),
angular_vector=Vector3(0.0, 0.0, 0.0)):
twist_msg = Twist()
twist_msg.linear = linear_vector
twist_msg.angular = angular_vector
self._base_publisher.publish(twist_msg)
def move_base(self, x, y, z):
print "MOVING"
twist_msg = Twist()
twist_msg.linear = Vector3(x, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, z)
self.cmd_vel_pub.publish(twist_msg)
def move_robot(distance):
"""
moves the robot straight on specified distance
"""
global pub
global is_currently_moving
is_currently_moving = True
move_command = Twist()
# no turning
move_command.angular = Vector3(0, 0, 0)
# just moving forward
move_command.linear = Vector3(SPEED, 0, 0)
# moving forward for specified distance
t_0 = rospy.Time.now().to_sec()
current_distance = 0
while current_distance < distance and movement_is_ok():
pub.publish(move_command)
t_1 = rospy.Time.now().to_sec()
current_distance = SPEED*(t_1-t_0)
move_command.linear = Vector3(0,0,0)
pub.publish(move_command)
is_currently_moving = False
def turn_towards_target(self, angle):
current_yaw = get_yaw_from_pose(self.current_pose.pose.pose)
# convert angle to radian value in [-pi, pi]
angle = math.remainder(angle, 2 * np.pi)
destination_yaw = current_yaw + angle
if destination_yaw > np.pi:
destination_yaw -= (2 * np.pi)
elif destination_yaw < -np.pi:
destination_yaw += (2 * np.pi)
r = rospy.Rate(60)
# turn until we face within 1 degree of destination angle
while abs(
get_yaw_from_pose(self.current_pose.pose.pose) -
destination_yaw) > (np.pi / 180):
# turn pi/12 rad/sec
turn_msg = Twist()
turn_vel = np.sign(destination_yaw - get_yaw_from_pose(
self.current_pose.pose.pose)) * (np.pi / 12)
turn_msg.angular = Vector3(0, 0, turn_vel)
self.movement_pub.publish(turn_msg)
r.sleep()
print("{0}/{1}: {2}".format(
str(get_yaw_from_pose(self.current_pose.pose.pose)),
str(destination_yaw),
str(
abs(
get_yaw_from_pose(self.current_pose.pose.pose) -
destination_yaw))))
# Halt
self.movement_pub.publish(Twist())
def turn_towards_target(self, angle):
#wait for first pose
while (not self.seen_first_pose):
time.sleep(1)
current_yaw = get_yaw_from_pose(self.current_pose.pose.pose)
# convert angle to radian value in [-pi, pi]
angle = math.remainder(angle, 2 * np.pi)
destination_yaw = current_yaw + angle
if destination_yaw > np.pi:
destination_yaw = -np.pi + (destination_yaw - np.pi)
r = rospy.Rate(60)
# turn until we face within 1 degree of destination angle
while abs(
get_yaw_from_pose(self.current_pose.pose.pose) -
destination_yaw) > (np.pi / 180):
# turn pi/16 rad/sec
turn_msg = Twist()
turn_vel = np.sign(destination_yaw - get_yaw_from_pose(
self.current_pose.pose.pose)) * (np.pi / 16)
turn_msg.angular = Vector3(0, 0, turn_vel)
self.movement_pub.publish(turn_msg)
r.sleep()
# Halt
self.movement_pub.publish(Twist())
def notifyClientUpdated(self, topic):
#rank the votes and convert to a numpy array
linearCandidates = self.getLinearCandidates()
angularCandidates = self.getAngularCandidates()
linearRankings, angularRankings = rankings = self.calculateRankings(rospy.get_rostime())
#sum the rankings of each candidate
linearBordaCount = np.sum(linearRankings, axis=0)
angularBordaCount = np.sum(angularRankings, axis=0)
#highest ranked candidate
bestLinearCandidateIndex = np.argmax(linearBordaCount)
bestAngularCandidateIndex = np.argmax(angularBordaCount)
bestLinearCandidate = linearCandidates[bestLinearCandidateIndex]
bestAngularCandidate = angularCandidates[bestAngularCandidateIndex]
#publish the best ranked action
rospy.loginfo("aggregator realtime_weighted_borda_full: %s - %s",(bestLinearCandidate,bestAngularCandidate),self.weights)
action=Twist()
if bestLinearCandidate:
action.linear = bestLinearCandidate.linear
if bestAngularCandidate:
action.angular = bestAngularCandidate.angular
self.publish(action)
def talker_direct(self):
self.motor_counter += 1
current_counter = self.motor_counter
pub = rospy.Publisher('lego_robot/cmd_vel_med', Twist, queue_size=10)
max_speed = 800
# running the robot at duty_cycle_sp of 100 gave me a speed of 880 something in the air
# running on the ground it got like 800
publishFrequency = 60
rate = rospy.Rate(publishFrequency) # 60hz
counter = 0
while not rospy.is_shutdown() and not self.stopped:
if self.motor_counter != current_counter:
return
# print(counter)
twist = Twist()
angular = geometry_msgs.msg.Vector3()
linear = geometry_msgs.msg.Vector3()
angular.x = math.radians(max_speed * self.duty_cycle_sp / 100)
# angular.x = kwargs['speed_sp'] * math.pi / 180
counter += 1
twist.angular = angular
twist.linear = linear
# rospy.loginfo(twist)
pub.publish(twist)
rate.sleep()
self.coast(pub, math.radians(max_speed * self.duty_cycle_sp / 100))
def talker_timed(self, kwargs):
self.motor_counter += 1
current_counter = self.motor_counter
pub = rospy.Publisher('lego_robot/cmd_vel_med', Twist, queue_size=10)
publishFrequency = 60
rate = rospy.Rate(publishFrequency) # 60hz
counter = 0
t_end = time.time() + kwargs['time_sp'] / 1000
while not rospy.is_shutdown(
) and not self.stopped and time.time() < t_end:
if self.motor_counter != current_counter:
return
# print(counter)
twist = Twist()
angular = geometry_msgs.msg.Vector3()
linear = geometry_msgs.msg.Vector3()
angular.x = math.radians(kwargs['speed_sp'])
# angular.x = kwargs['speed_sp'] * math.pi / 180
counter += 1
twist.angular = angular
twist.linear = linear
# rospy.loginfo(twist)
pub.publish(twist)
rate.sleep()
self.coast(pub, math.radians(kwargs['speed_sp']))
def coast(self, pub, currentSpeed):
'''
The idea of this function is to simulate coasting.
I've set it so that on coasting, the wheel slows down 0.5 radians ever 0.1 seconds.
So that's 5 radians per second.
'''
coastStep = 0.5
totalSteps = int(math.floor(currentSpeed / coastStep))
current_counter = self.motor_counter
for i in range(0, totalSteps):
if self.motor_counter != current_counter:
return
if i == totalSteps - 1:
currentSpeed = 0
else:
currentSpeed -= coastStep
twist = Twist()
angular = geometry_msgs.msg.Vector3()
linear = geometry_msgs.msg.Vector3()
angular.x = currentSpeed
twist.angular = angular
twist.linear = linear
# rospy.loginfo(twist)
pub.publish(twist)
time.sleep(0.1)
def daemon_worker(self):
try:
cmd_pub = rospy.Publisher(self.config["cmd_vel"],
Twist,
queue_size=10)
rospy.loginfo("Driver Daemon online")
while not rospy.core.is_shutdown():
try:
if self.flag == DriverStatus.PREEMPTING:
cx, cy, cr = self.get_current_loc()
tx, ty, _ = self.target_goal
delta_angle = math.atan2(ty - cy, tx - cx)
delta_distance = math.sqrt((ty - cy)**2 + (tx - cx)**2)
ANG_vel = delta_angle - cr
LIN_vel = delta_distance * self.GAIN_L
# * Fix ANG_vel
if ANG_vel > math.pi:
ANG_vel -= 2 * math.pi
elif ANG_vel < -math.pi:
ANG_vel += 2 * math.pi
ANG_vel *= self.GAIN_A
rospy.logwarn_throttle(
0.2,
"Da={:.3f} :: Dd={:.3f} :: Va={:.3f} :: Vl={:.3f}".
format(delta_angle, delta_distance, ANG_vel,
LIN_vel),
)
msg = Twist()
linear = Vector3()
angular = Vector3()
linear.x = LIN_vel
linear.y = 0
linear.z = ANG_vel
angular.x = 0
angular.y = 0
angular.z = 0
msg.linear = linear
msg.angular = angular
cmd_pub.publish(msg)
except Exception:
rospy.logerr("Error in Driver... Recovering.")
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_exception(exc_type,
exc_value,
exc_traceback,
limit=2,
file=sys.stdout)
rospy.rostime.wallsleep(0.01)
except Exception:
rospy.logfatal("Driver Daemon crashed or halted!")
def positionUpdateCallback(self, position):
# gets called whenever we receive a new position. It will then update the motorcomand
if (not (self.active)):
return #if we are not active we will return imediatly without doing anything
angleX = position.angleX
distance = position.distance
rospy.loginfo('Angle: {}, Distance: {}, '.format(angleX, distance))
# call the PID controller to update it and get new speeds
[uncliped_ang_speed,
uncliped_lin_speed] = self.PID_controller.update([angleX, distance])
# clip these speeds to be less then the maximal speed specified above
angularSpeed = np.clip(-uncliped_ang_speed, -self.max_speed,
self.max_speed)
linearSpeed = np.clip(-uncliped_lin_speed, -self.max_speed,
self.max_speed)
# create the Twist message to send to the cmd_vel topic
velocity = Twist()
velocity.linear = Vector3(linearSpeed, 0, 0.)
velocity.angular = Vector3(0., 0., angularSpeed)
rospy.loginfo('linearSpeed: {}, angularSpeed: {}'.format(
linearSpeed, angularSpeed))
self.cmdVelPublisher.publish(velocity)
def talker_forever(self, kwargs):
self.motor_counter += 1
current_counter = self.motor_counter
pub = rospy.Publisher('lego_robot/cmd_vel_' + self.wheel,
Twist,
queue_size=10)
publishFrequency = 60
rate = rospy.Rate(publishFrequency) # 60hz
counter = 0
while not rospy.is_shutdown() and not self.stopped:
if self.motor_counter != current_counter:
return
# print(counter, self.wheel, self.motor_counter)
twist = Twist()
angular = geometry_msgs.msg.Vector3()
linear = geometry_msgs.msg.Vector3()
angular.x = math.radians(kwargs['speed_sp'])
counter += 1
twist.angular = angular
twist.linear = linear
# rospy.loginfo(twist)
pub.publish(twist)
rate.sleep()
self.coast(pub, math.radians(kwargs['speed_sp']))
def positionUpdateCallback(self, twist):
angleX= twist.angular.z
distance = twist.linear.x
dis_error = self.dis_setPoint - distance
ang_error = self.ang_setPoint - angleX
linearSpeed = -dis_error*self.dis_Kp - self.dis_last_error*self.dis_Kd
angularSpeed = ang_error*self.ang_Kp + self.ang_last_error*self.ang_Kd
if(linearSpeed>self.line_max_speed):
linearSpeed=self.line_max_speed
if(linearSpeed<-self.line_max_speed):
linearSpeed=-self.line_max_speed
if(angularSpeed>self.angular_max_speed):
angularSpeed=self.angular_max_speed
if(angularSpeed<-self.angular_max_speed):
angularSpeed=-self.angular_max_speed
if(angleX==0):
angularSpeed=0.0
if(distance==0):
linearSpeed=0.0
self.dis_last_error = dis_error
self.ang_last_error = ang_error
# create the Twist message to send to the cmd_vel topic
velocity = Twist()
velocity.linear = Vector3(linearSpeed,0,0.)
velocity.angular= Vector3(0., 0.,angularSpeed)
rospy.loginfo('linearSpeed: {}, angularSpeed: {}'.format(linearSpeed, angularSpeed))
self.cmdVelPublisher.publish(velocity)
def coordinate_to_action(self, msg):
x = msg.x
y = msg.y
r = msg.z
depth_proportion = -0.025
depth_intercept = 1.35
depth = r*depth_proportion + depth_intercept
# print depth
y_transform = self.frame_height/2 - y
x_transform = x-self.frame_width/2
angle_diff = x_transform
print angle_diff
if (angle_diff-10) < 0 and (angle_diff + 10) > 0:
angle_diff = 0
if depth-.05<0.5 and depth+.05>0.5:
depth = 0.5
# print "x: ", x_transform
# print "y: ",y
# print "d: ", depth
# print "a: ", angle_diff
twist = Twist()
lin_proportion = -(0.5-depth)*0.07
twist.linear = Vector3(lin_proportion, 0, 0)
turn_proportion = -0.0005*(angle_diff)
twist.angular = Vector3(0, 0, turn_proportion)
self.move_pub.publish(twist.linear, twist.angular)
def move_robot(distance, speed=0.2):
"""
moves the robot straight on specified distance
An imitation of code used in ros wiki
http://wiki.ros.org/turtlesim/Tutorials/Rotating%20Left%20and%20Right
@param distance distance to move in feet
@param speed speed in which to move (default= 0.2)
"""
# convert entered distance to meter
distance = 0.3048 * distance
pub = MovementCommands.__get_publisher()
move_command = Twist()
# no turning
move_command.angular = Vector3(0, 0, 0)
# just moving forward
move_command.linear = Vector3(speed, 0, 0)
# moving forward for specified distance
t_0 = rospy.Time.now().to_sec()
current_distance = 0
while current_distance < distance and MovementCommands.is_cleared_to_go(
):
pub.publish(move_command)
t_1 = rospy.Time.now().to_sec()
current_distance = speed * (t_1 - t_0)
move_command.linear = Vector3(0, 0, 0)
pub.publish(move_command)
pub.unregister()
def doCommand(load_location, drop_location):
waypoints = loadWaypoints(load_location, drop_location)
print "Waypoints loaded: "
print waypoints
currentTargetIndex = 0
rate = rospy.Rate(10)
keepDriving = True
while not rospy.is_shutdown() and keepDriving:
try:
currentWaypoint = waypoints[currentTargetIndex]
currentPosDiff, currentRotDiff = goToLocation(
getWaypointName(currentWaypoint))
if abs(currentPosDiff[0]) < tolerance and abs(
currentPosDiff[1]) < tolerance:
print "Either something broke, or we're there!"
currentTargetIndex += 1
keepDriving = currentTargetIndex < len(waypoints)
except:
print(sys.exc_info())
continue
# continue
message = Twist()
message.linear = Vector3(0, 0, 0)
message.angular = Vector3(0, 0, 0)
zumy_pub.publish(message)
print "We should be stopping."
def getMsgFromTransform(trans, rot):
omega, theta = eqf.quaternion_to_exp(np.array(rot))
omega = np.reshape(omega, (3, 1))
v = eqf.find_v(omega, theta, trans)
twist = np.vstack((v, omega))
linear = Vector3()
computeScale = lambda x, y, z, target: (float(target) /
(x * x + y * y + z * z))**(0.5)
a = computeScale(v[0], v[1], v[2], speed)
print "Scale", a
linear.x = v[0] * a
linear.y = v[1] * a
linear.z = v[2] * a
angular = Vector3()
angular.x = omega[0] * theta / 2
angular.y = omega[1] * theta / 2
angular.z = omega[2] * theta / 2
message = Twist()
message.linear = linear
message.angular = angular
return message
def drive(publisher, x, rz):
"""
moves zumy to the goal
Args:
publisher: zumy publisher
x: desired x velocity
rz: desired z angular velocity
Returns:
"""
twistMessage = Twist()
l = Vector3()
l.x = x
l.y = 0
l.z = 0
twistMessage.linear = l
v = Vector3()
v.x = 0
v.y = 0
v.z = rz
twistMessage.angular = v
# print(twistMessage)
publisher.publish(twistMessage)
def drive_1m():
"""Drive the simulated robot one meter forward"""
# Get the current position of the robot and 1m in front
x = get_current_position()[0]
wanted_x = x + 1
# Set the current node to publish to the cmd_vel topic
speed_publisher = rospy.Publisher("cmd_vel", Twist, queue_size=10)
# Create the node
rospy.init_node("distance_controller", anonymous=True)
# Set the rate to update the topic
update_rate = rospy.Rate(10)
# While ROS is running and the current X is less than the one we want to reach keep driving forward
while not rospy.is_shutdown():
pub_data = Twist()
if x < wanted_x:
pub_data.angular = Vector3(0, 0, 0)
pub_data.linear = Vector3(0.2, 0, 0)
speed_publisher.publish(pub_data)
x = get_current_position()[0]
update_rate.sleep()
# If not stop the robot announce the maneuver is done then break out of the loop
else:
pub_data.linear = Vector3(0, 0, 0)
speed_publisher.publish(pub_data)
print("Maneuver Complete")
break
def positionUpdateCallback(self, position):
if (self.active):
angleX = position.angleX
distance = position.distance
rospy.logwarn('Angle: {}, Distance: {}, '.format(angleX, distance))
#CALLING PID CONTROLLER to update it and get new vectors of velocity
[uncliped_ang_speed, uncliped_lin_speed
] = self.PID_controller.update([angleX, distance])
# clip these speeds to be less then the maximal speed specified above
angularSpeed = np.clip(-uncliped_ang_speed, -self.max_speed,
self.max_speed)
linearSpeed = np.clip(-uncliped_lin_speed, -self.max_speed,
self.max_speed)
# create the Twist message to send to the cmd_vel topic
velocity = Twist()
velocity.linear = Vector3(linearSpeed, 0, 0.)
velocity.angular = Vector3(0., 0., angularSpeed)
rospy.loginfo('linearSpeed: {}, angularSpeed: {}'.format(
linearSpeed, angularSpeed))
self.cmd_vel_Publisher.publish(velocity)
def create_modelstate_message(coords, yaw, model_name='/'):
"""
Create a model state message for husky robot
"""
pose = Pose()
p = Point()
p.x = coords[0]
p.y = coords[1]
p.z = coords[2]
pose.position = p
qua = quaternion_from_euler(0, 0, yaw)
q = Quaternion()
q.x = qua[0]
q.y = qua[1]
q.z = qua[2]
q.w = qua[3]
pose.orientation = q
twist = Twist()
twist.linear = Vector3(0, 0, 0)
twist.angular = Vector3(0, 0, 0)
ms = ModelState()
ms.model_name = model_name
ms.pose = pose
ms.twist = twist
ms.reference_frame = 'sand_mine'
return ms
def drive(self, lin_vel, ang_vel): """Publishes specified linear and angular command velocities""" # Note: for the Neato platforms, only x-linear and z-rotational motion is possible twist = Twist() twist.linear = Vector3(lin_vel,0,0) twist.angular = Vector3(0,0,ang_vel) self.pub.publish(twist.linear, twist.angular)
def center_flag(self, flag):
if not self.is_centered(flag):
print "gotta align"
print "angle is currently " + str(flag.angle)
msg = Twist()
msg.linear = Vector3(0, 0, 0)
msg.angular = Vector3(0, 0, flag.angle)
self.velocity_publisher.publish(msg)
def reverse_robot():
global logger
logger.info("Reversing Robot ...\n")
import time
global vel_lin_buffer,vel_ang_buffer,reversing
global cmd_vel_pub,restored_bump_pub
global curr_cam_data,save_pose_data,image_dir,save_img_seq
reversing = True
logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY)
for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)):
lin_vec = Vector3(-l[0],-l[1],-l[2])
ang_vec = Vector3(-a[0],-a[1],-a[2])
twist_msg = Twist()
twist_msg.linear = lin_vec
twist_msg.angular = ang_vec
rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
cmd_vel_pub.publish(twist_msg)
# publish last twist message so the robot reverse a bit more
for _ in range(5):
rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
cmd_vel_pub.publish(twist_msg)
logger.debug("Finished posting cmd_vel messages backwards ...\n")
rospy.sleep(0.5)
logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY)
for _ in range(100):
rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
twist_msg = Twist()
twist_msg.linear = Vector3(0,0,0)
twist_msg.angular = Vector3(0,0,0)
cmd_vel_pub.publish(twist_msg)
logger.debug("Finished posting zero cmd_vel messages ...\n")
reversing = False
restored_bump_pub.publish(True)
if image_dir is not None:
if save_img_seq:
pickle.dump(curr_cam_data,open(image_dir+os.sep+'images_'+str(episode)+'.pkl','wb'))
curr_cam_data=[]
pickle.dump(save_pose_data,open(image_dir+os.sep+'pose_'+str(episode)+'.pkl','wb'))
save_pose_data=[]
logger.info("Reverse finished ...\n")
def do_GET(self):
global publisher
query_string = urlparse.urlparse(self.path).query
parameters = urlparse.parse_qs(query_string)
if 'type' not in parameters:
try:
if self.path == "/":
self.path = "/index.html"
elif self.path == "favicon.ico":
return
elif self.path == "map.gif":
# Draw robot position on map and send image back
draw_map()
return
fname,ext = os.path.splitext(self.path)
print "Loading file", self.path
with open(os.path.join(os.getcwd(),self.path[1:])) as f:
self.send_response(200)
self.send_header('Content-type', types_map[ext])
self.end_headers()
self.wfile.write(f.read())
return
except IOError:
self.send_error(404)
return
command_type = parameters['type'][0]
if command_type == 'base':
base_x = 0.0
if 'x' in parameters:
base_x = float(parameters['x'][0])
base_y = 0.0
if 'y' in parameters:
base_y = float(parameters['y'][0])
base_z = 0.0
if 'z' in parameters:
base_z = float(parameters['z'][0])
twist_msg = Twist()
twist_msg.linear = Vector3(base_x, base_y, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, base_z)
mobile_base_velocity.publish(twist_msg)
elif command_type == 'speak':
text = parameters['say'][0]
sr = SoundRequest()
sr.sound = -3 #Say text
sr.command = 1 #Play once
sr.arg = text
publisher.publish(sr)
#os.system("espeak -s 155 -a 200 '" + text + "' ")
# response
self.send_response(204)
return
def base_action(self, x, y, z, theta_x, theta_y, theta_z):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(x, y, z)
twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
base_publisher.publish(twist_msg)
def stop_uav(pub): """ Tells the UAV to hold at its current position. """ zero = Vector3(0,0,0) msg = Twist() msg.linear = zero msg.angular = zero pub.publish(msg)
def reverse_robot():
global logger
logger.info("Reversing Robot ...\n")
import time
global vel_lin_buffer,vel_ang_buffer,reversing
global cmd_vel_pub,restored_bump_pub
global curr_cam_data,save_pose_data,image_dir,save_img_seq
reversing = True
logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY)
for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)):
lin_vec = Vector3(-l[0],-l[1],-l[2])
ang_vec = Vector3(-a[0],-a[1],-a[2])
twist_msg = Twist()
twist_msg.linear = lin_vec
twist_msg.angular = ang_vec
rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
cmd_vel_pub.publish(twist_msg)
# publish last twist message so the robot reverse a bit more
for _ in range(5):
rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
cmd_vel_pub.publish(twist_msg)
logger.debug("Finished posting cmd_vel messages backwards ...\n")
rospy.sleep(0.5)
logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY)
for _ in range(100):
rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
twist_msg = Twist()
twist_msg.linear = Vector3(0,0,0)
twist_msg.angular = Vector3(0,0,0)
cmd_vel_pub.publish(twist_msg)
logger.debug("Finished posting zero cmd_vel messages ...\n")
reversing = False
restored_bump_pub.publish(True)
logger.info("Reverse finished ...\n")
#save_img_seq_thread = threading.Thread(target=save_img_sequence_pose_threading())
#save_img_seq_thread.start()
save_img_sequence_pose()
logger.info('Saving images finished...')
def gui_bridge():
#setup ROS node
rospy.init_node('gui_bridge')
pub = rospy.Publisher('/golfcart_pilot/abs_cmd', Twist)
#setup socket
HOST = ''
PORT = 50012
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind((HOST, PORT))
print 'Waiting for connection on port ', PORT
s.listen(1)
conn, addr = s.accept()
print 'Connected by', addr
rospy.sleep(0.5)
while not rospy.is_shutdown():
try:
data = conn.recv(1024)
if data.count("{") > 1:
data = data.split("}{")
data = "{"+data[len(data)-1]
guiCmd = eval(data)
print guiCmd
wheel_angle = float(guiCmd['wheel']) * 0.0174532925 # d2r
speed = float(guiCmd['speed'])
command = Twist()
command.linear = Vector3 (speed,0,0)
command.angular = Vector3 (0,wheel_angle,0)
pub.publish(command)
rospy.sleep(0.2) # 5Hz
except socket.error:
command = Twist()
command.linear = Vector3 (0,0,0)
command.angular = Vector3 (0,0,0)
pub.publish(command)
print 'gui_bridge: SOCKET ERROR OCCURED, STOPPING DRIVE OPERATIONS'
break
s.close()
def send_cmd_msg(self):
msg = Twist()
msg.linear = Vector3()
msg.angular = Vector3()
msg.linear.x = self.x
msg.linear.y = 0
msg.linear.z = 0
msg.angular.x = 0
msg.angular.y = 0
msg.angular.z = self.omega
self.publisher.publish(msg)
def send_cmd_msg(self, speed, omega):
#print('receieved odometry')
msg = Twist()
msg.linear = Vector3()
msg.angular = Vector3()
msg.linear.x = speed
msg.linear.y = 0
msg.linear.z = 0
msg.angular.x = 0
msg.angular.y = 0
msg.angular.z = omega
self.publisher.publish(msg)
def stop_robot():
import time
global cmd_vel_pub
global logger
logger.debug("Posting 0 cmd_vel data ....")
for _ in range(100):
rospy.sleep(0.01)
twist_msg = Twist()
twist_msg.linear = Vector3(0,0,0)
twist_msg.angular = Vector3(0,0,0)
cmd_vel_pub.publish(twist_msg)
logger.debug("Finished posting 0 cmd_vel data ...\n")
def move_base():
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
if(self.direction == Base.FORWARD):
twist_msg.linear = Vector3(0.5, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
elif(self.direction == Base.BACKWARD):
twist_msg.linear = Vector3(-0.5, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
elif(self.direction == Base.LEFT):
twist_msg.linear = Vector3(0.0, 0.5, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
elif(self.direction == Base.RIGHT):
twist_msg.linear = Vector3(0.0, -0.5, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
elif(self.direction == Base.COUNTER):
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.5)
elif(self.direction == Base.CLOCKWISE):
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, -0.5)
base_publisher.publish(twist_msg)
self.gui.show_text_in_rviz("Base!")
def reverse_robot():
global logger
logger.info("Reversing Robot ...\n")
import time
global vel_lin_buffer,vel_ang_buffer,reversing
global cmd_vel_pub,restored_bump_pub
reversing = True
logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY)
for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)):
lin_vec = Vector3(-l[0],-l[1],-l[2])
ang_vec = Vector3(-a[0],-a[1],-a[2])
twist_msg = Twist()
twist_msg.linear = lin_vec
twist_msg.angular = ang_vec
rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
cmd_vel_pub.publish(twist_msg)
# publish last twist message so the robot reverse a bit more
for _ in range(5):
rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
cmd_vel_pub.publish(twist_msg)
logger.debug("Finished posting cmd_vel messages backwards ...\n")
rospy.sleep(0.5)
logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY)
for _ in range(100):
rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
twist_msg = Twist()
twist_msg.linear = Vector3(0,0,0)
twist_msg.angular = Vector3(0,0,0)
cmd_vel_pub.publish(twist_msg)
logger.debug("Finished posting zero cmd_vel messages ...\n")
reversing = False
restored_bump_pub.publish(True)
logger.info("Reverse finished ...\n")
def move_base(self, isForward):
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
distance = 0.25
if not isForward:
distance *= -1
twist_msg = Twist()
twist_msg.linear = Vector3(distance, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
for x in range(0, 15):
rospy.loginfo("Moving the base")
base_publisher.publish(twist_msg)
time.sleep(0.15)
time.sleep(1.5)
def run(self, goal):
rospy.loginfo('Rotating base')
count = 0
r = rospy.Rate(10)
while count < 70:
if self._as.is_preempt_requested():
self._as.set_preempted()
return
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 1.0)
self._base_publisher.publish(twist_msg)
count = count + 1
r.sleep()
self._as.set_succeeded()
def multi_dof_joint_state(cls, msg, obj):
obj.header = decode.header(msg, obj.header, "")
obj.joint_names = msg["joint_names"]
for i in msg['_length_trans']:
trans = Transform()
trans.translation = decode.translation(msg, trans.translation, i)
trans.rotation = decode.rotation(msg, trans.rotation, i)
obj.transforms.append(trans)
for i in msg['_length_twist']:
tw = Twist()
tw.linear = decode.linear(msg, tw.linear, i)
tw.angular = decode.angular(msg, tw.angular, i)
obj.twist.append(twist)
for i in msg['_length_twist']:
wr = Wrench()
wr.force = decode.force(msg, wr.force, i)
wr.torque = decode.torque(msg, wr.torque, i)
obj.wrench.append(wr)
return(obj)
def notifyClientUpdated(self, topic):
#rank the votes and convert to a numpy array
linearCandidates = self.getLinearCandidates()
angularCandidates = self.getAngularCandidates()
linearRankings, angularRankings = self.calculateRankings(rospy.get_rostime())
#calculate Kemeny-Young rank aggregation
linear_min_dist, linear_best_rank = rankaggr_brute(linearRankings)
angular_min_dist, angular_best_rank = rankaggr_brute(angularRankings)
#publish the best ranked action
action=Twist()
if linear_best_rank:
action.linear = linearCandidates[linear_best_rank.index(0)].linear
if angular_best_rank:
action.angular = angularCandidates[angular_best_rank.index(0)].angular
rospy.loginfo("aggregator realtime_kemeny_full: {\nlinear: %s\nangular: %s\n}",linear_min_dist, angular_min_dist)
self.publish(action)
def command_drive(data): # initialize the message components header = Header() foo = TwistWithCovarianceStamped() bar = TwistWithCovariance() baz = Twist() linear = Vector3() angular = Vector3() # get some data to publish # fake covariance until we know better covariance = [1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0, 0,0,0,1,0,0, 0,0,0,0,1,0, 0,0,0,0,0,1] # to be overwritten when we decide what we want to go where linear.x = data.axes[1] * 15 linear.y = 0 linear.z = 0 angular.x = 0 angular.y = 0 angular.z = data.axes[0] * 10 # put it all together # Twist baz.linear = linear baz.angular = angular # TwistWithCovariance bar.covariance = covariance bar.twist = baz # Header header.seq = data.header.seq header.frame_id = frame_id header.stamp = stopwatch.now() # seq = seq + 1 # TwistWithCovarianceStamped foo.header = header foo.twist = bar # publish and log rospy.loginfo(foo) pub.publish(foo)
def spin_base(self, rotate_count):
# Adjust height and tuck arms before localization
self._sound_client.say("Please wait while I orient myself.")
self.torso.move(.15)
self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS
self.move_arm('l', 5.0, True)
self.move_arm('r', 5.0, True)
self.l_gripper.close_gripper()
self.r_gripper.close_gripper()
if not rotate_count:
rotate_count = 2
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.0, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.5)
start_time = rospy.get_rostime()
while rospy.get_rostime() < start_time + rospy.Duration(15.0 * rotate_count):
base_publisher.publish(twist_msg)
def move_uav(pub,curState,curGoal):
"""
Tells the UAV to move toward the current goal pose.
Orientation is currently ignored, but at some point we will want to use this
to control UAV yaw as well.
"""
# Calculate the direction we want to move in
diff = curGoal.position-curState.position
dist = np.linalg.norm(diff)
direction = diff/dist
# Calculate the speed we want to move at, and use this to set
# flight vector (direction*speed)
speed = min(max(dist*speedScale,minSpeed),maxSpeed)
flightVec = direction*speed
str = "speed parameters: (min: %f max: %f scale: %f" % \
(minSpeed,maxSpeed,speedScale)
rospy.logdebug(str)
rospy.logdebug("position: %s" % curState.position)
rospy.logdebug("target: %s" % curGoal.position)
rospy.logdebug("dist: %s" % dist)
rospy.logdebug("speed: %s" % speed)
rospy.logdebug("flightVec: %s" % flightVec)
# publish Twist message to tell UAV to move according to the
# flight vector
msg = Twist()
msg.angular = Vector3(0,0,0)
msg.linear = Vector3()
msg.linear.x = flightVec[0]
msg.linear.y = flightVec[1]
msg.linear.z = flightVec[2]
pub.publish(msg)
#!/usr/bin/env python
import roslib
roslib.load_manifest('rospy')
roslib.load_manifest('geometry_msgs')
import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3
if __name__ == "__main__":
rospy.init_node('base_test_node', anonymous=True)
topic_name = '/base_controller/command'
base_publisher = rospy.Publisher(topic_name, Twist)
twist_msg = Twist()
twist_msg.linear = Vector3(0.0, 0.1, 0.0)
twist_msg.angular = Vector3(0.0, 0.0, 0.0)
for i in range(100):
base_publisher.publish(twist_msg)
from nav_msgs.msg import Odometry
rospy.init_node('drives_square')
#def process_odom(msg):
# xdis =
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
#sub = rospy.Subscriber("/odom", Odometry, process_odom)
twist = Twist()
l0 = Vector3(0.0, 0.0, 0.0)
a0 = Vector3(0.0, 0.0, 0.0)
twist.linear = l0
twist.angular = a0
tl= 3.66 #time to 1 meter
ta= 1.7 #time to rotate 90 degrees
itime = rospy.get_time()
while not rospy.is_shutdown():
seconds = rospy.get_time() - itime
if seconds > 0 and seconds < tl:
l0 = Vector3(0.5, 0.0, 0.0)
a0 = Vector3(0.0, 0.0, 0.0)
elif seconds > tl and seconds < tl+ta:
l0 = Vector3(0.0, 0.0, 0.0)
a0 = Vector3(0.0, 0.0, -1.0)
elif seconds > tl+ta and seconds < 2*tl + ta:
l0 = Vector3(0.5, 0.0, 0.0)