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 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 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 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 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 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 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 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 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 default(self, ci='unused'):
twist = Twist()
twist.header = self.get_ros_header()
# Fill twist-msg with the values from the sensor
twist.linear.x = self.data['velocity'][0]
twist.linear.y = self.data['velocity'][1]
twist.linear.z = self.data['velocity'][2]
self.publish(twist)
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 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 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 get_motion(self): t = Twist() buf = StringIO() self.motion.serialize(buf) t.deserialize(buf.getvalue()) if DONT_BUMP: if any(self.impact): self.backingsince = time.time() self.motion.linear.x = 0 if self.backingsince is not None: if time.time() < self.backingsince + BUMP_REV_TIME: t.linear.x = -BUMP_REV_SPEED else: self.backingsince = None return t
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 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 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)
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 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 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 turn_left(self):
self.move(Twist(Vector3(0, 0, 0), Vector3(0, 0, 0.5)))
def turn_right(self):
self.move(Twist(Vector3(0, 0, 0), Vector3(0, 0, -0.5)))
def Robot_Stop(self):
twist = Twist()
twist.linear.x = 0
twist.angular.z = 0
self.pub_vel.publish(twist)
def callback(self, msg):
if not self.enabled:
return
data = msg.data
twist = Twist()
numOfFaces = data[1]
#twist.linear.x = 4*data.axes[1]
#twist.angular.z = 4*data.axes[0]
if numOfFaces == 1:
width = data[8]
height = data[9]
faceCenter_x = data[6] + data[8] / 2
faceCenter_y = data[7] + data[9] / 2
centerConst = data[2] / 15
left_x = data[2] / 2 - centerConst
right_x = data[2] / 2 + centerConst
if faceCenter_x < left_x:
twist.linear.x = 0.05
twist.angular.z = abs(faceCenter_x - left_x) * 0.01
self.result_pub.publish(twist)
elif faceCenter_x > right_x:
twist.linear.x = 0.05
twist.angular.z = -abs(faceCenter_x - right_x) * 0.01
self.result_pub.publish(twist)
else:
#self.result_pub.publish(twist)
if (width * height < 4000):
twist.linear.x = 0.1
self.result_pub.publish(twist)
elif (width * height > 7000):
twist.linear.x = -abs(width * height - 7000) * 0.0001
self.result_pub.publish(twist)
elif data[7] < height * 0.4:
twist.linear.x = -0.1
self.result_pub.publish(twist)
else:
self.result_pub.publish(twist)
self.enabled = False
msg = String()
msg.data = "photo"
self.trigger_pub.publish(msg)
elif numOfFaces > 1:
meanFaceCenter_x = 0.0
leftMost = data[2]
rightMost = 0
for i in range(numOfFaces):
thisCenter = data[6 + 6 * i] + data[8 + 6 * i] / 2.0
meanFaceCenter_x += thisCenter / numOfFaces
if thisCenter > rightMost:
rightMost = thisCenter
if thisCenter < leftMost:
leftMost = thisCenter
centerConst = data[2] / 8
left_x = data[2] / 2 - centerConst
right_x = data[2] / 2 + centerConst
if meanFaceCenter_x < left_x:
twist.linear.x = 0.05
twist.angular.z = 3
self.result_pub.publish(twist)
elif meanFaceCenter_x > right_x:
twist.linear.x = 0.05
twist.angular.z = -3
self.result_pub.publish(twist)
else:
if (leftMost > (data[2] / 5) and rightMost <
(4 * data[2] / 5)):
#cuvaj
twist.linear.x = -0.1
else:
twist.linear.x = 0.0
self.result_pub.publish(twist)
self.enabled = False
msg = String()
msg.data = "photo"
self.trigger_pub.publish(msg)
else:
self.result_pub.publish(twist)
leader_cmd_pose_enu_pub = rospy.Publisher("/xtdrone/leader/cmd_pose_enu",
Pose,
queue_size=10)
if control_type == 'vel':
leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu",
Twist,
queue_size=10)
else:
leader_cmd_accel_flu_pub = rospy.Publisher(
"/xtdrone/leader/cmd_accel_flu", Twist, queue_size=10)
leader_cmd_pub = rospy.Publisher("/xtdrone/leader_cmd",
String,
queue_size=10)
cmd = String()
pose = Pose()
twist = Twist()
forward = 0.0
leftward = 0.0
upward = 0.0
angular = 0.0
print_msg()
while (1):
key = getKey()
if key == 'w':
forward = forward + LINEAR_STEP_SIZE
print_msg()
if control_type == 'vel':
print(
def __init__(self, master, title="Drone Control Panel"):
frame = Frame(master, bg="yellow")
master.title(title)
frame.pack()
self.my_frame = frame
# standard velocity messages
self.up_vel = Twist()
self.up_vel.linear.z = 0.1
self.dn_vel = Twist()
self.dn_vel.linear.z = -0.1
self.fw_vel = Twist()
self.fw_vel.linear.x = 0.1
self.rv_vel = Twist()
self.rv_vel.linear.x = -0.1
self.lf_vel = Twist()
self.lf_vel.linear.y = 0.1
self.rt_vel = Twist()
self.rt_vel.linear.y = -0.1
# yaw messages
self.yaw_l = Twist()
self.yaw_l.angular.z = 0.1
self.yaw_r = Twist()
self.yaw_r.angular.z = -0.1
# quit button
self.quit_button = Button(frame, text="Quit", command=frame.quit)
self.quit_button.grid(row=0, column=4)
# velocity buttons
self.up = Button(frame,
text='Up',
command=lambda: self.vel_pub.publish(self.up_vel))
self.up.grid(row=1, column=3, padx=10, pady=10)
self.dn = Button(frame,
text='Down',
command=lambda: self.vel_pub.publish(self.dn_vel))
self.dn.grid(row=3, column=3, padx=10, pady=10)
self.fwd = Button(frame,
text='FWD',
command=lambda: self.vel_pub.publish(self.fw_vel))
self.fwd.grid(row=1, column=1, padx=10, pady=10)
self.back = Button(frame,
text='BACK',
command=lambda: self.vel_pub.publish(self.rv_vel))
self.back.grid(row=3, column=1, padx=10, pady=10)
self.left = Button(frame,
text='LEFT',
command=lambda: self.vel_pub.publish(self.lf_vel))
self.left.grid(row=2, column=0, padx=10, pady=10)
self.right = Button(frame,
text='RIGHT',
command=lambda: self.vel_pub.publish(self.rt_vel))
self.right.grid(row=2, column=2, padx=10, pady=10)
self.rightyaw = Button(
frame,
text='R TURN',
command=lambda: self.vel_pub.publish(self.yaw_r))
self.rightyaw.grid(row=1, column=2, padx=10, pady=10)
self.leftyaw = Button(frame,
text='L TURN',
command=lambda: self.vel_pub.publish(self.yaw_l))
self.leftyaw.grid(row=1, column=0, padx=10, pady=10)
# stop buttons
self.stop1 = Button(frame,
text='STOP',
bg="red",
command=self.stop_btn)
self.stop1.grid(row=2, column=1, padx=10, pady=10)
self.stop2 = Button(frame,
text='STOP',
bg="red",
command=self.stop_btn)
self.stop2.grid(row=2, column=3, padx=10, pady=10)
# landing and takeoff
self.land = Button(frame,
text='Land',
bg="red",
command=lambda: self.land_pub.publish(Empty()))
self.land.grid(row=4, column=2, padx=10, pady=10)
self.takeoff = Button(
frame,
text='Take off',
bg="green",
command=lambda: self.takeoff_pub.publish(Empty()))
self.takeoff.grid(row=4, column=0, padx=10, pady=10)
self.rst = Button(frame,
text='Reset',
bg="blue",
fg="white",
command=lambda: self.reset_pub.publish(Empty()))
self.rst.grid(row=4, column=1, padx=10, pady=10)
self.msg_pub = rospy.Publisher('monitor/status_msg', String)
self.vel_pub = rospy.Publisher('cmd_vel', Twist)
self.land_pub = rospy.Publisher('ardrone/land', Empty)
self.reset_pub = rospy.Publisher('ardrone/reset', Empty)
self.takeoff_pub = rospy.Publisher('ardrone/takeoff', Empty)
if rospy.has_param(name):
return rospy.get_param(name)
else:
print "parameter [%s] not defined. Defaulting to %.3f" % (name,
default)
return default
if __name__ == '__main__':
rospy.init_node('wander_fol')
global command, followDistance, stopDistance, max_speed, min_speed, move
g_last_twist_send_time = rospy.Time.now()
g_twist_pub = rospy.Publisher('cmd_vel_mux/input/teleop',
Twist,
queue_size=1)
g_target_twist = Twist() # initializes to zero
g_last_twist = Twist()
g_vel_scales[0] = fetch_param('~angular_scale', 0.6)
g_vel_scales[1] = fetch_param('~linear_scale', 0.8)
g_vel_ramps[0] = fetch_param('~angular_accel', 1.0)
g_vel_ramps[1] = fetch_param('~linear_accel', 1.0)
move = False
#followDistance=0.8
stopDistance = 0.6
max_speed = 0.5
min_speed = 0.01
scan_sub = rospy.Subscriber('scan', LaserScan, scan_callback)
cmd_vel_pub = rospy.Publisher('cmd_vel_mux/input/teleop',
Twist,
queue_size=1)
def brutestop():
# print("BRUTESTOP")
ob1.linear.x = 0
ob1.linear.y = 0
ob1.linear.z = 0
ob1.angular.x = 0
ob1.angular.y = 0
ob1.angular.z = 0
pub.publish(ob1)
dist_arr = []
ob1 = Twist()
mag = 0.0
dir = 0.0
def callback_lidar(msg):
global mag, dir, dist_arr
h_comp = 0.0
v_comp = 0.0
dist_arr = msg.ranges
for i in range(11):
if dist_arr[i] == float('inf'):
def findWall(self):
twist = Twist()
twist.linear.x = 0.2
twist.angular.z = 0
return twist
def receiveInputGlobal(self, empty): pose = [self.robotPTAMPose.position.x, self.robotPTAMPose.position.y, tan(euler_from_quaternion([ self.robotPTAMPose.orientation.x, self.robotPTAMPose.orientation.y, self.robotPTAMPose.orientation.z, self.robotPTAMPose.orientation.w])[2]),0] distance = linalg.norm(array([self.robotPTAMPose.position.x, self.robotPTAMPose.position.y]) - array([self.points[0], self.points[1]])) # distance2 = linalg.norm(array([self.goal[0], self.goal[1]]) # - array([self.points[0], self.points[1]])) # self.points[-1] = 1.2*self.tf*distance1/(distance1+distance2) goal = self.goal[:] my_points = self.points[:] print goal print my_points # if(self.map==1 or self.map==2): # if pose[0]>=4: # points_done = 1 # goal[-1]=14.0 # my_points = [] # else: # if pose[1]>=4: # my_points[-1]=7.0 # goal[-1]=21.0 # ps = PoseStamped() # ps.header.stamp = rospy.Time.now() # ps.header.frame_id="world" # if my_points==[]: # ps.pose.position.x = goal[0] # ps.pose.position.y = goal[1] # q = quaternion_from_euler(0,0,arctan(goal[2])) # else: # ps.pose.position.x = my_points[0] # ps.pose.position.y = my_points[1] # q = quaternion_from_euler(0,0,arctan(my_points[2])) # ps.pose.orientation.w = q[3] # ps.pose.orientation.x = q[0] # ps.pose.orientation.y = q[1] # ps.pose.orientation.z = q[2] # self.pose_pub.publish(ps) # elif self.map==4: # if pose[0] < -1.5: # goal = goal[0:1] # my_points = my_points[0:1] # points_done = 0 # print 'if' # elif pose[0] >=-1.5 and pose[0] < 1.5: # goal = goal[-1:] # my_points = my_points[-1:] # points_done = 0 # print 'elif1' # elif pose[0] >= 1.5: # goal = goal[-1:] # goal[0][-1]=14.0 # my_points = my_points[-1:] # points_done = 1 # print 'elif2' # print goal # print my_points points = [pose] + my_points+ goal print points, len(points) # if pose[1]<=4: # points_done = 3 # else: # points_done = 2 # else: # if pose[1]>=4: # points_done = 1 # else: # points_done = 0 status = String() self.state = 1 inp = [pose[0],pose[1],pose[2], goal[0],goal[1],goal[2], 0.0,0.0,0.0,0.0,0.0,0.0] # print inp # print points[1:-1] status.data = "BUSY" self.status_pub.publish(status) to = self.to tf = goal[-1] print tf # if points[1:-1]==[]: # coeffs = get_coeffs(inp,to,tf) # else: # coeffs = get_coeffs_points(inp,to,tf,self.points) coeffs = get_coeffs(inp,to,tf,my_points) self.status_pub.publish(status) t=to self.status_pub.publish(status) self.state = 1 r = rospy.Rate(10) cmd = Twist() self.GlobalPath.points = [] while t<tf: point1,kt1 = getPos(coeffs,to,tf,t,pose[1]) point1.x, point1.y, point1.z = point1.z, point1.x, 0.0 self.GlobalPath.points.append(point1) t = t + 0.1 t=to self.global_path_pub.publish(self.GlobalPath) while t<tf and self.state==1: self.global_path_pub.publish(self.GlobalPath) dx,dy,dk = getVel(coeffs,to,tf,t) point1,kt1 = getPos(coeffs,to,tf,t,pose[1]) point2,kt2 = getPos(coeffs,to,tf,min(t+1,tf),pose[1]) message = Float32MultiArray() message.data= [ 0.0,0.0,0.0, point2.z - point1.z, point2.x - point1.x, tan(arctan(kt2) - arctan(kt1)), 0.0,0.0,0.0,0.0,0.0,0.0,0.0] #print self.robotPTAMPose.position.x, self.robotPTAMPose.position.y self.status_pub.publish(status) yaw = euler_from_quaternion([ self.robotPTAMPose.orientation.x, self.robotPTAMPose.orientation.y, self.robotPTAMPose.orientation.z, self.robotPTAMPose.orientation.w])[2] if fabs(yaw) < pi/2.0: cmd.linear.x = sign(dx) * sqrt(dx**2 + dy**2) elif fabs(yaw) > pi/2.0: cmd.linear.x = -sign(dx) * sqrt(dx**2 + dy**2) else: cmd.linear.x = sign(yaw)*sign(dy) * sqrt(dx**2 + dy**2) cmd.angular.z = dk message.data[-1] = sign(cmd.linear.x) self.inp_pub.publish(message) lookahead = [ point1.z, point1.x, kt1, point2.z, point2.x, kt2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0] traj = array(self.trajjer(lookahead+[0,1,1])) # traj = traj.T # traj[0], traj[1] = traj[1], traj[0] # traj = traj.T self.lookaheadPath.points = traj.tolist() #self.lookaheadPath.pose = self.robotPTAMPose self.vel_pub.publish(cmd) r.sleep() t = t + 0.1 self.status_pub.publish(status) self.vel_pub.publish(Twist()) self.vel_pub.publish(Twist()) self.vel_pub.publish(Twist()) status.data = "DONE1" rospy.Rate(2).sleep() self.status_pub.publish(status)
def main():
rospy.init_node('Node', anonymous=True)
rate = rospy.Rate(10)
#global variables
global robot_target
global robot_locations
global main_rate
global beccontrol_default
global L
global s_star
global w
global y_hat
global start_time
global U
global beta
global pub_u_value
global color_103
global color_104
global n_robots
global last_velocity_command
global pub_velocity_command
#Subscribe to topics
rospy.Subscriber('/vicon/ORANGE/ORANGE', TransformStamped,
callback_position_104)
rospy.Subscriber('/vicon/PINK/PINK', TransformStamped,
callback_position_103)
#All-to-all
x_pink = robot_locations[robot_names.index('103')].linear.x - 3.53
x_orange = robot_locations[robot_names.index('104')].linear.x - 3.53
x_locations = list()
x_locations = [float(x_pink), float(x_orange)]
state = [0, 0]
#Determines if a state change based on location is allowed
state_change = [True, True]
# Keep node going until it is stopped
rate = rospy.Rate(main_rate) # 10hz
while not rospy.is_shutdown():
#All-to-all
x_pink = robot_locations[robot_names.index('103')].linear.x - 3.53
x_orange = robot_locations[robot_names.index('104')].linear.x - 3.53
s_star = (x_pink + x_orange) / (2 * 2)
y_hat = [
[x_pink],
[x_orange],
]
#x-locations after offset
x_locations = [float(x_pink), float(x_orange)]
#Read values of U from light intensity. U is automatically updated
read_u()
print(U)
print(state)
print((x_locations))
for i in range(0, n_robots):
#Depending on u, decide which state to be in if this is the start
if ((absol(x_locations[i]) > 1.5) and (U[i] > 1)):
#State to circle after decision is made
state[i] = 3
elif ((U[i] > 1) and (state_change[i] == True)):
#State to decide
state[i] = 2
elif ((U[i] < 1) and (absol(x_locations[i]) > 1.4)):
state[i] = 2
state_change[i] = True
elif ((U[i] < 1) and (absol(x_locations[i]) < 0.3)):
#State to circle at center
state[i] = 1
state_change[i] = True
#Decision has not been made at the start
if (state[i] == 1):
#Circle the center
#Set speeds to circle
last_velocity_command[i].linear.x = 0.33
last_velocity_command[i].angular.z = .7
pub_velocity_command[i].publish(last_velocity_command[i])
#Decision making
elif (state[i] == 2):
#output robot commands for solving equation
last_velocity_command[i] = waypoint_commands(
robot_locations, last_velocity_command, i)
#Reorientation code for setting angular speed
reorient(i)
pub_velocity_command[i].publish(last_velocity_command[i])
elif (state[i] == 3):
#Circle the decision made
#Set speeds to circle
last_velocity_command[i].linear.x = -0.33
last_velocity_command[i].angular.z = .7
pub_velocity_command[i].publish(last_velocity_command[i])
state_change[i] = False
U_pubber = Twist()
U_pubber.linear.x = U[robot_names.index('103')]
U_pubber.linear.y = U[robot_names.index('104')]
pub_u_value.publish(U_pubber)
rate.sleep()
def waypoint_commands(current_location_all, last_command_all, robot_i):
#Make local copies of variables
current_location = current_location_all[robot_i]
last_command = last_command_all[robot_i]
velocity_command = Twist()
global main_rate
global n_robots
global beccontrol_default
global L
global s_star
global w
global y_hat
global start_time
global U
global beta
#Control Law Settings
v_linear_max = .35 #max linear velocity
v_angular_max = 3 #max angular velocity
a_linear_max = 0.25
a_angular_max = 1.0
#x-offset (all)
x_pink = current_location_all[robot_names.index('103')].linear.x - 3.53
x_orange = current_location_all[robot_names.index('104')].linear.x - 3.53
x_vector = [
[x_pink],
[x_orange],
]
#Control
kp = 5
#y-offset (robot in question)
if robot_i == robot_names.index('103'):
current_location.linear.y = current_location.linear.y - 1.98
else:
current_location.linear.y = current_location.linear.y - 3.04
#Consensus (change alpha)
alpha = 0.5
w = w + (-alpha * np.sign(np.dot(L, y_hat))) / main_rate
y_hat = (np.dot(L, w) + x_vector) / main_rate
#Calculate horizontal velocity
epsilon = 0.01
threshold = 2
if robot_i == robot_names.index('103'):
velocity_command.linear.x = (
-1 * x_pink + U[robot_names.index('103')] * (math.tanh(x_orange)) +
beta[robot_names.index('103')]) / (main_rate)
else:
velocity_command.linear.x = (
-1 * x_orange + U[robot_names.index('104')] *
(math.tanh(x_pink)) + beta[robot_names.index('104')]) / (main_rate)
#---------------End Control Law----------------------------
#Limit Positive Acceleration
if velocity_command.linear.x > last_command.linear.x + a_linear_max / float(
main_rate):
velocity_command.linear.x = last_command.linear.x + a_linear_max / float(
main_rate)
#Check max velocities
velocity_command.linear.x = min(absol(velocity_command.linear.x),
v_linear_max) * np.sign(
velocity_command.linear.x)
velocity_command.angular.z = min(absol(velocity_command.angular.z),
v_angular_max) * np.sign(
velocity_command.angular.z)
#Set appropriate gains
velocity_command.linear.x = 2 * velocity_command.linear.x
#Take into consideration which direction the robot is facing
#if ((abs(current_location_all[robot_i].angular.z) > math.pi/2) and (abs(current_location_all[robot_i].angular.z) < math.pi)):
# velocity_command.linear.x = -velocity_command.linear.x
return velocity_command
def turnLeft(self):
twist = Twist()
twist.linear.x = 0
twist.angular.z = 0.2
return twist
def take_action(self):
msg = Twist()
linear_x = 0
angular_z = 0
regions = self.regions
#print regions
state_description = ''
front_limit = 0.7
side_limit = 0.7
print 'found', self.target_found, 'location', self.object_location
if regions['front'] > front_limit and regions['fleft'] > side_limit and regions['fright'] > side_limit:
state_description = 'case 1 - nothing'
#linear_x = 0.6
angular_z = 0
self.follow()
return
elif regions['front'] < front_limit and regions['fleft'] > side_limit and regions['fright'] > side_limit:
state_description = 'case 2 - front'
linear_x = 0
angular_z = 0.0
elif regions['front'] > front_limit and regions['fleft'] > side_limit and regions['fright'] < side_limit:
state_description = 'case 3 - fright'
linear_x = 0
if self.target_found and self.object_location > 0: #on the right
angular_z = -0.3
else:
angular_z = 0.3
#pid('right',regions)
elif regions['front'] > front_limit and regions['fleft'] < side_limit and regions['fright'] > side_limit:
state_description = 'case 4 - fleft'
linear_x = 0
if self.target_found and self.object_location > 0: #on the right
angular_z = -0.3
else:
angular_z = 0.3
#self.pid('left')
elif regions['front'] < front_limit and regions['fleft'] > side_limit and regions['fright'] < side_limit:
state_description = 'case 5 - front and fright'
linear_x = 0
#if self.object_location > 0: #on the right
# angular_z = -0.3
#else:
# angular_z = 0.3
elif regions['front'] < front_limit and regions['fleft'] < side_limit and regions['fright'] > side_limit:
state_description = 'case 6 - front and fleft'
linear_x = 0
#if self.object_location > 0: #on the right
# angular_z = 0.3
#else:
# angular_z = -0.3
vel, rot = self.pid('left')
linear_x = vel
angular_z = rot
return
elif regions['front'] < front_limit and regions['fleft'] < side_limit and regions['fright'] < side_limit:
state_description = 'case 7 - front and fleft and fright'
if regions['front'] < 0.5:
linear_x = -0.1
angular_z = -0.0
state_description+=' - To close!'
else:
linear_x = 0.1
angular_z = 0.0
elif regions['front'] > front_limit and regions['fleft'] < side_limit and regions['fright'] < side_limit:
state_description = 'case 8 - fleft and fright'
linear_x = 0.3
angular_z = 0
else:
state_description = 'unknown case'
rospy.loginfo(regions)
rospy.loginfo(state_description)
if not self.target_found:
self.drive(0,0)
rospy.loginfo('target lost!')
return
msg.linear.x = linear_x
msg.angular.z = angular_z
#pub_.publish(msg)
self.drive(linear_x,angular_z)
def move(x, z):
move_cmd = Twist()
move_cmd.linear.x = x
move_cmd.angular.z = z
NaviPub.publish(move_cmd)
def get_ctrl_output(self):
# get the location of the robot
try:
(translation, rotation) = self.trans_listener.lookupTransform(
"/map", "base_footprint", rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(rotation)
self.x = translation[0]
self.y = translation[1]
self.th = euler[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
translation = (0, 0, 0)
rotation = (0, 0, 0, 1)
euler = tf.transformations.euler_from_quaternion(rotation)
self.x = translation[0]
self.y = translation[1]
self.th = euler[2]
# Define Controller Gains
k1 = 0.5
k2 = 0.5
k3 = 0.5
# Distance to final point in current path
rho = np.linalg.norm(
np.asarray(self.x_goal[0:2]) - np.asarray([self.x, self.y]))
# Define relevant control parameters
alpha = wrapToPi(
np.arctan2(self.y_g - self.y, self.x_g - self.x) - self.th)
delta = wrapToPi(alpha + self.th - self.th_g)
#Define control inputs (V,om) - without saturation constraints
if self.Mode == 0: #'velocity_control'
rospy.loginfo("Velocity Mode")
V = self.V
om = self.om
else:
rospy.loginfo("Position Mode")
V = k1 * rho * np.cos(alpha)
om = k2 * alpha + k1 * np.sinc(
alpha / np.pi) * np.cos(alpha) * (alpha + k3 * delta)
# Check whether we're close to the goal, and override control inputs to force a stop
# at the target position
rospy.logwarn('Mission State is {}'.format(self.missionState))
if rho < self.STOP_THRESHOLD or self.missionState == self.MissionStates.END_OF_MISSION:
rospy.logwarn('Stopping Robot')
if self.missionState == self.MissionStates.END_OF_MISSION:
rospy.signal_shutdown(
'End of Mission. Shutting down controller.')
V = 0
om = 0
# Apply saturation limits
V = np.sign(V) * min(0.3, np.abs(V))
om = np.sign(om) * min(1.0, np.abs(om))
cmd_x_dot = V # forward velocity
cmd_theta_dot = om
cmd = Twist()
cmd.linear.x = cmd_x_dot
cmd.angular.z = cmd_theta_dot
return cmd
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
vel_msg = Twist()
pose_msg = Pose()
vel_pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10)
def spiral(speed):
sr = 0
r = rospy.Rate(speed)
while (pose_msg.x < 9) and (pose_msg.y < 9):
sr += 0.5
vel_msg.linear.x = sr
vel_msg.angular.z = speed
vel_pub.publish(vel_msg)
r.sleep()
vel_msg.linear.x = 0
vel_msg.angular.y = 0
vel_pub.publish(vel_msg)
def poseCallback(msg):
pose_msg.x = msg.x
pose_msg.y = msg.y
pose_msg.theta = msg.theta
q1 = msg.pose.pose.orientation.x
q2 = msg.pose.pose.orientation.y
q3 = msg.pose.pose.orientation.z
th = math.atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3))*180/math.pi
vx = msg.twist.twist.linear.x
vy = msg.twist.twist.linear.y
vth = msg.twist.twist.angular.z
if __name__ == '__main__':
rospy.init_node('go_to_goal')
twist_pub = rospy.Publisher('RosAria/cmd_vel', Twist, queue_size=1)
rospy.Subscriber('/RosAria/pose', Odometry, odom_cb, twist_pub)
desired_pose = Odometry()
desired_twist = Twist()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
th1 = th
distance = math.sqrt((x0-x)*(x0-x) + (y0-y)*(y0-y))
if (distance<0.075):
th2 = th0
else:
th2 = math.atan2(y0-y, x0-x)*180/math.pi
e_th = th2-th1
if (e_th>180):
e_th = e_th-360
def shark(self):
xc = self.x_bot1
yc = self.y_bot1
theta_c = math.atan2(yc, xc)
xs = self.x_bot2
ys = self.y_bot2
theta_s = math.atan2(ys, xs)
xloc = self.x_bot2 - self.x_bot1
yloc = self.y_bot2 - self.y_bot1
theta_loc = math.atan2(yloc, xloc)
print "position_xc = " + str(xc) + "position_yc = " + str(yc)
print "theta_c = " + str(theta_c) + "theta_s = " + str(theta_s)
print "theta loc " + str(theta_loc)
error_angle = theta_c - theta_s
P = 10 * error_angle
twist = Twist()
if P > 4:
P = 4
if P < -4:
P = -4
twist.linear.y = P
twist.angular.z = P
if np.sign(theta_c) == -1 and np.sign(
theta_s) == 1 and theta_s > np.pi / 2:
twist.linear.y = 4
twist.angular.z = 4
if np.sign(theta_c) == 1 and np.sign(
theta_s) == -1 and theta_s < -np.pi / 2:
twist.linear.y = -4
twist.angular.z = -4
print "error angle = " + str(P)
print "error sum angles = " + str(
np.absolute(theta_s) + np.absolute(theta_c))
hyp = math.hypot(ys, xs)
error_position = 1 - hyp
P = 10 * error_position
twist.linear.x = P
if math.fabs(error_angle) < 0.1:
twist.angular.z = 0
self.pub0.publish(twist)
print "error radio = " + str(P)
print "error radio = " + str(error_position)
print(self.orientationz * 180 / math.pi)
print(hyp)
def stop_btn(self):
rospy.loginfo("Stop button pressed")
# send zero velocity
self.vel_pub.publish(Twist())
def move_forward(self):
self.move(Twist(Vector3(0.5, 0, 0), Vector3(0, 0, 0)))
def OdomCallback(self, robot_state):
'''quaternion = (robot_state.pose.pose.orientation.x, robot_state.pose.pose.orientation.y, robot_state.pose.pose.orientation.z, robot_state.pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
print (euler[2])
'''
if self.safety_msg.flag == False and self.process_begin == False:
return
#initial_point e final_point devem ser parametros de acordo com a informacao recebida do lidar
if not self.process_begin:
return
if self.safety_msg.found_line and self.deviationparams.status == 0: #!!!
self.process_begin = False
self.iz = 0
return
v_max = 5.0
w_max = 0.5
goal_x = self.traj_x[self.iz]
goal_y = self.traj_y[self.iz]
# transform from quaternion for yaw degrees
quaternion = (robot_state.pose.pose.orientation.x,
robot_state.pose.pose.orientation.y,
robot_state.pose.pose.orientation.z,
robot_state.pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
yaw = euler[2]
dx = goal_x - robot_state.pose.pose.position.x
dy = goal_y - robot_state.pose.pose.position.y
# velocities coeficients
kp = 0.1
ka = 1
# linear and angular errors
p = math.sqrt(pow(dx, 2) + pow(dy, 2))
a = -yaw + math.atan2(dy, dx)
# calculates proportional linear and angular velocities
v_ref = kp * p + 0.1
w_ref = ka * a
#restricts the maximum velocities
if v_ref > v_max:
v_ref = v_max
if v_ref < -v_max:
v_ref = -v_max
if w_ref > w_max:
w_ref = w_max
if w_ref < -w_max:
w_ref = -w_max
twist_obj = Twist()
if self.iz == 2 and abs(a) > 0.15:
v_ref = 0
if self.iz >= len(self.traj_x) - 1:
twist_obj.linear.x = 0.0
twist_obj.angular.z = 0.0
print("Bigger iz")
# publishes velocities
twist_obj.linear.x = v_ref
twist_obj.angular.z = w_ref
self.move_obj.move_robot(twist_obj)
if p < 0.2:
print("Got to goal ") + str(self.iz)
self.iz = self.iz + 1
def move_backward(self):
self.move(Twist(Vector3(-0.5, 0, 0), Vector3(0, 0, 0)))
def run_control():
global pub, Vel, imagem, im_rgb
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
Vel = Twist()
process_image_size = (200, 50)
max_lateral_offset = process_image_size[0] / 2
max_row_width = process_image_size[0]
max_row_distance = process_image_size[0]
global v, w
v = 0
w = 0
nparticles = 100
pf = ParticleFilter(
transition=transition,
fitness=fitness,
nfeatures=4,
mu=[0, 0, 50, 100],
sigma=[1, 10, 50, 50],
lb=[-np.pi / 2, -max_lateral_offset, 0, 0],
ub=[np.pi / 2, max_lateral_offset, max_row_width, max_row_distance],
nparticles=nparticles,
distribution=['normal', 'normal', 'uniform', 'uniform'])
while not rospy.is_shutdown():
if (flag == 1):
imgb, imagempp, imga, im_rgba = preprocessing()
mask = cv2.resize(imagempp, process_image_size)
pf.next_step(
mask, [v, w]
) # I dont know the input on each image, so I will let it be [0.1,0] for now
best_particle = pf.return_best_mean()
angle = best_particle[0]
angle = -angle
font = cv2.FONT_HERSHEY_SIMPLEX
bottomLeftCornerOfText = (0, 100)
fontScale = 1
fontColor = (255, 255, 255)
lineType = 2
mask_best_particle = create_masks([best_particle], mask, plot=0)[0]
blended_image = blend_images_big(im_rgba, mask_best_particle)
cv2.putText(blended_image, str(angle), bottomLeftCornerOfText,
font, fontScale, fontColor, lineType)
#cv2.imshow('vector', vector)
cv2.imshow('frame', blended_image)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
print(angle)
actuate(angle, True)
v = Vel.linear.x
w = Vel.angular.z / Kpw
def turnRight(self):
twist = Twist()
twist.angular.z = -0.2
return twist
import threading
import math
import numpy
import time as tm
import matplotlib.pyplot as mt
import rospy
import copy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseWithCovariance
from geometry_msgs.msg import PoseStamped
global pub, move, cnt, time, free_time, global_distance, global_mod, x, y, a, b, w, z, flag, computation, odometry, positionx, positiony, robotx, roboty
rospy.init_node('obstacle_avoidance',anonymous=True)
pub=rospy.Publisher('/cmd_vel' ,Twist,queue_size=10)
move=Twist()
flag = 0
time = 0
x = 0
y = 0
a = 0
b = 0
cnt = 0
global_distance = 0
global_mod = 0
positionx = 0
positiony = 0
robotx = 0
roboty = 0
computation = []
odometryx = numpy.array([])
import sys
import rospy
import numpy as np
import math
from std_msgs.msg import String
from std_msgs.msg import Int16MultiArray
from std_msgs.msg import Int16
import time
import random
import geometry_msgs
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped
if __name__=='__main__':
rospy.init_node('stop')
while not rospy.is_shutdown():
try:
robot = ['/r0', '/r1', '/r2', '/r3', '/r4', '/r5']
for i in range(1, 6):
pub = rospy.Publisher(robot[i] + '/cmd_vel', Twist, queue_size = 10)
msg = Twist()
msg.linear.x = 0
msg.angular.z = 0
pub.publish(msg)
except KeyboardInterrupt:
print("shut down")
def followWall(self):
twist = Twist()
twist.linear.x = 0.2
twist.angular.z = -0.2
return twist
odom_sub = rospy.Subscriber('/odom', Odometry, getCurrentOdometry)
grid_sub = rospy.Subscriber('/map', OccupancyGrid, getGridInfo)
twist_pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 5)
sub_right = rospy.Subscriber('/custom/cliff_sensor_right', Bool, edge_detected, (twist_pub, 1))
sub_left = rospy.Subscriber('/custom/cliff_sensor_left', Bool, edge_detected, (twist_pub, 1))
sub_front_right = rospy.Subscriber('/custom/cliff_sensor_front_right', Bool, edge_detected, (twist_pub, -1))
sub_front_left = rospy.Subscriber('/custom/cliff_sensor_front_left', Bool, edge_detected, (twist_pub, -1))
face_pub = rospy.Subscriber('/custom/people_simple', Float32MultiArray, get_faces)
while not rospy.is_shutdown():
if odom_initialized and grid_initialized:
try:
if(driving == False):
goal = get_next_goal()
move_navigation(goal)
except rospy.ROSInterruptException:
rospy.loginfo("Navigation finished.")
twist_stop = Twist()
twist_stop.linear.x = 0
twist_stop.linear.y = 0
twist_stop.linear.z = 0
twist_stop.angular.x = 0
twist_stop.angular.y = 0
twist_stop.angular.z = 0
twist_pub.publish(twist_stop)
#!/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)
import rospy
from std_msgs.msg import String, Bool, Float32, ByteMultiArray
from geometry_msgs.msg import Twist, TransformStamped
from kobuki_msgs.msg import BumperEvent
import numpy as np
import math
import time
#DEFINE GLOBALS----------------------------------------------------------------
#Variables
robot_names = ['103', '104']
n_robots = len(robot_names)
robot_target = list() #List of robot target positions in twist
for i in range(0, n_robots):
robot_target.append(Twist())
robot_locations = list() #List of robot target positions in twist
for i in range(0, n_robots):
robot_locations.append(Twist())
main_rate = 10
last_velocity_command = list() #List of robot target positions in twist
for i in range(0, n_robots):
last_velocity_command.append(Twist())
#Set up publishers
pub_velocity_command = list()
for i in range(0, n_robots):
pub_velocity_command.append(
