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 publish_goal(self, x=0, y=0, z=0, yaw=0, roll=0, pitch=0):
# Create move_base goal
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = rospy.get_param('~frame_id', 'map')
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = z
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
rospy.loginfo(
'Executing move_base goal to position (x,y) %s, %s, with %s degrees yaw.'
% (x, y, yaw))
rospy.loginfo(
"To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'"
)
# Send goal
self.move_base.send_goal(goal)
rospy.loginfo('Inital goal status: %s' %
GoalStatus.to_string(self.move_base.get_state()))
status = self.move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
# Wait for goal result
self.move_base.wait_for_result()
rospy.loginfo('Final goal status: %s' %
GoalStatus.to_string(self.move_base.get_state()))
status = self.move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
def navigation_goal_to(self, x, y):
# Create move_base goal
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1
rospy.loginfo('Executing move_base goal to position (x,y) %s, %s.' %
(goal.target_pose.pose.position.x,
goal.target_pose.pose.position.y))
rospy.loginfo(
"To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'"
)
# Send goal
self.move_base.send_goal(goal)
rospy.loginfo('Inital goal status: %s' %
GoalStatus.to_string(self.move_base.get_state()))
status = self.move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
# Wait for goal result
self.move_base.wait_for_result()
rospy.loginfo('Final goal status: %s' %
GoalStatus.to_string(self.move_base.get_state()))
status = self.move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
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 start_job(self, job):
result = TransportResult()
try:
goal = job.get_goal()
color = goal.specification
item = self.warehouse.get(color)()
if item.available:
rospy.loginfo( 'Call stacker for job: ' + str([item.x, item.z, goal.address, 1]) )
self.current_gh = self.stacker.send_goal( StackerGoal([item.x, item.z, goal.address, 1]) )
while not rospy.is_shutdown(): # wait for stacker complete its job
goal_status = self.current_gh.get_goal_status()
rospy.logdebug( 'Goal status: ' + str(goal_status) )
if goal_status > 2: # actionlib_msgs/GoalStatus
rospy.loginfo( 'Goal status: ' + str(goal_status) )
break
rospy.sleep(1)
result.act = 'Job %s %s' % ( str(job.get_goal_id()), GoalStatus.to_string(self.current_gh.get_terminal_state()) )
job.set_succeeded(result)
else:
result.act = 'Spec %s is not available' % color
job.set_aborted(result)
rospy.logwarn('Job aborted')
except Exception:
rospy.logwarn('Exception arise!')
finally:
rospy.loginfo('Finishing')
self.finish()
self.make_bids()
self.busy = False
def __init__(self):
self.ang_fwd_threshold = rospy.get_param('~ang_fwd_threshold', 0.5)
self.ang_vel_intersection = rospy.get_param('~ang_vel_intersection',
1.82)
self.dist_alarm_1 = rospy.get_param('~dist_alarm_1', 5.0)
self.dist_alarm_2 = rospy.get_param('~dist_alarm_2', 3.0)
self.spin_cycle = rospy.Duration(rospy.get_param('~spin_cycle', 0.3))
self.plan_cycle = rospy.Duration(rospy.get_param('~plan_cycle', 1.0))
print(C_YELLO + '\rTask planner, GVG 서비스 확인중...' + C_END)
rospy.wait_for_service('gvg/nearest')
rospy.wait_for_service('gvg/neighbors')
rospy.wait_for_service('gvg/node')
self.get_nearest = rospy.ServiceProxy('gvg/nearest', Nearest)
self.get_neighbors = rospy.ServiceProxy('gvg/neighbors', Neighbors)
self.get_node = rospy.ServiceProxy('gvg/node', Node)
print(C_YELLO + '\rTask planner, GVG 서비스 확인 완료' + C_END)
print(C_YELLO + '\rTask planner, 자율주행 서비스 확인중...' + C_END)
self.publisher_cmd_vel = rospy.Publisher('cmd_vel',
Twist,
queue_size=1)
self.publisher_robot_motion = rospy.Publisher('interf/robot/motion',
RobotMotion,
queue_size=1)
self.publisher_nav_cue = rospy.Publisher('interf/nav_cue',
NavCue,
queue_size=1)
print(C_YELLO + '\rTask planner, Action 서비스 확인중...' + C_END)
self.move_result = GoalStatus()
self.move_result.status = 3
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
self.percussion_time = rospy.get_time()
print(C_YELLO + '\rTask planner, 토픽 구독중...' + C_END)
rospy.Subscriber('interf/cmd/assist', CmdAssist, self.percussion)
rospy.Subscriber('robot/pose', PoseWithCovarianceStamped,
self.update_robot_pose)
rospy.Subscriber('move_base/result', MoveBaseActionResult,
self.update_move_result)
self.time_start = rospy.Time.now()
rospy.Subscriber('time/start', Time, self.update_time)
print(C_YELLO + '\rTask planner, 초기자세로 이동중...' + C_END)
self.robot_state = S_INDIRECT_WAIT
self.mount_gvg()
print(C_GREEN + '\rTask planner, 자율주행 서비스 초기화 완료' + C_END)
print(C_YELLO + '\rTask planner, BCI 서비스 확인중...' + C_END)
rospy.wait_for_service('interf/nav2cmd')
self.get_cmd = rospy.ServiceProxy('interf/nav2cmd', Nav2Cmd)
print(C_YELLO + '\rTask planner, BCI 서비스 확인 완료' + C_END)
print(C_YELLO + '\rTask planner, 기동중...' + C_END)
rospy.Timer(self.plan_cycle, self.explosion)
print(C_GREEN + '\rTask planner, 초기화 완료\n' + C_END)
def execute_policy_cb(self, goal):
self.cancelled = False
self.policy_mdp = self.policy_utils.generate_policy_mdp(
goal.spec, self.closest_waypoint, rospy.Time.now())
if self.policy_mdp is None:
rospy.logerr("Failure to build policy for specification: " +
goal.spec.ltl_task)
self.mdp_as.set_aborted()
return
self.publish_feedback(None, None, self.policy_mdp.get_current_action())
if self.nav_before_action_exec:
starting_exec = True #used to make sure the robot executes calls topological navigation at least once before executing non-nav actions. This is too ensure the robot navigates to the exact pose of a waypoint before executing an action there
else:
starting_exec = False
while (self.policy_mdp.has_action_defined()
and not self.cancelled) or starting_exec:
next_action = self.policy_mdp.get_current_action()
if next_action in self.mdp.nav_actions or starting_exec:
starting_exec = False
current_nav_policy = self.policy_utils.generate_current_nav_policy(
self.policy_mdp)
status = self.execute_nav_policy(current_nav_policy)
rospy.loginfo(
"Topological navigation execute policy action server exited with status: "
+ GoalStatus.to_string(status))
if status != GoalStatus.SUCCEEDED:
self.policy_mdp.set_current_state(None)
break
else:
print("EXECUTE ACTION")
(status, state_update) = self.action_executor.execute_action(
self.mdp.action_descriptions[next_action])
executed_action = next_action
print(executed_action)
if not self.cancelled:
next_flat_state = self.policy_utils.get_next_state_from_action_outcome(
state_update, self.policy_mdp)
self.policy_mdp.set_current_state(next_flat_state)
if next_flat_state is None:
rospy.logerr(
"Error finding next state after action execution. Aborting..."
)
break
next_action = self.policy_mdp.get_current_action()
self.publish_feedback(executed_action, status, next_action)
if self.cancelled:
self.cancelled = False
rospy.loginfo("Policy execution preempted.")
self.mdp_as.set_preempted()
elif self.policy_mdp.current_flat_state is None:
rospy.loginfo("Policy execution failed.")
self.mdp_as.set_aborted()
else:
rospy.loginfo("Policy execution successful.")
self.mdp_as.set_succeeded()
def done_cb(self, state, r):
rospy.loginfo(
gstr({
'done state': GoalStatus.to_string(state),
'elapsed time': time.time() - self.start,
'loss': r.loss
}))
if state == GoalStatus.SUCCEEDED:
self.queue.put(r)
def waitOnGripperSuccess(self, lr): msg = GoalStatus() if lr == 'left' or lr == 'l': while(self.left_gripper_result != msg.SUCCEEDED): rospy.sleep(0.1) self.left_gripper_result = -1 if lr == "right" or lr == 'r': while(self.right_gripper_result != msg.SUCCEEDED): rospy.sleep(0.1) self.right_gripper_result = -1
def move_goal(self, point, frame_time=0):
'''
指定されたゴール地点に移動する
Parameters
----------
point : geometry_msgs/Point
カメラ座標系のオブジェクト位置
Returns
-------
client.get_result : Bool
actionlibの実行結果
'''
# tfを取得
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
try:
# カメラ → ベースリンクへの座標変換を取得
trans1 = tfBuffer.lookup_transform('base_link', self.cam_frame,
rospy.Time(0),
rospy.Duration(3.0))
# ベースリンク → マップへの座標変換を取得
trans2 = tfBuffer.lookup_transform('map', 'base_link',
rospy.Time(0),
rospy.Duration(3.0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
print(e)
else:
goal = self.transform_pose(
self.calc_goal(self.transform_point(point, trans1)), trans2)
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.stamp = rospy.Time.now()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose = goal
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
#rospy.loginfo('connected to server')
client.send_goal(goal_pose)
rospy.loginfo('Send goal pos : [%5.3f, %5.3f, %5.3f]',
goal.position.x, goal.position.y, goal.position.z)
self.set_marker(goal_pose)
#client.wait_for_result()
client.wait_for_result(rospy.Duration(10))
rospy.loginfo(client.get_state())
#http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html
Status = GoalStatus()
if client.get_state() != Status.SUCCEEDED:
client.cancel_goal()
rospy.loginfo('Failed to Move')
else:
rospy.loginfo('Succeed to Move')
return #client.get_state()
def print_status(status):
g_status = GoalStatus()
if status == g_status.PREEMPTED:
print("PREEMPTED")
elif status == g_status.ABORTED:
print("ABORTED")
elif status == g_status.REJECTED:
print("REJECTED")
elif status == g_status.SUCCEEDED:
print("SUCCEEDED")
def __init__(self, action_goal, transition_cb, feedback_cb, send_goal_fn,
send_cancel_fn):
self.action_goal = action_goal
self.transition_cb = transition_cb
self.feedback_cb = feedback_cb
self.send_goal_fn = send_goal_fn
self.send_cancel_fn = send_cancel_fn
self.state = CommState.WAITING_FOR_GOAL_ACK
self.mutex = threading.RLock()
self.latest_goal_status = GoalStatus(status=GoalStatus.PENDING)
self.latest_result = None
def __init__(self):
self.feedback = ProjectedGraspingFeedback()
self.result = ProjectedGraspingResult()
self.goal_received = False
self.object_to_grasp = False
self.action_name = 'projected_grasping_server'
self.action_server = actionlib.ActionServer(self.action_name,
ProjectedGraspingAction,
self.action_callback,
self.cancel_cb,
auto_start=False)
self.action_server.start()
self.action_status = GoalStatus()
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 __init__(self, name, anonymous=False):
super(CalibrationMasterNode, self).__init__(name, anonymous=anonymous)
self._replies = {}
self._received = 0
self._status = GoalStatus()
self._result = CalibrationResult()
self._server = actionlib.SimpleActionServer("/calibration/server",
CalibrationAction,
execute_cb=self._process,
auto_start=False)
self._server.register_preempt_callback(self._cancel)
self._server.start()
def move_pose(self, pose_map):
'''
指定されたゴール姿勢に移動する
Parameters
----------
pose_map : geometry_msgs/Pose
map座標系のポーズ
Returns
-------
ret: int
actionlibの実行結果 0:succeed 1:failed
'''
goal = pose_map
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.stamp = rospy.Time.now()
goal_pose.target_pose.header.frame_id = self.map_frame
goal_pose.target_pose.pose = goal
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()
# rospy.loginfo('connected to server')
client.send_goal(goal_pose)
rospy.loginfo('Send goal pos : [%5.3f, %5.3f, %5.3f]', goal.position.x,
goal.position.y, goal.position.z)
self.set_marker(goal_pose)
client.wait_for_result(rospy.Duration(10))
# http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html
Status = GoalStatus()
while (client.get_state() != Status.SUCCEEDED):
if ((client.get_state() != Status.ACTIVE)
or (client.get_state() != Status.SUCCEEDED)):
client.wait_for_result(rospy.Duration(5)) # timeout時間を設定
if (client.get_state() != Status.ACTIVE):
break
client.wait_for_result(rospy.Duration(1))
if (client.get_state() == Status.SUCCEEDED):
rospy.loginfo('Succeed to Move')
ret = 0
else:
client.cancel_goal()
rospy.loginfo('Failed to Move')
ret = 1
return ret
def __init__(self):
self._feedback = RequestTrajectoryFeedback()
self._result = RequestTrajectoryResult()
self.goal_received = False
self._action_name = 'request_trajectory'
self.action_server = actionlib.ActionServer(self._action_name,
RequestTrajectoryAction,
self.action_callback,
self.cancel_cb,
auto_start=False)
self.action_server.start()
self.action_status = GoalStatus()
# Variable definition
self.grip_left = 'left_gripper_tool_frame'
self.grip_right = 'right_gripper_tool_frame'
self.gripper = self.grip_right
self.frame_base = 'base_link'
self.nJoints = 8 + 3
self.arm = 'left'
self.joint_values = np.empty([self.nJoints])
self.joint_values_kdl = kdl.JntArray(self.nJoints)
self.eef_pose = kdl.Frame()
self.far = False
# self.old_error = [1.0, 1.0, 1.0]
# TODO: find appropriate max acceleration
self.accel_max = np.array(
[0.05, 0.05, 0.1, 0.01, 0.64, 0.64, 0.75, 0.75, 0.75, 1.05, 1.05])
#self.accel_max = np.array([0.3, 0.3, 0.3, 1.02, 1.9, 1.9, 1.95, 1.95, 1.95, 2.05, 2.05])
joint_topic = rospy.get_param('joint_topic')
rospy.Subscriber(joint_topic, JointState, self.joint_callback)
self.pub_clock = rospy.Publisher('/simulator/projection_clock',
ProjectionClock,
queue_size=3)
self.pub_velocity = rospy.Publisher('/simulator/commands',
JointState,
queue_size=3)
self.pub_plot = rospy.Publisher('/data_to_plot',
PoseArray,
queue_size=10)
print 'Ready'
# Wait until goal is received
rospy.Timer(rospy.Duration(0.05), self.generate_trajectory)
def main():
rospy.init_node("autonomy")
client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for robot action server")
client.wait_for_server()
rospy.loginfo("Server found")
while True:
failure = True
while failure:
try:
x = float(
input("Enter x-coordinate of where you want to go\n"))
y = float(
input("Enter y-coordinate of where you want to go\n"))
input("Hit <Enter> when you are ready")
failure = False
except ValueError:
print("Oops enter a valid value\n")
failure = True
rospy.loginfo("Sent")
action_goal = MoveBaseActionGoal()
action_goal.goal.target_pose.header.frame_id = "map"
action_goal.goal.target_pose.pose.position.x = x
action_goal.goal.target_pose.pose.position.y = y
action_goal.goal.target_pose.pose.orientation.w = 1
client.send_goal(action_goal.goal)
input("Press <Enter> to stop robot or continue")
if client.get_state() == GoalStatus.ACTIVE:
rospy.loginfo("Canceling action")
client.cancel_goal()
try:
print("result: ", GoalStatus.to_string(client.get_state()))
except Exception as e:
print(e)
while not rospy.is_shutdown():
rospy.spin()
def rasterDone(self, status, result):
if status == GoalStatus().SUCCEEDED:
if result.max_concentration > self.max_conc_val:
self.max_conc_val = result.max_concentration
self.max_conc_at.x, self.max_conc_at.y, self.max_conc_at.z = result.max_concentration_point
self.raster_scan_complete = True
if self.initial_scan_complete:
rospy.loginfo(
"Non initial Raster Scan Complete. Following Wind")
self.algorithm = self.FOLLOW_WIND
self.lost_plume_max = 1
else:
rospy.loginfo("Initial Raster scan complete")
self.initial_scan_complete = True
else:
rospy.logerr("Raster Scan not completed. Status = %s" %
status.text)
def update_status(self, status_array):
with self.mutex:
if self.state == CommState.DONE:
return
status = _find_status_by_goal_id(status_array,
self.action_goal.goal_id.id)
# You mean you haven't heard of me?
if not status:
if self.state not in [
CommState.WAITING_FOR_GOAL_ACK,
CommState.WAITING_FOR_RESULT, CommState.DONE
]:
self._mark_as_lost()
return
self.latest_goal_status = status
# Determines the next state from the lookup table
if self.state not in _transitions:
rospy.logerr("CommStateMachine is in a funny state: %i" %
self.state)
return
if status.status not in _transitions[self.state]:
rospy.logerr(
"Got an unknown status from the ActionServer: %i" %
status.status)
return
next_state = _transitions[self.state][status.status]
# Knowing the next state, what should we do?
if next_state == NO_TRANSITION:
pass
elif next_state == INVALID_TRANSITION:
rospy.logerr("Invalid goal status transition from %s to %s" %
(CommState.to_string(self.state),
GoalStatus.to_string(status.status)))
else:
if hasattr(next_state, '__getitem__'):
for s in next_state:
self.transition_to(s)
else:
self.transition_to(next_state)
def start_job(self, job):
rospy.logdebug('Storage.start_job: ' + str(job))
result = TransportResult()
try:
goal = job.get_goal()
color = goal.specification
cell = self.warehouse.get(color)()
if cell.available:
self.conveyor_destination('warehouse')
self.conveyor_load(
goal.address
) # plant must already wait for low level unload premission
rospy.logdebug('Address: ' + str(goal.address))
while not rospy.is_shutdown():
product = self.conveyor_product_ready()
if product.ready:
break
rospy.sleep(1)
self.current_gh = self.stacker.send_goal(
StackerGoal([12, 1, cell.x,
cell.z])) # [12, 1] is conveyor
while not rospy.is_shutdown(
): # wait for stacker complete its job
goal_status = self.current_gh.get_goal_status()
if goal_status > 2: # actionlib_msgs/GoalStatus
break
rospy.logdebug('Stacker goal status: ' + str(goal_status))
rospy.sleep(1)
result.act = 'Job %s %s' % (
str(job.get_goal_id()),
GoalStatus.to_string(self.current_gh.get_terminal_state()))
job.set_succeeded(result)
else:
self.conveyor_destination('garbage')
self.conveyor_load(goal.address)
result.act = 'Place for %s is not available. Throwing to garbage.' % color
rospy.sleep(10) # wait until chunk throwed down
job.set_succeeded(result)
finally:
self.finish()
self.make_bids()
self.busy = False
def _send_command(self, client, goal, blocking=False, timeout=None):
if client not in self.clients:
return False
self.clients[client].send_goal(goal)
start_time = rospy.Time.now()
# rospy.loginfo(goal)
rospy.sleep(0.1)
if self.COMMAND_LOGS:
rospy.loginfo("Command sent to %s client" % client)
if not blocking: # XXX why isn't this perfect?
return None
status = 0
end_time = rospy.Time.now() + rospy.Duration(timeout + 0.1)
while (not rospy.is_shutdown()) and \
(rospy.Time.now() < end_time) and \
(status < GoalStatus.SUCCEEDED) and \
(type(self.clients[client].action_client.last_status_msg) != type(None)):
status = self.clients[
client].action_client.last_status_msg.status_list[
-1].status # XXX get to 80
self.rate.sleep()
# It's reporting time outs that are too early
# FIXED: See comments about rospy.Time(0)
text = GoalStatus.to_string(status)
if GoalStatus.SUCCEEDED <= status:
if self.COMMAND_LOGS:
rospy.loginfo("Goal status {} achieved. Exiting".format(text))
else:
if self.ERROR_LOGS:
rospy.loginfo("Ending due to timeout {}".format(text))
result = self.clients[client].get_result()
elapsed_time = (rospy.Time.now() - start_time).to_sec()
print('Executed in {:.3f} seconds. Predicted to take {:.3f} seconds.'.
format(elapsed_time, timeout))
#state = self.clients[client].get_state()
#print('Goal state {}'.format(GoalStatus.to_string(state)))
# get_goal_status_text
return result
def action_status_callback(msg):
global action_current_status
global action_current_id
status_list = GoalStatus()
status_list = msg.status_list
# print(str("*** CB --- ACTION --- LENGTH OF A GOAL STATUS LIST IS:" + str(len(status_list))))
if (len(status_list) != 1):
# assuming that there will always be 1 goal status
# print(str("*** CB --- ACTION --- LENGTH OF A GOAL STATUS LIST IS NOT 1 but " + str(len(status_list))))
return
id_goal_str = status_list[0].goal_id.id
# print(str("*** CB --- id_goal_str:" + str(id_goal_str) ))
# tuck_arm action is executed first, robot will wait until its finish
idx = id_goal_str.find("tuck_arm")
if (idx != -1):
# found
action_current_id = "tuck_arm"
action_current_status = status_list[0].status
def __done_callback__(self, state, result):
"""
Callback when the execution of __goto_waypoint__ has finished
We check if the execution was successful and trigger the next command
"""
if state == GoalStatus.SUCCEEDED:
# Publish stop message and reduce number of random waypoints
rospy.loginfo('Reached waypoint')
self.stop_pub.publish(Empty())
# Sample was valid, so reduce count by one
if self.random_waypoint_number > 0:
self.random_waypoint_number -= 1
else:
# Execution was not successful, so abort execution and reset the simulation
rospy.loginfo('Action returned: {}'.format(
GoalStatus.to_string(state)))
self.abort_pub.publish(Empty())
self.__reset_simulation__()
if self.random_waypoint_number > 0:
rospy.loginfo('Resample this random waypoint')
# Wait shortly before publishing the next command
rospy.sleep(0.5)
if self.random_waypoint_number > 0:
# Their are still random waypoints to generate
item = self.mission[self.mission_index]
self.__goto_random__(item[1])
else:
# Previous command has finished, so go to the next command in the list
self.mission_index += 1
self.__send_next_command__()
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 do_gps_goal(lat, long, marker_only=False):
rospy.init_node('gps_goal')
# Check for degrees, minutes, seconds format and convert to decimal
if ',' in lat:
degrees, minutes, seconds = lat.split(',')
degrees, minutes, seconds = float(degrees), float(minutes), float(
seconds)
if lat[0] == '-': # check for negative sign
minutes = -minutes
seconds = -seconds
lat = degrees + minutes / 60 + seconds / 3600
if ',' in long:
degrees, minutes, seconds = long.split(',')
degrees, minutes, seconds = float(degrees), float(minutes), float(
seconds)
if long[0] == '-': # check for negative sign
minutes = -minutes
seconds = -seconds
long = degrees + minutes / 60 + seconds / 3600
goal_lat = float(lat)
goal_long = float(long)
rospy.loginfo('Given GPS goal: lat %s, long %s.' % (goal_lat, goal_long))
# Get the lat long coordinates of our map frame's origin which must be publshed on topic /local_xy_origin. We use this to calculate our goal within the map frame.
origin_topic = '/local_xy_origin'
rospy.loginfo('Listening for origin location on topic %s' % origin_topic)
origin_pose = rospy.wait_for_message(origin_topic, PoseStamped, timeout=10)
origin_lat = origin_pose.pose.position.y
origin_long = origin_pose.pose.position.x
rospy.loginfo('Received origin: lat %s, long %s.' %
(origin_lat, origin_long))
# Calculate distance and azimuth between GPS points
geod = Geodesic.WGS84 # define the WGS84 ellipsoid
g = geod.Inverse(
origin_lat, origin_long, goal_lat, goal_long
) # Compute several geodesic calculations between two GPS points
hypotenuse = distance = g['s12'] # access distance
rospy.loginfo(
"The distance from the origin to the goal is {:.3f} m.".format(
distance))
azimuth = g['azi1']
rospy.loginfo(
"The azimuth from the origin to the goal is {:.3f} degrees.".format(
azimuth))
# Convert polar (distance and azimuth) to x,y translation in meters (needed for ROS) by finding side lenghs of a right-angle triangle
x = adjacent = math.cos(azimuth) * hypotenuse
y = opposite = math.sin(azimuth) * hypotenuse
rospy.loginfo(
"The translation from the origin to the goal is (x,y) {:.3f}, {:.3f} m."
.format(x, y))
# TODO(Dan): Set target yaw pose
# import tf_conversions
# if yaw:
# euler_tuple = tf_conversions.transformations.euler_from_quaternion(quaternion)
# else:
# # Use azimuth as yaw
# Publish goal as point message for visualization purposes
marker_topic = 'point_to_marker' # publish GPS point in ROS coordinates for visualization
point_publisher = rospy.Publisher(marker_topic,
PointStamped,
queue_size=10)
rospy.sleep(1) # allow time for subscribers to join
gps_point = PointStamped()
gps_point.point.x = x
gps_point.point.y = y
gps_point.point.z = 0
point_publisher.publish(gps_point)
rospy.loginfo("Published point {:.3f}, {:.3f} on topic {}.".format(
x, y, marker_topic))
if marker_only:
return
# Create move_base goal
rospy.loginfo("Connecting to move_base...")
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
move_base.wait_for_server()
rospy.loginfo("Connected.")
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1
rospy.loginfo(
'Executing move_base goal to position (x,y) %s, %s.' %
(goal.target_pose.pose.position.x, goal.target_pose.pose.position.y))
rospy.loginfo(
"To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'"
)
# Send goal
move_base.send_goal(goal)
rospy.loginfo('Inital goal status: %s' %
GoalStatus.to_string(move_base.get_state()))
status = move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
# Wait for goal result
move_base.wait_for_result()
rospy.loginfo('Final goal status: %s' %
GoalStatus.to_string(move_base.get_state()))
status = move_base.get_goal_status_text()
if status:
rospy.loginfo(status)
def print_status(line_limit=20):
if all_tasks_data is None or active_tasks_data is None:
return
active_ids = set(t.task_id for t in active_tasks_data.execution_queue)
non_active_tasks = [
t for t in all_tasks_data.execution_queue
if not t.task_id in active_ids
]
active_tasks = active_tasks_data.execution_queue
line_count = 0
rospy.loginfo("")
rospy.loginfo("")
rospy.loginfo(
datetime.fromtimestamp(rospy.get_rostime().secs).replace(
tzinfo=localtz).strftime("%H:%M:%S %d/%m/%y"))
line_count += 1
if len(active_tasks) > 0:
rospy.loginfo("Current tasks:")
line_count += 1
for t in active_tasks:
rospy.loginfo(" %s" % pretty(t))
line_count += 1
if goal_string is not None:
rospy.loginfo("Policy:")
rospy.loginfo(' as LTL string %s' % goal_string)
line_count += 2
if feedback_data is not None:
rospy.loginfo(' executed action: %s, status %s' %
(feedback_data.feedback.executed_action,
GoalStatus.to_string(
feedback_data.feedback.execution_status)))
rospy.loginfo(' next action: %s' %
(feedback_data.feedback.next_action))
rospy.loginfo(
' policy completing with probability %d, expected time %ds' %
(feedback_data.feedback.probability,
feedback_data.feedback.expected_time.to_sec()))
line_count += 3
rospy.loginfo("Batch started at %s", start_time(active_tasks[0]))
line_count += 1
rospy.loginfo(" finish by %s", end_time(active_tasks[0]))
line_count += 1
else:
rospy.loginfo("No active tasks")
line_count += 1
if len(non_active_tasks) > 0:
rospy.loginfo("Future tasks (in some order):")
line_count += 1
printed = 0
for t in non_active_tasks:
if line_count > line_limit - 2:
rospy.loginfo(" ... and another %s tasks not printed" %
(len(non_active_tasks) - printed))
return
else:
rospy.loginfo(
" %s after %s" %
(pretty(t), rostime_to_python(
t.start_after,
tz=localtz).strftime("%H:%M:%S %d/%m/%y")))
line_count += 1
printed += 1
else:
rospy.loginfo("No additional tasks")
def dig_result_cb(self, userdata, status, result):
rospy.loginfo("DIG state returned with status: " +
GoalStatus.to_string(status))
userdata.direction_out = 'dump'
return 'succeeded'
def dump_result_cb(self, userdata, status, result):
rospy.loginfo("DUMP state returned with status: " +
GoalStatus.to_string(status))
userdata.direction_out = 'dig'
userdata.sub_run += 1
return 'succeeded'
