def __init__(self):
rospy.init_node('fake_move_base')
self.result = MoveBaseActionResult()
self.feedback = MoveBaseActionFeedback()
self.fmb = actionlib.SimpleActionServer('move_base', MoveBaseAction,
self.execute_cb)
def __init__(self):
self.costmap = OccupancyGrid()
self.flagm = 0
self.flagg = 0
self.init = 0
self.newcost = []
self.grid = OccupancyGrid()
self.feedback = MoveBaseActionFeedback()
self.prevpnt = -1
self.nextpnt = 0
self.sample = []
self. stuckpnt = []
self.isstuck = False
self.offsetcnt = 0
self.firstgoal = 0
self.checkpnt = 0
self.pntlist = []
self.poly = []
self.polynum = 0
self.polydone = False
self.polypath = []
self.count_id = 1
self.send = True
self.currentpos =[]
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.mapCb,queue_size=1)
self.global_sub = rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid, self.costCb,queue_size=1)
self.globalup_sub = rospy.Subscriber('/move_base/global_costmap/costmap_updates', OccupancyGridUpdate, self.costupCb,queue_size=1)
self.result_sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.sendNavCb)
self.feedback_sub = rospy.Subscriber('move_base/feedback', MoveBaseActionFeedback, self.feedbackCb)
self.poly_sub = rospy.Subscriber('/clicked_point',PointStamped,self.polyCb,queue_size=1)
self.pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1)
self.line_pub = rospy.Publisher('visualization_marker',Marker,queue_size = 10)
def __init__(self):
rospy.init_node("move_turtle_node")
vel_topic = rospy.get_param("~cmd_vel_topic", "/turtle1/cmd_vel")
position_topic = rospy.get_param("~pose_topic", "/turtle1/pose")
self._action_name = rospy.get_param("~action_name", "/move_turtle")
self.robot_speed = rospy.get_param("~robot_max_speed", 1.5) #m/s
self.pull_point = rospy.get_param("~pull_distance", 0.3) # m
self.kp = rospy.get_param("~controller_gain", 10.0)
self.plot_enable = rospy.get_param("~plot_enable", False)
self.no_points = rospy.get_param("~path_number_of_points", 100)
hz = rospy.get_param("~rate", 200)
self.position = [0.0, 0.0]
self.orientation = 0.0
self.__sub = rospy.Subscriber(position_topic, Pose, self.__callback)
self.pub = rospy.Publisher(vel_topic, Twist, queue_size=10)
self.speed_msg = Twist()
self.rate = rospy.Rate(hz)
self._feedback = MoveBaseActionFeedback()
self._result = MoveBaseActionResult()
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.goTo,
auto_start=False)
self._as.start()
flag = True
# 进行判断,如果在半米内有已经存的点,则不存
for it in points:
if (it.x - people.x)**2 + (it.y - people.y)**2 < 0.25:
flag = False
break
if flag:
rospy.loginfo("ADD POS X:%f Y:%f" % (people.x, people.y))
points.append(people)
wiw_pub.publish(people)
# 更新move_base返回的机器所在的绝对坐标
def run2(data):
global feedbackData
feedbackData = data
#rospy.loginfo("updata feedbackData")
if __name__ == "__main__":
rospy.init_node("nc_wiw_sive_point")
wiw_pub = rospy.Publisher('Kinect/wiw_point_save', Point, queue_size=10)
pointData = Point()
feedbackData = MoveBaseActionFeedback()
points = list()
save_point_pub = rospy.Subscriber("/Kinect/wiw_point", Point, run1)
move_base_feedback_pub = rospy.Subscriber("/move_base/feedback",
MoveBaseActionFeedback, run2)
rospy.spin()
sample = []
newcost = []
flagm = 0
flagg = 0
init = 0
prevpnt = 0
offsetcnt = 0
stuckpnt = []
firstgoal=0
checkpnt = 0
cost_update = OccupancyGridUpdate()
costmap = OccupancyGrid()
grid = OccupancyGrid()
feedback = MoveBaseActionFeedback()
result = MoveBaseActionResult()
newPose = PoseStamped()
q = Quaternion()
pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=30)
def costupCb(data):
global flagg, flagm
global init
global newcost
flagg = 1
cost_update = data
def handle_picture_taking(self, subject):
thingy = MoveBaseActionFeedback()
thingy.feedback.base_position.pose.position.x = 2.0
thingy.feedback.base_position.pose.position.y = 2.0
thingy.feedback.base_position.pose.orientation.w = 1.0
self.pub_dummy.publish(thingy)
rospy.loginfo('Seen something')
rospy.loginfo(subject)
#############################################################################
if 'door' in subject: #Door option: [open, closed] door BETWEEN [room] and [room]
if 'open' in subject:
open_status = 'true'
else:
open_status = 'false'
if 'bedroom' in subject:
thing_id = 'door_bedroom'
room1 = 'bedroom'
room2 = 'living_room'
if 'bathroom' in subject:
thing_id = 'door_bathroom'
room1 = 'bathroom'
room2 = 'kitchen'
if 'hall' in subject:
thing_id = 'door_hall'
room1 = 'hall'
room2 = 'kitchen'
else:
thing_id = 'door_entrance'
room1 = 'hall'
room2 = 'outside'
line1 = self.linewriter('type', [thing_id, 'door'])
line2 = self.linewriter('connects', [thing_id, room1, room2])
line3 = self.linewriter('isOpen', [thing_id, open_status])
to_write = [line1, line2, line3]
#############################################################################
elif subject[
0] in self.objects: #[object] in [room] (room) on [furniture] (furniture)
if len(subject) < 5:
self.pub_talk.publish("Invalid object please try again")
thing_id = subject[0] #TODO add matching to IDs
if thing_id == 'gum':
thing_id = 'chewing gum'
position_string = self.object_position() #TODO get actual position
self.pub_pic.publish(subject[0] + '.jpg')
item = thing_id
if subject[3] == 'room':
room = subject[2] + '_' + subject[3]
subject.pop(3)
else:
room = subject[2]
if len(subject) == 6:
furniture = subject[4] + '_' + subject[5]
elif len(subject) == 7:
furniture = subject[4] + '_' + subject[5] + '_' + subject[6]
else:
furniture = subject[4]
line1 = self.linewriter('type', [thing_id, item])
line2 = self.linewriter('in', [thing_id, room])
line3 = self.linewriter('on', [thing_id, furniture])
line4 = self.linewriter('position ', [thing_id, position_string])
line6 = self.linewriter('picture', [thing_id, thing_id + '.jpg'])
to_write = [line1, line2, line3, line4, line6]
###################################################################################
elif subject[
0] in self.furniture: # [couch, bed, chair, lamp (furniture)] in [room]
if subject[1] == 'in':
thing_id = subject[0]
else:
thing_id = subject[0] + '_' + subject[1]
subject.pop(1)
if len(subject) == 4:
room = subject[2] + '_' + subject[3]
else:
room = subject[2]
if thing_id == 'armchair':
thing_id = 'arm_chair'
if thing_id == 'night' or thing_id == 'night_stand':
thing_id = 'nightstand'
line1 = self.linewriter('type', [thing_id, thing_id])
line2 = self.linewriter('in', [thing_id, room])
to_write = [line1, line2]
else:
self.pub_talk.publish("Invalid noun please try again")
return
f = open(self.outfolder + 'sementic_map.txt', 'a+')
for line in to_write:
f.write(line + '\n')
rospy.loginfo(line)
f.close()