def robot_state_publisher(self):
if self.current_robot_status['ready']:
state_num = 0
if self.current_robot_status['busy']:
state_num = 1
if self.current_robot_status['direct_teaching']:
state_num = 2
if self.current_robot_status['collision']:
state_num = 3
if self.current_robot_status['emergency']:
state_num = 4
status_msg = GoalStatusArray()
status_msg.header.stamp = rospy.Time.now()
status = GoalStatus()
status.goal_id.stamp = rospy.Time.now()
status.goal_id.id = ""
status.status = state_num
status.text = ROBOT_STATE[state_num]
status_msg.status_list = [status]
self.indy_state_pub.publish(status_msg)
def navToPose(goal):
print "Starting Navigation!"
# get path from A*
while not stopDrive:
# find the global path
globalPathServ = getGlobalPath(navBot.cur.pose, goal.pose)
globalPath = globalPathServ.path
if globalPath.poses: # if there's something in the list of poses
print " Global Navigation"
navBot.rotateTowardPose(globalPath.poses[0]) # rotate to update local map
localPathServ = getLocalPath(navBot.cur.pose, globalPath.poses[0].pose)
localPath = localPathServ.path
while not localPath.poses: # if there's nothing in the list of poses
print " - blocked global waypoint, moving to next"
del globalPath.poses[0]
if not globalPath.poses:
print " - no valid path, ending navigation"
return
localPathServ = getLocalPath(navBot.cur.pose, globalPath.poses[0].pose)
localPath = localPathServ.path
print " Local Navigation"
navBot.goToPose(localPath.poses[0])
else:
print " - goal not navigable, ending navigation"
break
gridDist = math.sqrt((globalPath.poses[-1].pose.position.x - navBot.cur.pose.position.x)**2 + (globalPath.poses[-1].pose.position.y - navBot.cur.pose.position.y)**2)
if (len(globalPath.poses) == 1 and len(localPath.poses) == 1) or gridDist <= navBot.distThresh * 3:
print " Finished All Navigation!"
break
navBot.rotateTo(getAngleFromPose(goal.pose))
statuses = GoalStatusArray()
status = GoalStatus()
status.status = 3
statuses.status_list.append(status)
status_pub.publish(statuses)
def PubCancleRequest(self, ID, data): #pub cancle request to planner
CanclePub = rospy.Publisher('/%s/cancle' % self.ControlBase,
GoalStatus,
queue_size=1)
PubData = GoalStatus()
PubData.goal_id = int(ID.split('/')[1])
PubData.status = data
CanclePub.publish(PubData)
def make_goal_array(self):
for path in range(0, len(self.path_points) - 1):
new_goal = GoalStatus()
new_goal.goal_id.id = str(path)
new_goal.status = 0
new_goal.text = 'PENDING'
self.goal_arr.status_list.append(new_goal)
def goal_status_array(cls, msg, obj):
obj.header = decode.header(msg, obj.header, "")
for i in range(msg['_length']):
goal_status = GoalStatus()
goal_status.goal_id.stamp = decode.time_stamp(msg,
goal_id.stamp, i)
goal_status.goal_id.id = msg['%s_id' % i]
goal_status.status = msg['%s_status' % i]
goal_status.text = msg['%s_text' % i]
obj.status_list.append(goal_status)
return(obj)
def navToPose(goal):
print
"Starting Navigation!"
# get path from A*
while not stopDrive:
# find the global path
globalPathServ = getGlobalPath(navBot.cur.pose, goal.pose)
globalPath = globalPathServ.path
if globalPath.poses: # if there's something in the list of poses
print
" Global Navigation"
navBot.rotateTowardPose(
globalPath.poses[0]) # rotate to update local map
localPathServ = getLocalPath(navBot.cur.pose,
globalPath.poses[0].pose)
localPath = localPathServ.path
while not localPath.poses: # if there's nothing in the list of poses
print
" - blocked global waypoint, moving to next"
del globalPath.poses[0]
if not globalPath.poses:
print
" - no valid path, ending navigation"
return
localPathServ = getLocalPath(navBot.cur.pose,
globalPath.poses[0].pose)
localPath = localPathServ.path
print
" Local Navigation"
navBot.goToPose(localPath.poses[0])
else:
print
" - goal not navigable, ending navigation"
break
gridDist = math.sqrt((globalPath.poses[-1].pose.position.x -
navBot.cur.pose.position.x)**2 +
(globalPath.poses[-1].pose.position.y -
navBot.cur.pose.position.y)**2)
if (len(globalPath.poses) == 1 and len(localPath.poses)
== 1) or gridDist <= navBot.distThresh * 3:
print
" Finished All Navigation!"
break
navBot.rotateTo(getAngleFromPose(goal.pose))
statuses = GoalStatusArray()
status = GoalStatus()
status.status = 3
statuses.status_list.append(status)
status_pub.publish(statuses)
def updateGripperPosition(self, msg):
print "Commanding", self.arm_name, 'gripper to', str(msg.data)
self.pause_control_loop = True # pause the control loop to free the serial bus
rospy.sleep(1.0)
angles = {self.arm_name+'_joint1':0.0, self.arm_name+'_joint2':0.0,self.arm_name+'_joint3':0.0, \
self.arm_name+'_joint4':0.0,self.arm_name+'_joint5':0.0,self.arm_name+'_joint6':0.0,self.arm_name+'_gripper':0.0}
vels = {self.arm_name+'_joint1':0.0, self.arm_name+'_joint2':0.0,self.arm_name+'_joint3':0.0, \
self.arm_name+'_joint4':0.0,self.arm_name+'_joint5':0.0,self.arm_name+'_joint6':0.0,self.arm_name+'_gripper':0.0}
angles[self.arm_name+"_gripper"] = int(msg.data)
vels[self.arm_name+"_gripper"] = 20.0
msg = GoalStatus()
msg.status = msg.ACTIVE
self.gripper_result_pub.publish(msg)
self.commandAllJointAngles(angles, vels)
msg.status = msg.SUCCEEDED
self.gripper_result_pub.publish(msg)
rospy.sleep(0.1)
msg.status = msg.PENDING
self.gripper_result_pub.publish(msg)
def fake_auto_demo():
rospy.init_node('autonomous_demo23')
#Requirements
pub1 = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10)
pub2 = rospy.Publisher('/imu/data', Imu, queue_size=10)
pub3 = rospy.Publisher('/odometry/wheel', Odometry, queue_size=10)
pub4 = rospy.Publisher('/gps_waypoint_handler/status/gps/fix',
String,
queue_size=10)
pub5 = rospy.Publisher('/pass_gate_topic', String, queue_size=10)
#Autonomous movement
#pub5 = rospy.Publisher('/move_base/status',GoalStatusArray,queue_size=10)
#Image Detect
pub6 = rospy.Publisher('/artag_detect_topic', String, queue_size=10)
#Image Reach
pub7 = rospy.Publisher('/artag_reach_topic', String, queue_size=10)
pub8 = rospy.Publisher('/done_app_topic', String, queue_size=10)
rate = rospy.Rate(10) # 10hz
count = 0
while not rospy.is_shutdown():
print("0: All sensors are good.")
print("1: All sensors except encoder are good.")
print("2: Damaged Sensors")
print("3: Got Waypoint")
print("4: Did not get any waypoint")
print("5: Reached to way point")
print("6: Did not Reached to way point")
print("7: Detected AR Tag")
print("8: Did not detect AR Tag")
print("9: Reached AR Tag")
print("10: Did not reached AR Tag")
imuMsg = Imu()
imuMsg.orientation.x = 5
imuMsg.orientation.y = 5
gpsMsg = NavSatFix()
gpsMsg.latitude = 5
gpsMsg.longitude = 5
encoderMsg = Odometry()
encoderMsg.pose.pose.position.x = 5
encoderMsg.pose.pose.position.y = 5
wpStatusMsgLow = GoalStatus()
wpStatusMsgLow.status = 3
wpStatusArray = []
wpStatusArray.append(wpStatusMsgLow)
wpStatusMsg = GoalStatusArray()
wpStatusMsg.status_list = wpStatusArray
userInput = raw_input()
if userInput == "0": #All sensors are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
pub3.publish(encoderMsg)
elif userInput == "1": # All sensors except encoder are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
#pub3.publish("0")
elif userInput == "2": #Damaged Sensors
pub1.publish(gpsMsg)
#pub2.publish("0")
pub3.publish(encoderMsg)
elif userInput == "3": # Got Waypoint
pub4.publish("1")
elif userInput == "4": #Did not get any waypoint
pub4.publish("0")
elif userInput == "5": #Reached to way point
#pub5.publish(wpStatusMsg)
pub4.publish("2")
#elif userInput == "6": #Did not reached to way point
#pub5.publish("0")
elif userInput == "7": #Detected Image
pub6.publish("1")
#pub5.publish("1")
elif userInput == "8": #Did not Detect Image
pub6.publish("0")
elif userInput == "9": #Reached Image
pub7.publish("1")
elif userInput == "10": #Did not reached image
pub7.publish("0")
elif userInput == "11":
pub5.publish("1")
elif userInput == "12":
pub8.publish("1")
else:
print("Invalid entry")
rate.sleep()
def PubCancleRequest(self, ID, data): #pub cancle request to planner
CanclePub = rospy.Publisher('/%s/cancle'%self.ControlBase, GoalStatus, queue_size = 1)
PubData = GoalStatus()
PubData.goal_id = int(ID.split('/')[1])
PubData.status = data
CanclePub.publish(PubData)