def send_command(self, command):
"""Send command to the robot.
@param command: command to send to robot (tuple)
returns: respons from robot (tuple)
"""
#msg = ','.join(map(str, command))
msg = ','.join(str(x) for x in command) + '\r'
#rospy.loginfo("msg=%s", str(msg))
if not self.fake_connection:
done = False
start_time = rospy.get_rostime()
while not done:
try:
if self.sci is None:
self.sci = SerialInterface(self.tty, self.baudrate)
with self.sci.lock:
self.sci.flush_input()
self.sci.send(msg)
rsp = self.sci.read()
#rsp = msg
#rospy.loginfo("rsp=%s", str(rsp))
done = True
except Exception, err:
rospy.logerr(str(err))
if rospy.get_rostime() - start_time > self.connection_timeout:
raise rospy.ROSInterruptException("connection to robot seems broken")
self.sci = None # assume serial interface is broken
rospy.loginfo('waiting for connection')
rospy.sleep(1.) # wait 1 seconds before retrying
def main():
rospy.init_node('state_machine')
sm = smach.StateMachine(outcomes=['END'])
with sm:
smach.StateMachine.add('LISTEN', Listen(), transitions={'Bell':'NAVIGATION'})
smach.StateMachine.add('NAVIGATION', Navigation(), transitions={'SPEECH':'SPEECH','RECOGNITION':'RECOGNITION','DELIMAN_IN_KITCHEN':'DELIMAN_IN_KITCHEN','DOCTOR_IN_BEDROOM':'DOCTOR_IN_BEDROOM','BYE':'BYE','Failed':'NAVIGATION','Unknown':'END'})
smach.StateMachine.add('RECOGNITION', Recognition(), transitions={'Doctor':'DOCTOR','Deliman':'DELIMAN','Postman':'POSTMAN','Unknown':'UNKNOWN_PERSON','Unrecognised':'SPEECH','No Face':'SPEECH','Unsure':'SPEECH'})
smach.StateMachine.add('CONFIRM_RECOGNITION', ConfirmRecognition(), transitions={'Doctor':'DOCTOR','Deliman':'DELIMAN','Postman':'POSTMAN','Unknown':'UNKNOWN_PERSON','Unrecognised':'RECOGNITION','Unsure':'CONFIRM_RECOGNITION'})
smach.StateMachine.add('SPEECH', SpeechState(), transitions={'NAVIGATION':'NAVIGATION','RECOGNITION':'RECOGNITION','LISTEN':'LISTEN','POSTMAN_ENTERED':'POSTMAN_ENTERED','POSTMAN_WAIT':'POSTMAN_WAIT','DELIMAN_ENTERED':'DELIMAN_ENTERED','DELIMAN_WAIT':'DELIMAN_WAIT','DOCTOR_ENTERED':'DOCTOR_ENTERED','DOCTOR_WAIT':'DOCTOR_WAIT','CONFIRM_RECOGNITION':'CONFIRM_RECOGNITION'})
smach.StateMachine.add('DOCTOR', Doctor(), transitions={'NAVIGATION':'NAVIGATION'})
smach.StateMachine.add('DELIMAN', Deliman(), transitions={'NAVIGATION':'NAVIGATION'})
smach.StateMachine.add('POSTMAN', Postman(), transitions={'NAVIGATION':'NAVIGATION'})
smach.StateMachine.add('UNKNOWN_PERSON', UnknownPerson(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('POSTMAN_ENTERED', PostmanEntered(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('POSTMAN_WAIT', PostmanWait(), transitions={'Leaving':'BYE'})
smach.StateMachine.add('DELIMAN_ENTERED', DelimanEntered(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('DELIMAN_IN_KITCHEN', DelimanInKitchen(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('DELIMAN_WAIT', DelimanWait(), transitions={'Leaving':'SPEECH'})
smach.StateMachine.add('DOCTOR_ENTERED', DoctorEntered(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('DOCTOR_IN_BEDROOM', DoctorInBedroom(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('DOCTOR_WAIT', DoctorWait(), transitions={'Leaving':'SPEECH'})
smach.StateMachine.add('BYE', Bye(), transitions={'Speech':'SPEECH'})
rospy.sleep(2)
Recognition.unknown_count = 0
outcome = sm.execute()
def create_test_sock(pcap_filename):
rospy.sleep(0.1)
import pcapy
from StringIO import StringIO
from impacket import ImpactDecoder
body_list = []
cap = pcapy.open_offline(pcap_filename)
decoder = ImpactDecoder.EthDecoder()
while True:
header, payload = cap.next()
if not header: break
udp = decoder.decode(payload).child().child()
body_list.append(udp.child().get_packet())
data_io = StringIO(''.join(body_list))
class MockSocket(object):
def recv(self, byte_count):
rospy.sleep(0.0001)
data = data_io.read(byte_count)
if data == "":
rospy.signal_shutdown("Test completed.")
return data
def settimeout(self, timeout):
pass
return MockSocket()
def logger():
rospy.init_node('log_imu_topic', anonymous=False)
rospy.Subscriber("uav0/imu", Imu, imu_callback)
while not rospy.is_shutdown ():
rospy.sleep(0.1)
def load_config_from_param():
# first, make sure parameter server is even loaded
while not rospy.search_param("/joints"):
rospy.loginfo("waiting for parameter server to load with joint definitions")
rospy.sleep(1)
rospy.sleep(1)
joints = rospy.get_param('/joints')
for name in joints:
rospy.loginfo( "found: " + name )
s = Servo()
key = '/joints/' + name + '/'
s.bus = rospy.get_param(key + 'bus')
s.servoPin = rospy.get_param(key + 'servoPin')
s.minPulse = rospy.get_param(key + 'minPulse')
s.maxPulse = rospy.get_param(key + 'maxPulse')
s.minGoal = rospy.get_param(key + 'minGoal')
s.maxGoal = rospy.get_param(key + 'maxGoal')
s.rest = rospy.get_param(key + 'rest')
s.maxSpeed = rospy.get_param(key + 'maxSpeed')
s.smoothing = rospy.get_param(key + 'smoothing')
s.sensorpin = rospy.get_param(key + 'sensorPin')
s.minSensor = rospy.get_param(key + 'minSensor')
s.maxSensor = rospy.get_param(key + 'maxSensor')
servos[name] = s
return servos
def broadcast(self):
f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "rb")
self.camera_transform = pickle.load(f)
f.close()
#SOMETIMES NEED TO INVERT X & Y AXES; just change from 1 to -1 and vice versa
offset = tfx.transform([SKETCH_OFFSET, 0, CAP_OFFSET], [[1, 0, 0], [0, 1, 0], [0, 0, 1]])
self.camera_transform = tfx.transform(self.camera_transform).as_transform() * offset
transform = tfx.inverse_tf(self.camera_transform)
pt = transform.translation
rot = transform.rotation
msg = TransformStamped()
msg.header.stamp = rospy.Time.now()
msg.transform.rotation.x = rot.x
msg.transform.rotation.y = rot.y
msg.transform.rotation.z = rot.z
msg.transform.rotation.w = rot.w
msg.child_frame_id = "left_BC"
msg.transform.translation.x = pt.x
msg.transform.translation.y = pt.y
msg.transform.translation.z = pt.z
msg.header.frame_id = "registration_brick_right"
msg.header.stamp = rospy.Time.now()
print('boo')
while not rospy.is_shutdown():
msg.header.stamp = rospy.Time.now()
self.pub.publish([msg])
rospy.sleep(0.1)
def main_loop(self):
img = Image()
while not rospy.is_shutdown():
#print "getting image..",
image = self.camProxy.getImageRemote(self.nameId)
#print "ok"
# TODO: better time
img.header.stamp = rospy.Time.now()
img.header.frame_id = self.frame_id
img.height = image[1]
img.width = image[0]
nbLayers = image[2]
#colorspace = image[3]
if image[3] == kYUVColorSpace:
encoding = "mono8"
elif image[3] == kRGBColorSpace:
encoding = "rgb8"
elif image[3] == kBGRColorSpace:
encoding = "bgr8"
else:
rospy.logerror("Received unknown encoding: {0}".format(image[3]))
img.encoding = encoding
img.step = img.width * nbLayers
img.data = image[6]
self.info_.width = img.width
self.info_.height = img.height
self.info_.header = img.header
self.pub_img_.publish(img)
self.pub_info_.publish(self.info_)
rospy.sleep(0.0001)# TODO: is this necessary?
self.camProxy.unsubscribe(self.nameId)
def execute(self, userdata):
log('In state SPEECH. Message: ' + SpeechState.msg)
say(SpeechState.msg)
rospy.sleep(1)
return SpeechState.next_state
def speak(message):
string = message
music_stream_uri = 'http://translate.google.com/translate_tts?tl=en&q='+string
player = gst.element_factory_make("playbin","player")
player.set_property('uri',music_stream_uri)
player.set_state(gst.STATE_PLAYING)
rospy.sleep(4)
def execute(self,userdata):
self.angle = userdata.angle_in
print self.angle
move_cmd = Twist()
while not rospy.is_shutdown():
move_cmd = Twist()
self.odom_angle = self.get_odom_angle()
last_angle = self.odom_angle
turn_angle = 0
angular_speed = self.speed
while abs(float(turn_angle)) < abs(float(self.angle)):
if rospy.is_shutdown():
return "succeeded"
if self.angle < 0:
move_cmd.angular.z = -angular_speed
else:
move_cmd.angular.z = angular_speed
self.cmd_vel.publish(move_cmd)
self.r.sleep()
self.odom_angle = self.get_odom_angle()
delta_angle = self.secretindigal * self.normalize_angle(self.odom_angle - last_angle)
turn_angle += delta_angle
last_angle = self.odom_angle
rospy.sleep(0.5)
if move_cmd.angular.z != 0.0:
self.cmd_vel.publish(move_cmd)
else:
break
self.cmd_vel.publish(Twist())
return "succeeded"
def serWrite(myStr):
try:
for i in range(len(myStr)):
ser.write(myStr[i])
rospy.sleep(0.0001) # Sometimes, a delay seems to help. Maybe?
except:
rospy.logerr("[GS] Unable to send data. Check connection.")
def complete_timing_trials():
rospy.init_node('time_trials')
times = dict(zip(['pickup', 'place'], [[], []]))
awesome = pick_and_place()
for i in range(5):
#raw_input("pick up object?")
start = rospy.get_rostime().to_sec()
result = awesome.pick_up()
while not result:
result = awesome.pick_up()
continue
end = rospy.get_rostime().to_sec()
times['pickup'].append(int(end - start)+1) # ceiling round off
#raw_input("place object?")
rospy.sleep(1) # wait for robot to see AR tag
start = rospy.get_rostime().to_sec()
result = awesome.place(use_offset=False)
while not result:
result = awesome.place()
end = rospy.get_rostime().to_sec()
times['place'].append(int(end - start)+1) # ceiling round off
with open("time_results.csv", "wb") as f:
for key in times:
f.write( "%s, %s\n" % (key, ", ".join(repr(e) for e in times[key])))
def __init__(self):
rospy.init_node('vel_def')
self.scale_rotRob = rospy.get_param("~scaleRotRob",0.5)
self.sigmaVel = rospy.get_param("~sigmaVel",0.5)
self.minVel = rospy.get_param("~minVel",0.025)
self.maxVel = rospy.get_param("~maxVel",0.3)
self.minVelForRot = rospy.get_param("~minVelForRot",0.08)
self.maxRotRob = rospy.get_param("~maxRotRob",0.5)
self.maxRotCam = rospy.get_param("~maxRotCam",0.5)
self.scale_lin = rospy.get_param("~scale_lin",0.5)
self.sigmaRot = rospy.get_param("~sigmaRot",0.08)
self.scale_rotCam = rospy.get_param("~scaleRotCam",0.5)
self.scale_rotInit = rospy.get_param("~scaleRotInit",1)
rospy.Subscriber("~servo", Twist, self.convert_vel)
sub = rospy.Subscriber('~joy', Joy, self.joy_cb)
sub = rospy.Subscriber('/imu/data', Imu, self.imu_cb)
self.rob_twist_pub = rospy.Publisher("~rob_twist_pub", Twist)
self.pan_pub_fl = rospy.Publisher("~pan_pub_float",Float64)
self.pan_pub_tw = rospy.Publisher("~pan_pub_twist",Twist)
self.ang_rob = 0
self.omega_z = 0
self.reached_goal = 0
self.reconfig_srv = Server(VelConvertConfig, self.reconfig_cb)
rospy.sleep(1.0)
def start(self): while not rospy.is_shutdown(): self.kikloi+=1 print "kikloi=", self.kikloi state=randrange(2) print "state=", state if state==0: philosopherState="STATE_NOTHING" print philosopherState self.philosopherIState="ISTATE_NOTHING" rospy.sleep(0.1) elif state==1: philosopherState="STATE_ACTIVE" print philosopherState self.stateActive() print "state Active" if self.kikloi<=self.allagi: self.hunger_level_=self.philosopher.recalculateHunger(self.philosopherIState,self.last_hunger_update_,self.hunger_level_) else: self.hunger_level_=self.findTheRightRecalculateHunger(self.scenario_code) print "recalculate" self.last_hunger_update_=rospy.get_time() philosopherIState="ISTATE_WAITING" else: print "state error"
def main():
frizzle = QuadcopterBrain()
# Quadcopter node (carl) must be initialized before get_param will work
outside = rospy.get_param("Quadcopter/outside", False)
rospy.loginfo("In outside mode: %s.", outside)
rospy.loginfo("If incorrect, add _outside:=True to the rosrun call")
frizzle.quadcopter.clear_waypoints()
rospy.loginfo("Sleeping for 3 seconds...")
rospy.sleep(3)
#To change waypoints, change the file shown below
mission_waypoints = WaypointTools.open_waypoint_file(
"great_lawn_waypoints.json")
if outside:
frizzle.quadcopter.arm()
#If mission file has all of the points of interest, use:
#for point in mission_waypoints:
# mission.append(point)
#frizzle.fly_path(mission)
frizzle.fly_path([mission_waypoints["F"],
mission_waypoints["E"],
mission_waypoints["G"]])
def __init__(self, name, log_level=rospy.INFO, start=True):
rospy.init_node(name, log_level=log_level)
self.clear_octomap_service = rospy.ServiceProxy('/octomap_server/clear_bbx', BoundingBoxQuery)
self.reset_octomap_service = rospy.ServiceProxy('/octomap_server/reset', Empty)
is_sim = rospy.get_param("~is_simulation", False)
self.is_ready = True
rospy.wait_for_service('/obstacle_generator')
rospy.wait_for_service('/goal_manager/add_goal', 5)
rospy.wait_for_service('/move_base/make_plan', 5)
if not is_sim:
rospy.wait_for_message('/odometry/filtered', Odometry, timeout=None)
rospy.sleep(0.02)
self.generate_obstacle_service = rospy.ServiceProxy('/obstacle_generator', GenerateObstacle)
self.send_goal_service = rospy.ServiceProxy('/goal_manager/add_goal', AddGoal)
rospy.Subscriber("/goal_manager/current", GoalWithPriority, self.goal_callback)
rospy.Subscriber("/goal_manager/last_goal_reached", Bool, self.last_goal_callback)
self.tf_listener = tf.TransformListener()
self.goal_count = 0
self.tf_listener.waitForTransform("/odom", "/base_footprint", rospy.Time(), rospy.Duration(6.0))
self.is_ready = True
self.save_start_pos()
rospy.loginfo("StateAi {} started.".format(name))
if start:
self.on_start()
rospy.spin()
def __init__(self, port=None, baud=57600, timeout=5.0):
""" Initialize node, connect to bus, attempt to negotiate topics. """
self.mutex = thread.allocate_lock()
self.lastsync = rospy.Time(0)
self.lastsync_lost = rospy.Time(0)
self.timeout = timeout
self.synced = False
self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray)
if port== None:
# no port specified, listen for any new port?
pass
elif hasattr(port, 'read'):
#assume its a filelike object
self.port=port
else:
# open a specific port
try:
self.port = Serial(port, baud, timeout=self.timeout*0.5)
except SerialException as e:
rospy.logerr("Error opening serial: %s", e)
rospy.signal_shutdown("Error opening serial: %s" % e)
raise SystemExit
self.port.timeout = 0.01 # Edit the port timeout
time.sleep(0.1) # Wait for ready (patch for Uno)
# hydro introduces protocol ver2 which must match node_handle.h
# The protocol version is sent as the 2nd sync byte emitted by each end
self.protocol_ver1 = '\xff'
self.protocol_ver2 = '\xfe'
self.protocol_ver = self.protocol_ver2
self.publishers = dict() # id:Publishers
self.subscribers = dict() # topic:Subscriber
self.services = dict() # topic:Service
self.buffer_out = -1
self.buffer_in = -1
self.callbacks = dict()
# endpoints for creating new pubs/subs
self.callbacks[TopicInfo.ID_PUBLISHER] = self.setupPublisher
self.callbacks[TopicInfo.ID_SUBSCRIBER] = self.setupSubscriber
# service client/servers have 2 creation endpoints (a publisher and a subscriber)
self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_PUBLISHER] = self.setupServiceServerPublisher
self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_SUBSCRIBER] = self.setupServiceServerSubscriber
self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_PUBLISHER] = self.setupServiceClientPublisher
self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_SUBSCRIBER] = self.setupServiceClientSubscriber
# custom endpoints
self.callbacks[TopicInfo.ID_PARAMETER_REQUEST] = self.handleParameterRequest
self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest
self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest
rospy.sleep(2.0) # TODO
self.requestTopics()
self.lastsync = rospy.Time.now()
def lookAroundAction(self):
self.say("I'm looking for somebody. Please come closer.")
p = PointStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "/base_link"
p.point.x = 5.0
sign = random.choice([-1, 1])
p.point.y = sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
p.point.y = -1 * sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
p.point.y = sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
p.point.y = -1 * sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
rospy.loginfo("Looking around")
rospy.sleep(1)
def publish_robot_status():
status_updates = rospy.Publisher('status_updates', String)
while not rospy.is_shutdown():
update = robot.status
rospy.loginfo(update)
status_updates.publish(String(update))
rospy.sleep(0.5)
def send_clusters():
pub = rospy.Publisher( 'test_cluster', Cluster2 )
rospy.init_node( 'test_cluster' )
rospy.loginfo( 'Cluster test started. Sending cluster points...' )
br = tf.TransformBroadcaster()
while not rospy.is_shutdown():
#points = np.random.randn( points_number, 3 )
#points = points / 2.5 * range_size
points = []
for i in range( points_number ):
points.append([
np.random.randint( -1 * range_size, range_size ),
np.random.randint( -1 * range_size, range_size ),
np.random.randint( -1 * range_size, range_size )])
points = np.asarray( points )
pub.publish( gen_cluster_msg( points, get_points_label( points )))
br.sendTransform((0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"base_link",
"map")
rospy.sleep( 1. / fps )
def align(self):
with self.lock:
angle = self._scan_angle
no_data_count = 0
count = 0
epsilon = 0.05
cmd = Twist()
while True: #self._inital_wall_angle:
if angle == 0: # magic number, means we have no data yet
no_data_count += 1
if no_data_count == 40:
return False
if self._stop or rospy.is_shutdown():
return False
elif count > 20:
with self.lock:
self._centred_gyro_angle = self._gyro_angle
cmd.angular.z = 0.0
self.cmd_vel_publisher.publish(cmd)
return True
if math.fabs(angle) < 0.05:
count = count + 1
# The work
if angle > 0:
cmd.angular.z = -0.2
else:
cmd.angular.z = 0.2
self.cmd_vel_publisher.publish(cmd)
rospy.sleep(0.05)
with self.lock:
angle = self._scan_angle
print "end of align"
def startlistening(self, msg):
speech = msg.data
sl = 'jude'
if sl in speech:
self.ct = 1
self.soundhandle.say('i am here', self.voice)
rospy.sleep(0.02)
def execute(self, userdata):
self.publisher = rospy.Publisher('/Module_Enable', ModuleEnableMsg)
self.subscriber = rospy.Subscriber('/Task_Completion', String, self.taskCompleted)
self.buoySubscriber = rospy.Subscriber('img_rec/buoys/red', ImgRecObject, self.buoyLoc)
self.depthSubscriber = rospy.Subscriber('Sub_Depth', Float32, self.depth)
msg = ModuleEnableMsg()
msg.Module = 'NewBuoyTask'
msg.State = True
self.publisher.publish(msg)
#keep trying until preempted, success, or timeout
while self.timeout > 0:
if self.buoyHit:
self.beDone()
return 'succeeded'
if self.preempt_requested():
self.beDone()
self.service_preempt()
return 'preempted'
if self.objectLost:
self.scanForBuoy()
else:
self.descendToBuoy()
self.alignWithBouy()
self.advance()
self.extendTimeout()
# we decide the object is lost until we receive another message
self.objectLost = True
rospy.sleep(1)
self.timeout -= 1
self.objectLost = True
#we timed out
self.beDone()
return 'timeout'
def talker():
global joy_value
global last_joy
rospy.init_node('vrep_ros_teleop')
sub = rospy.Subscriber('~joy', Joy, joy_cb)
pub = rospy.Publisher('~twistCommand', Twist, queue_size=1)
axis_linear = rospy.get_param("~axis_linear",1)
axis_angular = rospy.get_param("~axis_angular",0)
scale_linear = rospy.get_param("~scale_linear",5.)
scale_angular = rospy.get_param("~scale_angular",10.)
timeout = True
while not rospy.is_shutdown():
twist = Twist()
if (rospy.rostime.get_time() - last_joy) < 0.5:
if timeout:
timeout = False
rospy.loginfo("Accepting joystick commands")
twist.linear.x = joy_value.axes[axis_linear] * scale_linear
twist.angular.z = joy_value.axes[axis_angular] * scale_angular
if twist.linear.x < 0:
twist.angular.z = - twist.angular.z
else:
if not timeout:
timeout = True
rospy.loginfo("Timeout: ignoring joystick commands")
pub.publish(twist)
rospy.sleep(0.1)
def start(self):
signal.signal(signal.SIGINT, self.signal_handler) # Catch Ctrl-Cs
self.initRobot()
self.initComms("haptic_mpc")
rospy.sleep(1.0) # Delay to allow the ROS node to initialise properly.
self.initControlParametersFromServer()
r = rospy.Rate(self.frequency)
self.time_step = 1.0/self.frequency
rospy.loginfo("Haptic MPC: Waiting for Robot Haptic State message")
while not self.getJointAngles():
r.sleep()
rospy.loginfo("Haptic MPC: Got Robot Haptic State message")
rospy.loginfo("Haptic MPC: Resetting desired joint angles to current position")
self.publishDesiredJointAngles(self.getJointAngles())
# Main control loop
rospy.loginfo("Haptic MPC: Starting MPC")
# lasttime = rospy.Time.now()
while not rospy.is_shutdown():
self.updateController()
#rospy.spin() # For debug - run once
r.sleep()
def run(self):
while self.joint_names is None:
print "Waiting for joint state information from %s/state topic" %self.controller
rospy.sleep(2)
print "Received joint state information. Sending neck to default position (0.0m)"
head_up_msg = self.head_up_msg()
self.goal_pub.publish(head_up_msg)
def goal_cb(self, goal):
self.update_goal(goal.goal)
update_rate = rospy.Rate(40)
command = Twist()
while not (rospy.is_shutdown() or self.at_goal):
command.linear.x = self.get_trans_x()
command.linear.y = self.get_trans_y()
command.angular.z = self.get_rot()
(x,y,z) = self.avoid_obstacles()
if x is not None:
command.linear.x = x
if y is not None:
command.linear.y = y
command.angular.z += z
if command.linear.y > 0:
if not self.left_clear():
command.linear.y = 0.0
elif command.linear.y < 0:
if not self.right_clear():
command.linear.y = 0.0
#print "Sending vel_command: \r\n %s" %self.command
self.vel_out.publish(command)
rospy.sleep(0.01) #Min sleep
update_rate.sleep() #keep pace
if self.at_goal:
print "Arrived at goal"
result = ServoResult()
result.arrived = Bool(True)
self.servoing_as.set_succeeded(result)
def execute(self, userdata):
sss.move("gripper", "open", blocking=False)
# sss.move("arm", "zeroposition")
for object in userdata.object_list:
# ToDo: need to be adjusted to correct stuff
if object.pose.pose.position.z <= 0.0 or object.pose.pose.position.z >= 0.10:
continue
sss.move("arm", "zeroposition", mode="planned")
#object.pose.pose.position.z = object.pose.pose.position.z + 0.02
object.pose.pose.position.x = object.pose.pose.position.x + 0.01
object.pose.pose.position.y = object.pose.pose.position.y - 0.005
handle_arm = sss.move("arm", [object.pose.pose.position.x, object.pose.pose.position.y, object.pose.pose.position.z, "/base_link"], mode="planned")
if handle_arm.get_state() == 3:
sss.move("gripper", "close", blocking=False)
rospy.sleep(3.0)
sss.move("arm", "zeroposition", mode="planned")
return 'succeeded'
else:
rospy.logerr('could not find IK for current object')
return 'failed'
def __init__(self):
#start node
rospy.init_node("map_saver")
rospy.on_shutdown(self.die)
#get parameters from server
self.save_dir = rospy.get_param("map_save_dir","map_log/") #default to .ros/map_log
update_rate = rospy.get_param("map_save_update_rate",120) #update once every 2 min
#create directory and set clean_open
self.clean_open=self.create_dir()
#create dictionaries
self.edges={} #dictionary of edge_id:edge_obj
self.nodes={} #dictionary of node_id:node_obj
#begin listening
rospy.Subscriber("/node",GVGNode, self.node_handler)
rospy.Subscriber("/edge",GVGEdgeMsg, self.edge_handler)
self.updated=False
#while alive and able to open the file
self.alive=True
while(self.clean_open and self.alive):
if(self.updated):
self.dump(str(rospy.get_time()))
self.updated=False
rospy.sleep(update_rate)
raw_keys = masterPoseDict['poses'].keys()
if len(raw_keys) == 0:
continue
sorted_keys = sorted(
raw_keys,
key=lambda x: int(x[9:])) # 9 because 'trainEnv_' is 9 characters
print('sorted keys:')
print(len(sorted_keys))
for i, pose_name in enumerate(sorted_keys):
print("iter number: " + str(i) + " \n\n\n")
gazeboMod.delete_obstacles()
print("POSE: " + str(pose_name))
new_pose = masterPoseDict['poses'][pose_name]
gazeboMod.permute_obstacles(new_pose)
print("Loaded new pose and permuted obstacles")
rospy.sleep(30) # make sure the scene is generated
call = create_call(executable, rootPath, pose_name)
### Printing the executable call and allowing user to manually cycle through environments for demonstration
print(call)
### Uncomment below to call pointcloud_to_pcd executable which takes snapshot of the streaming pointcloud data
### and saves it to a .pcd file in a desired file location (as specified by prefix in the command call)
print("Calling executable... \n\n\n")
t = time.time()
stderr_f = open('log.txt', 'w')
p = subprocess.Popen(call, stderr=stderr_f)
rospy.sleep(2)
p.terminate() # original version
#set goal
goal.target_pose.pose.position.x = 0.0
goal.target_pose.pose.position.y = 1.0
goal.target_pose.pose.orientation.w = 1.0
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
# Start the robot toward the next location
self.move_base.send_goal(goal)
# Allow 30 seconds to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(30))
rospy.loginfo("OK 1")
def shutdown(self):
rospy.loginfo("Stopping the robot...")
#self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
try:
x = NavTest()
rospy.sleep(2)
x.shutdown()
rospy.loginfo("OK 2")
except rospy.ROSInterruptException:
rospy.loginfo("Navigation test finished.")
def check_mood(topic):
s = "Currently, public opinion about covid is mostly neutral. On Twitter posts there are more negative posts on the topic than positive."
speak_comp(s)
if __name__ == "__main__":
tlk = "Adjusting for ambient noise, please be quiet"
speak_comp(tlk)
with m as source:
r.adjust_for_ambient_noise(source)
rospy.sleep(1)
check = True
count = 0
while check:
if count == 0:
s = recognize(
"Hi, I am Palpi. Your personal assistant in times of covid19. I can help with covid tips, reports, statistics or tell you news about possible vaccine. What would you line to know?"
).lower()
else:
s = recognize(
"Would you like to know more about tips, reports, public opinion or statistics?"
).lower()
print("heard: " + s)
print(type(s))
parsed = interpreter.parse(s)
obj = objectify(parsed)
def filterCB(self, msg):
if msg.mode is controllerFusionMsg.IGNORE:
self.is_collision_expected = True
rospy.sleep(0.1) # TODO
else:
self.is_collision_expected = False
def get_current_pos(self, frame='end'):
while self.joint_position[0] == 0:
rospy.loginfo('Waiting for joint angles...')
rospy.sleep(1)
return self.forward_kinematics(self.joint_position, frame)
def stopSlowly():
global VelocityMessage_publisher
global VelocityMes
VelocityMessage.linear.x = 0.09
VelocityMessage_publisher.publish
rospy.sleep(0.2)
VelocityMessage.linear.x = 0.08
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.2)
VelocityMessage.linear.x = 0.07
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.2)
VelocityMessage.linear.x = 0.06
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.2)
VelocityMessage.linear.x = 0.05
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.2)
VelocityMessage.linear.x = 0.04
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.2)
VelocityMessage.linear.x = 0.03
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.2)
VelocityMessage.linear.x = 0.02
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.2)
VelocityMessage.linear.x = 0.01
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.2)
VelocityMessage.linear.x = 0.0
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.5)
VelocityMessage.linear.x = 0.0
VelocityMessage_publisher.publish(VelocityMessage)
rospy.sleep(0.5)
def shutdown(self):
rospy.loginfo("Stopping the robot...")
#self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def gripper_open(self):
self._gripper.open()
rospy.sleep(1.0)
def sleep(t):
try:
rospy.sleep(t)
except:
pass
def gripper_close(self):
self._gripper.close()
rospy.sleep(1.0)
vel_linear_x_send = 100 * vel_linear_x + 100
vel_angular_z_send = 30 * vel_angular_z + 90 # [60 - 120]3
str1 = str(chr(int(vel_linear_x_send)))
#print str1
#print vel_linear_x_send
str1 = str(chr(int(vel_angular_z_send)))
#print str1
#print vel_angular_z_send
#ser.write(str(chr(int(vel_linear_x_send))))
#ser.write(str(chr(int(vel_angular_z_send))))
a = chr(int(vel_linear_x_send))
b = chr(int(vel_angular_z_send))
s = a + b
print s
#ser.write(chr(int(vel_linear_x_send)))
#ser.write(chr(int(vel_angular_z_send)))
ser.write(s)
ser.isOpen()
rospy.init_node('llc_node', anonymous=True)
rospy.Subscriber("/cmd_vel", Twist, cmdvel_callback)
#while 1:
while not rospy.is_shutdown():
#time.sleep(0.05)
rospy.sleep(0.02)
rospy.spin()
#rospy.Subscriber("/cmd_vel", Twist, cmdvel_callback)
def move_to_start(self, start_angles=None):
print("Moving the {0} arm to start pose...".format(self._limb_name))
if not start_angles:
start_angles = dict(zip(self._joint_names, [0]*7))
self._guarded_move_to_joint_position(start_angles)
rospy.sleep(1.0)
"rh_LFJ1": 90, "rh_LFJ2": 90, "rh_LFJ3": 90, "rh_LFJ4": 0, "rh_LFJ5": 0,
"rh_WRJ1": 0, "rh_WRJ2": 0}
# store step 2 Bio_Tac
store_2_BioTac = {"rh_THJ1": 20, "rh_THJ2": 36, "rh_THJ3": 0, "rh_THJ4": 30, "rh_THJ5": -3,
"rh_FFJ1": 90, "rh_FFJ2": 90, "rh_FFJ3": 90, "rh_FFJ4": 0,
"rh_MFJ1": 90, "rh_MFJ2": 90, "rh_MFJ3": 90, "rh_MFJ4": 0,
"rh_RFJ1": 90, "rh_RFJ2": 90, "rh_RFJ3": 90, "rh_RFJ4": 0,
"rh_LFJ1": 90, "rh_LFJ2": 90, "rh_LFJ3": 90, "rh_LFJ4": 0, "rh_LFJ5": 0,
"rh_WRJ1": 0, "rh_WRJ2": 0}
# store step 3
store_3 = {"rh_THJ1": 0, "rh_THJ2": 0, "rh_THJ3": 0, "rh_THJ4": 65, "rh_THJ5": 0}
for x in range(0, 100):
print "We're on iteration number %d" % (x)
rospy.sleep(1)
hand_commander.move_to_joint_value_target_unsafe(store_3, 1.1, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(start_pos, 1.1, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(flex_ff, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(ext_ff, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(flex_mf, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(ext_mf, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(flex_rf, 1.0, False, angle_degrees=True)
rospy.sleep(1.1)
hand_commander.move_to_joint_value_target_unsafe(ext_rf, 1.0, False, angle_degrees=True)
def shutdown(self):
if self.goal_sent:
self.move_base.cancel_goal()
rospy.loginfo("Stop")
rospy.sleep(1)
import Utils
import rospy
rospy.init_node("flight")
ir = Utils.IR()
desription = {
"6897": "Noose through Ring",
"9867": "Noose through Gate",
"b04f": "Fly to start point with purple led and landing",
"30cf": "Fly to start point with blue led and landing",
"18e7": "Fly to payload zone with yellow led and landing"
}
ir_cmds = ["6897", "9867", "b04f", "30cf", "18e7"]
ccc = ir.waitData(ir_cmds)
print(desription[ccc])
rospy.sleep(0.2)
global scanMessageQueue
scanMessageQueue = Queue.Queue()
#Subscribe to map updates
map_sub = rospy.Subscriber('/map', OccupancyGrid, mapCallback, queue_size=1) #check
scan_sub = rospy.Subscriber('/hsrb/base_scan', LaserScan, scanCallback, queue_size=1)
# costmap_sub = rospy.Subscriber('/move_base/local_costmap/costmap', OccupancyGrid, costmapCallback, queue_size=1)
#Start requesting position in background
Thread(target=request_pos_at_rate, name="Request_pos_at_rate Thread", args=[POS_REQUEST_RATE]).start()
Thread(target=scanProcessing, name="Request_pos_at_rate Thread").start()
print "Waiting for the initial position from tf...",
while not receivedInitPos or not receivedNewMap or not receivedNewCostMap:
rospy.sleep(.1)
pass
print "DONE"
print "Started exploration."
#control = RobotControl(rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=5), POS_REQUEST_RATE, ROTATE_AROUND_GRANULARITY, ANGLE_TOLERANCE, POS_TOLERANCE)
# control.rotate(math.pi)
exploreEnvironment()
global save_map_path
print "Done with exploration. Exiting..."
print "saving map"
os.system(save_map_path)
global xact, yact, yaw, vact, wact
xact, yact, yaw, vact, wact = 0.0, 0.0, 0.0, 0.0, 0.0
vref = 8
Radius = 40.0
xact, yact, theta_old = Radius, 0.0, 0.0
x_ref, y_ref, theta_up = Radius, 0.0, 0.0
rospy.init_node('check', anonymous=True)
rospy.Subscriber('/agent_0/odom', Odometry, datagps)
pub_speed = rospy.Publisher('/agent_0/cmd_vel', Twist, queue_size=10)
pub_graph_data = rospy.Publisher('/graph_data',
Float64MultiArray,
queue_size=100)
graph_data = Float64MultiArray()
speed_car = Twist()
rospy.sleep(5.0)
time_init = rospy.get_time()
while True:
global xact, yact, yaw, vact, wact
time_curr = rospy.get_time()
deltat = time_curr - time_init
theta_dot_up = vref / Radius
theta_up = theta_old + deltat * theta_dot_up
x_ref = Radius * math.cos(theta_up)
y_ref = Radius * math.sin(theta_up)
theta_act = yaw
if theta_act < 0:
theta_act += 2 * math.pi
def test_AA_initial_state(self): # test the initial state of kill signal
self.AlarmBroadcaster.clear_alarm()
rospy.sleep(0.1)
self.assertEqual(self.AlarmListener.is_cleared(),
True,
msg='current state of kill signal is raised')
def turn(deg):
angle_to_achieve = (get_yaw() + deg)
angle_to_achieve -= 360. if angle_to_achieve > 360. else 0.
vel.linear.x = 0
vel.angular.z =0.4
rate = rospy.Rate(5)
print "Target::" + str(angle_to_achieve)
rospy.sleep(.5)
while not rospy.is_shutdown() and abs(angle_diff(get_yaw(), angle_to_achieve)) > 0.5:
vel_pub.publish(vel)
print "yaw_now::"+ str(get_yaw())
rate.sleep()
vel.angular.z = 0
vel_pub.publish(vel)
# while not rospy.is_shutdown():
# vel.angular.z = 0.4
# vel_pub.publish(vel)
# print "YAW:: " + str(get_yaw())
# rate.sleep()
print "YAW1:: " + str(get_yaw())
rospy.sleep(2.)
print "YAW1:: " + str(get_yaw())
turn(90)
print "YAW2:: " + str(get_yaw())
rospy.sleep(2.)
turn(180)
print "YAW1:: " + str(get_yaw())
def listener():
rospy.init_node('listener', anonymous=True)
rospy.sleep(rospy.get_param('delay', 0.0))
rospy.Subscriber("chatter", String, callback)
rospy.spin()
def main():
# initiate node
rospy.init_node('SNU_drone', anonymous=True)
# controller object
cnt = Controller()
# flight mode object
modes = fcuModes()
# ROS loop rate
rate = rospy.Rate(20.0)
# Subscribe to drone's local position
rospy.Subscriber('mavros/local_position/pose', PoseStamped, cnt.posCb) # cnt.local_pos = Drone's local_position
rospy.sleep(0.2)
rospy.Subscriber('mavros/global_position/global', NavSatFix, cnt.posCb_glob) # cnt.global_pos = Drone's GPS position(lat, long, alt)
rospy.sleep(0.2)
rospy.Subscriber('/Target_GPS_msg', Target_GPS, cnt.GPS_callback)
rospy.sleep(0.2)
# Setpoint publisher
sp_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)
rospy.sleep(0.2)
sp_glob_pub = rospy.Publisher('mavros/setpoint_raw/global', GlobalPositionTarget, queue_size=1)
rospy.sleep(0.2)
##########################################
#### Now the Drone starts operation!! ####
##########################################
# # Arm the drone
modes.setArm()
rospy.sleep(1)
# # We need to send few setpoint messages, then activate OFFBOARD mode, to take effect
k=0
while k<10:
sp_pub.publish(cnt.sp)
rate.sleep()
k = k + 1
# # activate OFFBOARD mode
modes.setOffboardMode()
# # TakeOff
modes.setTakeoff(cnt.global_pos)
rospy.sleep(3)
# Update sp_glob to Target's GPS
cnt.updateSp_glob()
print(cnt.sp_glob)
rospy.sleep(1)
# # Update sp to Target's local position
# # cnt.updateSp()
# # rospy.sleep(1)
# # Move the drone to Target's GPS position
# sp_glob_pub.publish(cnt.sp_glob)
modes.setTakeoff(cnt.sp_glob)
rospy.sleep(5)
# # Move the drone to Target's local position by Camera info
# # sp_pub.publish(cnt.sp)
# # rospy.sleep(3)
# Land the drone
modes.setAutoLandMode()
rospy.sleep(3)
# Disarm the drone
modes.setDisarm()
rospy.sleep(2)
# -*- coding:utf-8 -*-
import rospy
import numpy as np
from geometry_msgs.msg import Twist, Vector3
from sensor_msgs.msg import LaserScan
dist = None
def scaneou(dado):
global dist
dist = np.array(dado.ranges)[0]
if __name__ == "__main__":
rospy.init_node("le_scan")
velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size=3)
recebe_scan = rospy.Subscriber("/scan", LaserScan, scaneou)
while not rospy.is_shutdown():
if dist < 1:
velocidade = Twist(Vector3(-0.2, 0, 0), Vector3(0, 0, 0))
velocidade_saida.publish(velocidade)
rospy.sleep(0.5)
elif dist > 1.2:
velocidade = Twist(Vector3(0.2, 0, 0), Vector3(0, 0, 0))
velocidade_saida.publish(velocidade)
rospy.sleep(0.5)
numberOfObjectDetectionRuns += 1
numberOfAllSearchedObjectTypes += len(self.searched_object_types)
rospy.loginfo("Number of object detections: " +
str(numberOfObjectDetectionRuns))
rospy.loginfo("Number of all searched_object_types: " +
str(numberOfAllSearchedObjectTypes))
rospy.loginfo("Average number of searched_object_types per run: " +
str((1.0 * numberOfAllSearchedObjectTypes /
numberOfObjectDetectionRuns)))
detectors_manager = ObjectDetectorsManager()
if detectors_manager.start_recognizers(
self.searched_object_types) is 'aborted':
return 'aborted'
# wait till object detectors will be started
rospy.sleep(2.0)
# Listen for 'detection_duration * number_of_searched_object_types' seconds for detected objects
rospy.loginfo("All requested recognizers should now publish to " +
"/stereo/objects")
wait_time = rospy.get_param("/nbv/speedFactorRecognizer") * len(
self.searched_object_types)
future = time() + wait_time
sub = rospy.Subscriber('/stereo/objects', AsrObject,
self.detection_callback)
while (time() < future):
pass
# unregister, otherwise the callbacks are still called
sub.unregister()
if detectors_manager.stop_recognizers(
def docking(self):
print "DOCKINGGGGGGGGGGGGGGGG"
# TEST 3 CASES HERE EACH WITH DIFF MOVE & ROTATE (-150 extreme)
#Case 1: camera extreme right of tgt (face tgt)
current_angle = 0
rate = rospy.Rate(0.5)
if (self.ar_logo_errx < -80 and self.range_ahead >= 0.8
and self.range_ahead != 999):
print "CASE 1"
#rotate 90 deg & move forward
while (current_angle < 2):
self.cmd_vel_pub.publish(self.twist)
rospy.sleep(1)
print "CURR ANGLE:", current_angle
self.twist.angular.z = 16.0 #4.8
current_angle = current_angle + 1
rate.sleep()
self.move(0.1, 1.0, 1)
current_angle = 0
while (current_angle < 4):
self.cmd_vel_pub.publish(self.twist)
rospy.sleep(1)
print "CURR ANGLE3:", current_angle
self.twist.linear.x = 0
self.twist.angular.z = -20.0
current_angle = current_angle + 1
rate.sleep()
self.travel_init_wp()
#Case 2: camera mid (face tgt)
elif (self.ar_logo_errx >= -80 and self.ar_logo_errx <= 110
and self.range_ahead >= 0.8 and self.range_ahead != 999):
print "CASE 2"
self.move(0.1, 0.7, 1) #(speed, distance, forward)
self.travel_init_wp()
#Case 3: camera extreme left of tgt (face tgt)
elif (self.ar_logo_errx > 110 and self.range_ahead >= 0.8
and self.range_ahead != 999):
print "CASE 3"
#rotate 90 deg & move forward
while (current_angle < 2):
self.cmd_vel_pub.publish(self.twist)
rospy.sleep(1)
print "CURR ANGLE:", current_angle
self.twist.angular.z = -16.0
current_angle = current_angle + 1
rate.sleep()
self.move(0.1, 1.0, 1)
current_angle = 0
while (current_angle < 4):
self.cmd_vel_pub.publish(self.twist)
rospy.sleep(1)
print "CURR ANGLE3:", current_angle
self.twist.linear.x = 0
self.twist.angular.z = 20.0
current_angle = current_angle + 1
rate.sleep()
self.travel_init_wp()
elif (self.range_ahead < 0.8 or self.range_ahead == 999):
print "STOP"
self.twist.linear.x = 0.0
self.twist.angular.z = 0.0
def main():
global publisher_motor, publisher_ping, publisher_servo, publisher_odom, publisher_render
global subscriber_odometry, subscriber_state
global IR_THRESHOLD, CYCLE_TIME, RENDER_LIMIT
global pose2D_sparki_odometry, servo_angle, ir_readings, ping_distance
global render_buffer
global servo_angle, display, task_complete, initial_start
#TODO: Init your node to register it with the ROS core
init()
servos = list(range(0, 90, 15))
while not rospy.is_shutdown():
#TODO: Implement CYCLE TIME
start_time = time.time()
# push ping command
publisher_ping.publish(Empty())
# MOTORS
motor_left = 1.0
motor_right = 1.0
# LOOP CLOSURE
if ir_readings['L'] < IR_THRESHOLD and ir_readings[
'C'] < IR_THRESHOLD and ir_readings['R'] < IR_THRESHOLD:
# close the loop, do relevant things
if not initial_start:
if not on_start:
rospy.loginfo("Loop Closure Triggered")
if servos:
new_servo_angle = servos.pop(0)
servo_msg = Int16(new_servo_angle)
publisher_servo.publish(servo_msg)
servo_angle = new_servo_angle
elif not task_complete:
display = True
task_complete = True
on_start = True
else:
on_start = False
initial_start = False
pass
# FOLLOW LINE
if ir_readings['C'] < IR_THRESHOLD:
motor_left = 1.0
motor_right = 1.0
else:
if ir_readings['L'] < IR_THRESHOLD:
motor_left = -1.0
motor_right = 1.0
if ir_readings['R'] < IR_THRESHOLD:
motor_left = 1.0
motor_right = -1.0
motor_msg = Float32MultiArray()
motor_msg.data = [float(motor_left), float(motor_right)]
publisher_motor.publish(motor_msg)
if render_buffer >= RENDER_LIMIT:
render_buffer = 0
publisher_render.publish(Empty())
#TODO: Implement line following code here
# To create a message for changing motor speed, use Float32MultiArray()
# (e.g., msg = Float32MultiArray() msg.data = [1.0,1.0] publisher.pub(msg))
#TODO: Implement loop closure here
#if :
# rospy.loginfo("Loop Closure Triggered")
#TODO: Implement CYCLE TIME
render_buffer += CYCLE_TIME
rospy.sleep(max(CYCLE_TIME - (start_time - time.time()), 0))
def checkoutDirt(self): # todo (rmb-ma) rospy.sleep(1.)
def speak_wait(s):
nwords = len(s.split(" "))
sperword = 0.4
publish_weather(s)
tts.say(s)
rospy.sleep(nwords * sperword)
return
self.br.sendTransform(
(self.w_P_c.position.x, self.w_P_c.position.y,
self.w_P_c.position.z),
(self.w_P_c.orientation.x, self.w_P_c.orientation.y,
self.w_P_c.orientation.z, self.w_P_c.orientation.w),
rospy.Time.now(), self.camera_frame_name, self.base_frame_name)
self.br.sendTransform(
(self.ee_P_m.position.x, self.ee_P_m.position.y,
self.ee_P_m.position.z),
(self.ee_P_m.orientation.x, self.ee_P_m.orientation.y,
self.ee_P_m.orientation.z, self.ee_P_m.orientation.w),
rospy.Time.now(), self.marker_frame_name + "_nominal",
self.robot_ee_frame_name)
#
if __name__ == '__main__':
rospy.init_node('camera_robot_calibration')
est = camera_robot_calibration_ros()
while not rospy.is_shutdown():
est.publish_tfs()
rospy.sleep(0.01)
rospy.spin()
l_leg.setup()
r_leg.setup()
#print r_leg.setup()
print "l_leg:", l_leg.get_joint_names()
print "r_leg:", r_leg.get_joint_names()
print "ankle:", ankle_angles
print len(thigh_angles)
rate = rospy.Rate(5)
#plt.plot(time, ankle_angles, 'g', lw=3)
#plt.show()
while not rospy.is_shutdown():
for i, thetha in enumerate(thigh_angles):
print "Moving left leg to: [", thigh_angles[
i], "] [", tibia_angles[i], "] [", ankle_angles[i], "]"
l_leg.set_angles(
[thigh_angles[i], tibia_angles[i], ankle_angles[i]])
print "Moving right leg to: [", thigh_angles_c[
i], "] [", tibia_angles_c[i], "] [", ankle_angles_c[i], "]"
r_leg.set_angles(
[thigh_angles_c[i], tibia_angles_c[i], ankle_angles_c[i]])
if (tibia_angles[i] > 1.2):
print('ping-------------------------------left')
if (tibia_angles_c[i] < -1.2):
print('ping-------------------------------right')
#l_leg.set_joint_states([ankle_angles[i],phalange_angles[i],thigh_angles[i],tibia_angles[i]])
#r_leg.set_joint_states([ankle_angles_c[i],phalange_angles_c[i],thigh_angles_c[i],tibia_angles_c[i]])
print "ok i=", i
print "-------------------------------------"
rospy.sleep(time_step * 1.0)
def __init__(self):
rospy.on_shutdown(self.cleanup)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
pub.publish("Waiting for move_base action server...")
# Wait for the action server to become available
self.move_base.wait_for_server(rospy.Duration(120))
rospy.loginfo("Connected to move base server")
pub.publish("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by the user in RViz
initial_pose = PoseWithCovarianceStamped()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Get the initial pose from the user
rospy.loginfo("Click the 2D Pose Estimate button in RViz to set the robot's initial pose")
pub.publish("Click the 2D Pose Estimate button in RViz to set the robot's initial pose")
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
point_pose = String()
rospy.Subscriber('lm_data', String, self.update_point_pose)
while True:
rospy.loginfo("Waiting for your order...")
pub.publish("Waiting for your order...")
rospy.sleep(3)
rospy.wait_for_message('lm_data', String)
if(self.point_pose.data == "GO TO POINT A"):
A_x = 1.8
A_y = -5.51
A_theta = 1.5708
elif(self.point_pose.data == "GO TO POINT B"):
B_x = -2.2
B_y = -2.53
B_theta = 1.5708
elif(self.point_pose.data == "GO TO POINT C"):
C_x = 1.9
C_y = 1.18
C_theta = 1.5708
rospy.loginfo("Ready to go")
pub.publish("Ready to go")
rospy.sleep(1)
locations = dict()
quaternion = quaternion_from_euler(0.0, 0.0, A_theta)
locations['A'] = Pose(Point(A_x, A_y, 0.000), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]))
self.goal = MoveBaseGoal()
rospy.loginfo("Starting navigation test")
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo("Going to point "+A_name)
pub.publish("Going to point "+A_name)
rospy.sleep(2)
self.goal.target_pose.pose = locations['A']
self.move_base.send_goal(self.goal)
waiting = self.move_base.wait_for_result(rospy.Duration(300))
if waiting == 1:
rospy.loginfo("Reached point "+A_name)
pub.publish("Reached point "+A_name)
rospy.sleep(2)
def processImage(self, rosImage):
# Discard old images
#rospy.loginfo( "times" + str(rosImage.header.stamp) +","+ str(rospy.Time.now()))
if rospy.Time.now() - rosImage.header.stamp < rospy.Duration(0.5):
# rospy.loginfo("Received image")
try:
img = self.bridge.imgmsg_to_cv(rosImage, desired_encoding="passthrough")
except CvBridgeError, e:
print "Macana:", e
img = np.asarray(img)
# rangeFiltered = inRange(img, np.asarray([0,0,0]), np.asarray([200,100,100]))
# gaussian_blur = GaussianBlur(rangeFiltered,(201,201),10)
# retval, thrs = threshold(gaussian_blur, 200, 255, THRESH_BINARY)
# img = bitwise_and(img, img, mask=thrs)
canny = Canny(img, 100, 200)
lines = HoughLinesP(canny, 1, cv.CV_PI/180, 50, minLineLength=10,maxLineGap=3)
# rospy.loginfo("Processed Image")
# if (lines != None):
# for l in lines[0]:
# #rospy.loginfo(l)
# line(img, (l[0], l[1]),(l[2],l[3]), cv.CV_RGB(255, 0, 0), 3, 8)
if lines <> None:
lines, lineMarkers = self.transformLines(lines, rosImage.header.stamp)
# rospy.loginfo("Transformed Points")
self.publishLines(lineMarkers,lines, rosImage.header.stamp)
# rospy.loginfo("Marker published")
else:
self.publishLines([],[],rosImage.header.stamp)
rospy.sleep(.5)