def move_forward():
global current_pos
global facing
pub.publish(stdmsg.Empty())
rospy.sleep(0.5)
print "forward start x: {}".format(x_displacement)
r = rospy.Rate(11)
while (x_displacement < 0.47 and not rospy.is_shutdown()):
#while (x_displacement < 0.2 and not rospy.is_shutdown()):
command.linear.x = 0.1
constant_command_service(command)
r.sleep()
command.linear.x = 0.0
constant_command_service(command)
print "forward end x: {}".format(x_displacement)
pub.publish(stdmsg.Empty())
rospy.sleep(0.5)
if facing == LEFT:
current_pos = (current_pos[0] - 1, current_pos[1])
elif facing == RIGHT:
current_pos = (current_pos[0] + 1, current_pos[1])
elif facing == UP:
current_pos = (current_pos[0], current_pos[1] - 1)
elif facing == DOWN:
current_pos = (current_pos[0], current_pos[1] + 1)
def execute(self):
self._dynamic_reconfigure_client.update_configuration(
{'enabled': 'True'})
found_markers = self._initialise_rotation()
if not found_markers:
result = self._rotate.execute()
if not result or self._stop_requested:
self._post_execute(False)
return
rospy.loginfo("AR Pair Approach : found both ar pair markers.")
if self._parameters['search_only']:
rospy.loginfo(
"AR Pair Approach : aborting initialisation and approach as requested."
)
return
rospy.loginfo(
"AR Pair Approach : setting an initial pose from the global ar pair reference."
)
self._publishers['initial_pose_trigger'].publish(std_msgs.Empty())
rospy.loginfo("AR Pair Approach : enabling the approach controller")
self._publishers['enable_approach_controller'].publish(
std_msgs.Empty())
while not rospy.is_shutdown() and not self._stop_requested:
if self._controller_finished:
self._controller_finished = False
break
rospy.sleep(0.1)
rospy.loginfo("AR Pair Approach : disabling the approach controller")
self._publishers['disable_approach_controller'].publish(
std_msgs.Empty())
if rospy.is_shutdown() or self._stop_requested:
self._post_execute(False)
else:
self._post_execute(True)
def calibrate(self, timeout=5.0):
"""
Calibrate the gripper
"""
self._pub_calibrate.publish(stdmsg.Empty())
self.enable()
dataflow.wait_for(
test=lambda: self.calibrated(),
timeout=timeout,
body=lambda: self._pub_calibrate.publish(stdmsg.Empty())
)
def initialize_commands():
rospy.init_node('mazesolvernode', anonymous=True)
rospy.wait_for_service('make_maze')
rospy.wait_for_service('print_maze')
rospy.wait_for_service('get_wall')
rospy.wait_for_service('constant_command')
# odometry
global pub
pub = rospy.Publisher('/mobile_base/commands/reset_odometry',
stdmsg.Empty,
queue_size=10)
rospy.Subscriber('/odom', Odometry, odometry_callback)
while (pub.get_num_connections() <= 0):
rospy.sleep(0.1)
pub.publish(stdmsg.Empty())
## end odom
global make_maze_service, print_maze_service, get_wall_service
global constant_command_service
make_maze_service = rospy.ServiceProxy('make_maze', MakeNewMaze)
print_maze_service = rospy.ServiceProxy('print_maze', std_srv.Empty)
get_wall_service = rospy.ServiceProxy('get_wall', GetMazeWall)
constant_command_service = rospy.ServiceProxy('constant_command',
ConstantCommand)
#calibrate_odometry()
# runs code
make_maze_service(MAZE_SIZE, MAZE_SIZE)
print_maze_service()
solve_maze()
def test_publish_with_existing_data():
console.banner("Publish Existing Data")
py_trees.blackboard.Blackboard.set(variable_name=blackboard_key(),
value=std_msgs.Empty())
subscriber, root = create_all_the_things()
tree = py_trees_ros.trees.BehaviourTree(root=root,
unicode_tree_debug=False)
tree.setup()
executor = rclpy.executors.SingleThreadedExecutor()
executor.add_node(subscriber.node)
executor.add_node(tree.node)
assert_banner()
start_time = time.monotonic()
while (
(time.monotonic() - start_time) < timeout()) and subscriber.count == 0:
tree.tick()
executor.spin_once(timeout_sec=0.05)
assert_details("root.status", "SUCCESS", root.status)
assert (root.status == py_trees.common.Status.SUCCESS)
py_trees.blackboard.Blackboard.clear()
tree.shutdown()
subscriber.shutdown()
executor.shutdown()
def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", **kwargs):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
if not ( goal[idx]["primitive_action"] in ['test'] ):
return None
# beahviors
root = py_trees.composites.Sequence(name="TestMove")
frame = rospy.get_param("arm_base_frame", '/ur_arm_base')
blackboard = py_trees.blackboard.Blackboard()
s_move1 = MoveJoint.MOVEJ(name="Move1", controller_ns=controller_ns,
action_goal=blackboard.init_config)
pose = Pose()
pose.position.z=-0.1
goal_dict = {'pose': pose,
'frame': frame}
s_move2 = MovePose.MOVEPR(name="Move2", controller_ns=controller_ns,
action_goal=goal_dict)
pose = Pose()
pose.position.z=0.1
goal_dict = {'pose': pose,
'frame': frame}
s_move3 = MovePose.MOVEPR(name="Move3", controller_ns=controller_ns,
action_goal=goal_dict)
pose = Pose()
pose.position.z=-0.1
goal_dict = {'pose': pose,
'frame': frame}
s_move4 = MovePose.MOVEPR(name="Move4", controller_ns=controller_ns,
action_goal=goal_dict)
pose = Pose()
pose.position.z=0.1
goal_dict = {'pose': pose,
'frame': frame}
s_move5 = MovePose.MOVEPR(name="Move5", controller_ns=controller_ns,
action_goal=goal_dict)
root.add_children([s_move1, s_move3, s_move2]) #, s_move4, s_move5])
return root
def suppress_left(self):
msg = std_msgs.Empty()
if self._left:
if self._contact:
self.left_contact_pub.publish(msg)
if self._collisions:
self.left_collisions_pub.publish(msg)
if self._body:
self.left_body_pub.publish(msg)
if self._overwrench:
self.left_overwrench_pub.publish(msg)
if self._gravity:
self.left_gravity_pub.publish(msg)
def turn_left():
global facing
pub.publish(stdmsg.Empty())
rospy.sleep(0.5)
rospy.loginfo("started left turn, theta: {}".format(theta_displacement))
r = rospy.Rate(11)
while (theta_displacement < 1.48 and not rospy.is_shutdown()):
command.angular.z = 0.5
constant_command_service(command)
r.sleep()
command.angular.z = 0.0
constant_command_service(command)
rospy.loginfo("ended left turn")
rospy.loginfo("left turn theta: {}".format(theta_displacement))
pub.publish(stdmsg.Empty())
rospy.sleep(0.5)
facing = getNextLeftDirection()
def create_root(idx="1",
goal=std_msgs.Empty(),
controller_ns="",
**kwargs):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
return None
def _tracked_poses_callback(self, msg):
if self._initialise:
# send pose to pose initialisation node
msg.header.stamp -= rospy.Duration(0.2) # TODO: get latest common time
self._pub_init_pose.publish(msg)
empty_msg = std_msgs.Empty()
if not self.param['simulation']:
# disable the pose tracker
params = { 'enabled' : 'False' }
config = self._client.update_configuration(params)
# send result
self._pub_result.publish(empty_msg)
self.loginfo("Initialisation done.")
self._initialise = False
def disable(self, interactions_loader):
'''
@param interactions_loader : used to unload interactions.
@type rocon_interactions.RoleAppLoader
'''
success = False
message = "unknown error"
self._lock.acquire()
self._shutdown_publisher.publish(std_msgs.Empty())
try:
if self.msg.interactions != '':
# Can raise YamlResourceNotFoundException, MalformedInteractionsYaml
interactions_loader.load(self.msg.interactions, namespace=self._namespace, load=False)
if self.msg.parameters != '':
namespace = concert_msgs.Strings.SERVICE_NAMESPACE + '/' + self.msg.name
load_parameters_from_file(self.msg.parameters, namespace, self.msg.name, load=False)
launcher_type = self.msg.launcher_type
force_kill = False
if launcher_type == concert_msgs.ServiceProfile.TYPE_CUSTOM:
count = 0
while not rospy.is_shutdown() and self._proc.poll() is None:
rospy.rostime.wallsleep(0.5)
if count == 2 * ServiceInstance.shutdown_timeout:
self._proc.terminate()
if count == 2 * ServiceInstance.kill_timeout:
self.loginfo("waited too long, force killing..")
self._proc.kill()
force_kill = True
break
count = count + 1
elif launcher_type == concert_msgs.ServiceProfile.TYPE_ROSLAUNCH:
rospy.loginfo("Service Manager : shutting down roslaunched concert service [%s]" % self.msg.name)
count = 0
# give it some time to naturally die first.
while self._roslaunch.pm and not self._roslaunch.pm.done:
if count == 2 * ServiceInstance.shutdown_timeout:
self._roslaunch.shutdown()
rospy.rostime.wallsleep(0.5)
count = count + 1
elif launcher_type == concert_msgs.ServiceProfile.TYPE_SHADOW:
pass # no processes to kill
success = True
message = "wouldn't die so the concert got violent (force killed)" if force_kill else "died a pleasant death (terminated naturally)"
except (rocon_interactions.YamlResourceNotFoundException, rocon_interactions.MalformedInteractionsYaml) as e:
success = False
message = "error while disabling [%s][%s]" % (self.msg.name, str(e))
self._lock.release()
return success, message
def _init_services(self):
'''
This initialises all the app manager services. It depends on whether we're initialising for standalone,
or connected (pairing/concert) modes. This should not be activated multiply!
'''
if self._initialising_services:
# We could use a lock to protect this, but since the only places we call this is in the
# and in the spin(), then we just use a flag to protect.
return False
self._initialising_services = True
if self._services:
for service in self._services.values():
service.shutdown()
for publisher in self._publishers.values():
publisher.unregister()
self._services = {}
self._publishers = {}
self._service_names = {}
self._publisher_names = {}
base_name = self._gateway_name if self._gateway_name else self._param['robot_name'] # latter option is for standalone mode
for name in self._default_service_names:
self._service_names[name] = '/' + base_name + '/' + name
for name in self._default_publisher_names:
self._publisher_names[name] = '/' + base_name + '/' + name
self._application_namespace = base_name + '/' + RappManager.default_application_namespace # ns to push apps into (see rapp.py)
try:
# Advertisable services - we advertise these by default advertisement rules for the app manager's gateway.
self._services['platform_info'] = rospy.Service(self._service_names['platform_info'], rapp_manager_srvs.GetPlatformInfo, self._process_platform_info)
self._services['list_apps'] = rospy.Service(self._service_names['list_apps'], rapp_manager_srvs.GetAppList, self._process_get_app_list)
self._services['status'] = rospy.Service(self._service_names['status'], rapp_manager_srvs.Status, self._process_status)
self._services['invite'] = rospy.Service(self._service_names['invite'], rapp_manager_srvs.Invite, self._process_invite)
# Flippable services
self._services['start_app'] = rospy.Service(self._service_names['start_app'], rapp_manager_srvs.StartApp, self._process_start_app)
self._services['stop_app'] = rospy.Service(self._service_names['stop_app'], rapp_manager_srvs.StopApp, self._process_stop_app)
# Latched publishers
self._publishers['app_list'] = rospy.Publisher(self._publisher_names['app_list'], rapp_manager_msgs.AppList, latch=True)
# Force an update on the gateway
self._gateway_publishers['force_update'].publish(std_msgs.Empty())
except Exception as unused_e:
traceback.print_exc(file=sys.stdout)
self._initialising_services = False
return False
self._publish_app_list()
self._initialising_services = False
return True
def execute(self):
'''
Drop this into threading.Thread or QThread for execution
'''
if self._running:
rospy.logerr(
"AR Pair Search: already executing a motion, ignoring the request"
)
return
self._cmd_vel_publisher = rospy.Publisher(self._cmd_vel_topic,
geometry_msgs.Twist,
queue_size=10)
self._stop = False
self._running = True
start = rospy.get_rostime()
rospy.sleep(0.3)
result = False
twist = self._twist
while not self._stop and not rospy.is_shutdown():
update = self.yaw_direction * self.yaw_absolute_rate / 10.0
if math.fabs(twist.angular.z) < self.yaw_absolute_rate:
twist.angular.z = twist.angular.z + update
else:
# Make sure it is exact so the inequality in the while loop doesn't mess up next time around
twist.angular.z = self.yaw_direction * self.yaw_absolute_rate
now = rospy.get_rostime()
rospy.logdebug("AR Pair Search: rotate: %s rad/s [%ss]" %
(twist.angular.z, str(now.secs - start.secs)))
try:
self._cmd_vel_publisher.publish(twist)
except rospy.ROSException: # shutdown
break
self._rate.sleep()
if not rospy.is_shutdown():
cmd = geometry_msgs.Twist()
cmd.angular.z = 0.0
self._cmd_vel_publisher.publish(cmd)
self._initialise_pose_trigger.publish(std_msgs.Empty())
result = True
self._cmd_vel_publisher.unregister()
self._cmd_vel_publisher = None
self._running = False
return result
def _init_callback(self, msg):
self.loginfo("Initialisation started.")
# enable the pose tracker
if self.param['simulation']:
pose_msg = geometry_msgs.PoseWithCovarianceStamped()
pose_msg.header.frame_id = "ar_global"
pose_msg.header.stamp = rospy.Time.now() - rospy.Duration(0.2) # TODO: get latest common time
pose_msg.pose.pose.position.x = 1.0
pose_msg.pose.pose.position.y = 0.0
pose_msg.pose.pose.position.z = 0.0
quat = tf.transformations.quaternion_from_euler(0, 0, 3.1416)
pose_msg.pose.pose.orientation = geometry_msgs.Quaternion(*quat)
self._pub_init_pose.publish(pose_msg)
# send success right away
empty_msg = std_msgs.Empty()
self._pub_result.publish(empty_msg)
self.loginfo("Initialisation done.")
else:
params = { 'enabled' : 'True' }
config = self._client.update_configuration(params)
self._initialise = True
def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", **kwargs):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
# beahviors
root = py_trees.composites.Sequence(name="Handover")
blackboard = py_trees.blackboard.Blackboard()
if goal[idx]["primitive_action"] in ['handover']:
obj = goal[idx]['object'].encode('ascii','ignore')
destination = goal[idx]['destination'].encode('ascii','ignore')
else:
return None
# TODO
if destination=='na':
destination = None
# ----------------- Init Task ----------------
s_init = MoveJoint.MOVEJ(name="Init", controller_ns=controller_ns,
action_goal=blackboard.init_config)
# ----------------- Handover ---------------------
s_move1 = MoveJoint.MOVEJ(name="Front", controller_ns=controller_ns,
action_goal=[np.pi/2, -np.pi/2., np.pi/2., np.pi/2., -np.pi/2., 0])
s_move2 = Gripper.GOTO(name="Open", controller_ns=controller_ns,
action_goal=blackboard.gripper_open_pos, check_contact=True)
wm_remove = WorldModel.REMOVE(name="Delete", action_goal={'obj_name': obj})
task = py_trees.composites.Sequence(name="Handover")
task.add_children([s_move1, s_move2, s_init, wm_remove])
return task
def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", **kwargs):
root = py_trees.composites.Sequence(name="Attach")
blackboard = py_trees.blackboard.Blackboard()
if goal[idx]["primitive_action"] in ['move']:
obj = goal[idx]['object'].encode('ascii','ignore')
destination = goal[idx]['destination'].encode('ascii','ignore')
else:
return None
if destination=='na':
destination='plate'
print("no plate, so selected plate as a destination")
#s_init3 = MoveJoint.MOVEJ(name="Init", controller_ns=controller_ns,
# action_goal=blackboard.init_config)
pose_est2 = WorldModel.POSE_ESTIMATOR(name="Plan"+idx,
object_dict = {'target':obj,
'destination': destination})
s_move21 = MovePose.MOVEPROOT(name="Top1",
controller_ns=controller_ns,
action_goal={'pose': "Plan"+idx+"/place_top_pose"})
s_move22 = MovePose.MOVEP(name="Approach", controller_ns=controller_ns,
action_goal={'pose': "Plan"+idx+"/place_pose"})
s_move23 = Gripper.GOTO(name="Open", controller_ns=controller_ns,
action_goal=blackboard.gripper_open_pos,
force=blackboard.gripper_open_force)
s_move24 = MovePose.MOVEP(name="Top", controller_ns=controller_ns,
action_goal={'pose': "Plan"+idx+"/place_top_pose"})
attach = py_trees.composites.Sequence(name="Attach")
attach.add_children([pose_est2, s_move21, s_move22, s_move23, s_move24])
return attach
def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", rec_topic_list=None):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
if not ( goal[idx]["primitive_action"] in ['gripper_close', 'gripper_open'] ):
return None
# beahviors
## root = py_trees.composites.Sequence(name="Gripper")
root = py_trees.composites.Parallel(name="Gripper",\
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
action = goal[idx]['primitive_action'].encode('ascii','ignore')
blackboard = py_trees.blackboard.Blackboard()
if action.find('gripper_close')>=0:
s_move = Gripper.GOTO(name="Close", controller_ns=controller_ns,
action_goal=blackboard.gripper_close_pos,
force=blackboard.gripper_close_force)
else:
s_move = Gripper.GOTO(name="Open", controller_ns=controller_ns,
action_goal=blackboard.gripper_open_pos,
force=blackboard.gripper_open_force)
if rec_topic_list is None:
root.add_child(s_move)
else:
logger = Rosbag.ROSBAG(name="logger", topic_list=rec_topic_list)
root.add_children([s_move, logger])
return root
def on_button_off_clicked(self):
start_message = std_msgs.Empty()
self._off_pub.publish(start_message)
disarm_message = std_msgs.Bool(False)
self._arm_bridge_pub.publish(disarm_message)
def pause(self):
self._land_publisher.publish(msg.Empty())
def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", **kwargs):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
# beahviors
root = py_trees.composites.Sequence(name="Move")
blackboard = py_trees.blackboard.Blackboard()
grasp_offset_z = 0.02
if goal[idx]["primitive_action"] in ['slide']:
obj = goal[idx]['object'].encode('ascii','ignore')
destination = goal[idx]['destination'].encode('ascii','ignore')
else:
return None
# TODO
if destination=='na':
destination='place_tray'
print "destination is not assigned, so selected place-tray as a destination"
# ----------------- Move Task ----------------
s_init3 = MoveJoint.MOVEJ(name="Init", controller_ns=controller_ns,
action_goal=blackboard.init_config)
# ----------------- Pick ---------------------
slide = py_trees.composites.Sequence(name="Slide")
pose_est1 = WorldModel.POSE_ESTIMATOR(name="Plan"+idx,
object_dict = {'target': obj})
s_move10 = MovePose.MOVEPROOT(name="Top1",
controller_ns=controller_ns,
action_goal={'pose': "Plan"+idx+"/grasp_top_pose"})
s_move11 = MovePose.MOVEP(name="Top2", controller_ns=controller_ns,
action_goal={'pose': "Plan"+idx+"/grasp_top_pose"})
s_move12 = Gripper.GOTO(name="Open", controller_ns=controller_ns,
action_goal=blackboard.gripper_open_pos,
force=blackboard.gripper_open_force)
s_move13 = MovePose.MOVEP(name="Approach", controller_ns=controller_ns,
action_goal={'pose': "Plan"+idx+"/grasp_pose"})
# s_move14 = Gripper.GOTO(name="Close", controller_ns=controller_ns,
# action_goal=200)
pose_est2 = WorldModel.POSE_ESTIMATOR(name="Plan"+idx,
object_dict = {'target': obj,
'destination': destination})
# en_close_pose=True)
s_move22 = MovePose.MOVES(name="Approach", controller_ns=controller_ns,
action_goal={'pose': "Plan"+idx+"/place_pose"})
# s_move23 = Gripper.GOTO(name="Open", controller_ns=controller_ns,
# action_goal=50)
s_move24 = MovePose.MOVEP(name="Top", controller_ns=controller_ns,
action_goal={'pose': "Plan"+idx+"/place_top_pose"})
slide.add_children([pose_est1, s_move10, s_move11, s_move13, \
pose_est2, s_move22, s_move24, s_init3])
task = py_trees.composites.Sequence(name="Move")
task.add_child(slide)
return task
def create_root(goal=std_msgs.Empty()):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
# beahviors
root = py_trees.composites.Selector(name="Scan or Die")
die = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Uh Oh",
colour="red")
ere_we_go = py_trees.composites.Sequence(name="Ere we Go")
undock = py_trees_ros.actions.ActionClient(
name="UnDock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(False))
scan_or_be_cancelled = py_trees.composites.Selector(
"Scan or Be Cancelled")
cancelling = py_trees.composites.Sequence("Cancelling?")
is_cancel_requested = py_trees.blackboard.CheckBlackboardVariable(
name="Cancel?",
variable_name='event_cancel_button',
expected_value=True)
move_home_after_cancel = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
move_out_and_scan = py_trees.composites.Sequence("Move Out and Scan")
move_base = py_trees_ros.actions.ActionClient(
name="Move Out",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
scanning = py_trees.composites.Parallel(
name="Scanning",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
scan_context_switch = py_trees_ros.tutorials.behaviours.ScanContext(
"Context Switch")
scan_rotate = py_trees_ros.actions.ActionClient(
name="Rotate",
action_namespace="/rotate",
action_spec=py_trees_msgs.RotateAction,
action_goal=py_trees_msgs.RotateGoal())
scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Blue", colour="blue")
move_home_after_scan = py_trees_ros.actions.ActionClient(
name="Move Home",
action_namespace="/move_base",
action_spec=move_base_msgs.MoveBaseAction,
action_goal=move_base_msgs.MoveBaseGoal())
celebrate = py_trees.composites.Parallel(
name="Celebrate",
policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
celebrate_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(
name="Flash Green", colour="green")
celebrate_pause = py_trees.timers.Timer("Pause", duration=3.0)
dock = py_trees_ros.actions.ActionClient(
name="Dock",
action_namespace="/dock",
action_spec=py_trees_msgs.DockAction,
action_goal=py_trees_msgs.DockGoal(True))
# Subtree
root.add_children([ere_we_go, die])
ere_we_go.add_children([undock, scan_or_be_cancelled, dock, celebrate])
scan_or_be_cancelled.add_children([cancelling, move_out_and_scan])
cancelling.add_children([is_cancel_requested, move_home_after_cancel])
move_out_and_scan.add_children(
[move_base, scanning, move_home_after_scan])
scanning.add_children(
[scan_context_switch, scan_rotate, scan_flash_blue])
celebrate.add_children([celebrate_flash_green, celebrate_pause])
return root
def _takeoff(self):
self._takeoff_publisher.publish(msg.Empty())
def create_root(idx="1",
goal=std_msgs.Empty(),
controller_ns="",
**kwargs):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
# beahviors
root = py_trees.composites.Sequence(name="Move")
blackboard = py_trees.blackboard.Blackboard()
## grasp_offset_z = 0.02
if goal[idx]["primitive_action"] in ['place']:
if 'object' in goal[idx].keys():
obj = goal[idx]['object'].encode('ascii', 'ignore')
elif 'obj' in goal[idx].keys():
obj = goal[idx]['obj'].encode('ascii', 'ignore')
else:
rospy.logerr("MOVE: No place object")
sys.exit()
destination = goal[idx]['destination'].encode('ascii', 'ignore')
if 'destination_offset' in goal[idx].keys():
destination_offset = goal[idx][
'destination_offset'] #.encode('ascii','ignore')
else:
destination_offset = [0, 0, 0, 0, 0, 0]
else:
return None
s_init3 = MoveJoint.MOVEJ(name="Init",
controller_ns=controller_ns,
action_goal=blackboard.init_config)
# ----------------- Place ---------------------
place = py_trees.composites.Sequence(name="Place")
pose_est2 = WorldModel.POSE_ESTIMATOR(name="Plan" + idx,
object_dict={
'target':
obj,
'destination':
destination,
'destination_offset':
destination_offset
})
#from IPython import embed; embed(); sys.exit()
s_move21 = MovePose.MOVEP(
name="Top",
controller_ns=controller_ns,
action_goal={'pose': "Plan" + idx + "/place_top_pose"})
s_move22 = MovePose.MOVEP(
name="Approach",
controller_ns=controller_ns,
action_goal={'pose': "Plan" + idx + "/place_pose"})
s_move23 = Gripper.GOTO(name="Open",
controller_ns=controller_ns,
action_goal=blackboard.gripper_open_pos,
force=blackboard.gripper_open_force)
s_move24 = MovePose.MOVEP(
name="Top",
controller_ns=controller_ns,
action_goal={'pose': "Plan" + idx + "/place_top_pose"})
place.add_children(
[pose_est2, s_move21, s_move22, s_move23, s_move24, s_init3])
return place
def on_button_start_clicked(self):
start_message = std_msgs.Empty()
self._start_pub.publish(start_message)
def on_button_disable_perception_clicked(self):
disable_perception_msg = std_msgs.Empty()
self._disable_perception_pub.publish(disable_perception_msg)
def on_button_vislam_reset_clicked(self):
vislam_reset_msg = std_msgs.Empty()
self._vislam_reset_pub.publish(vislam_reset_msg)
def create_root(idx="1",
goal=std_msgs.Empty(),
controller_ns="",
**kwargs):
"""
Create the job subtree based on the incoming goal specification.
Args:
goal (:class:`~std_msgs.msg.Empty`): incoming goal specification
Returns:
:class:`~py_trees.behaviour.Behaviour`: subtree root
"""
# beahviors
root = py_trees.composites.Sequence(name="Pick")
blackboard = py_trees.blackboard.Blackboard()
if goal[idx]["primitive_action"] in ['pick']:
if 'object' in goal[idx].keys():
obj = goal[idx]['object'].encode('ascii', 'ignore')
elif 'obj' in goal[idx].keys():
obj = goal[idx]['obj'].encode('ascii', 'ignore')
else:
rospy.logerr("Pick: No pick object")
sys.exit()
## destination = goal['2']['destination'].encode('ascii','ignore')
else:
return None
# ------------ Compute -------------------------
s_init1 = MoveJoint.MOVEJ(name="Init",
controller_ns=controller_ns,
action_goal=blackboard.init_config)
s_init2 = MoveJoint.MOVEJ(name="Init2",
controller_ns=controller_ns,
action_goal=blackboard.init_config)
# ----------------- Pick ---------------------
pose_est1 = WorldModel.POSE_ESTIMATOR(name="Plan" + idx,
object_dict={'target': obj})
s_move11 = MovePose.MOVEP(
name="Top",
controller_ns=controller_ns,
action_goal={'pose': "Plan" + idx + "/grasp_top_pose"})
s_move12 = Gripper.GOTO(name="Open",
controller_ns=controller_ns,
action_goal=blackboard.gripper_open_pos,
force=blackboard.gripper_open_force)
s_move13 = MovePose.MOVEP(
name="Approach",
controller_ns=controller_ns,
action_goal={'pose': "Plan" + idx + "/grasp_pose"})
s_move14 = Gripper.GOTO(name="Close",
controller_ns=controller_ns,
action_goal=blackboard.gripper_close_pos,
force=blackboard.gripper_close_force)
s_move15 = MovePose.MOVEP(
name="Top",
controller_ns=controller_ns,
action_goal={'pose': "Plan" + idx + "/grasp_top_pose"})
pick = py_trees.composites.Sequence(name="Pick")
pick.add_children([
pose_est1, s_init1, s_move11, s_move12, s_move13, s_move14,
s_move15, s_init2
])
return pick
def on_button_land_clicked(self):
land_message = std_msgs.Empty()
self._land_pub.publish(land_message)
def publish_button_message(self, publisher):
publisher.publish(std_msgs.Empty())
def on_button_force_hover_clicked(self):
force_hover_msg = std_msgs.Empty()
self._force_hover_pub.publish(force_hover_msg)
