def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._action_topic = userdata.move_group_prefix + userdata.action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._move_group = userdata.move_group
self._joint_names = None
self._joint_names = userdata.joint_names
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(JointConstraint(joint_name=self._joint_names[i], position=userdata.joint_values[i]))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' % (self._move_group, str(e)))
self._planning_failed = True
def __init__(self,
planner,
tolerance=0.2,
controller,
recovery_behaviors=[]):
super(MoveBaseActionState,
self).__init__(output_keys=[
'outcome', 'final_pose', 'dist_to_goal', 'angle_to_goal'
],
input_keys=['target_pose'],
outcomes=['succeeded', 'failed', 'aborted'])
self._planner = planner
self._tolerance = tolerance
self._controller = controller
self._recovery_behaviors = recovery_behaviors
# Create the action client when building the behavior.
self._topic = '/move_base_flex/move_base'
self._client = ProxyActionClient(
{self._topic:
MoveBaseAction}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal.
self._error = False
def __init__(self,
controller='motion/controller/joint_state_head',
tolerance=0.17,
timeout=10.0,
joint_topic="joint_states",
outcomes=['done', 'failed', 'timeout']):
super(SetJointStateBase, self).__init__(outcomes=outcomes)
# Store topic parameter for later use.
self._controller = controller
self._joint_topic = joint_topic
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# create proxies
self._action_client = ProxyActionClient(
{self._controller: SetOperationalAction})
self._pose_publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
self._pose_subscriber = ProxySubscriberCached(
{self._joint_topic: JointState})
# timestamp
self._timestamp = None
# error in enter hook
self._error = False
def __init__(self):
"""Constructor"""
super(MoveBaseState,
self).__init__(outcomes=['arrived', 'failed'],
input_keys=['Direction', 'curr_pose'])
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
self._arrived = False
self._failed = False
# Direction and Correction Parameters
self.origin = [-12.0, -8.0]
# self.origin = [4.15, 17.4]
self.quadrant = 1
self.turn_metric = [3.25, 2.5]
self.straight_metric = 2.0
self.back_metric = 1.0
# Locations of the Goal and the Charging Stations
self.charging_station_1 = [-8.32, 4.97]
self.charging_station_2 = [6.79, 1.03]
self.parking_sign = [11.0, -3.0]
self.parking_point = [13.125, -9.36]
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'])
self.topic = "move_distance"
self.client = ProxyActionClient({
self.topic:riptide_controllers.msg.MoveDistanceAction})
def __init__(self, timeout=5.0, max_delay=-1.0, wait_duration=5, action_topic=None):
'''
Constructor
'''
super(ExecuteKnownTrajectoryState, self).__init__(
input_keys=['action_topic', 'trajectory'],
outcomes=['done', 'failed', 'param_error'],
output_keys=['status_text', 'goal_names', 'goal_values'] )
self.action_client = None
self.goal_names = None
self.goal_values = None
self.timeout_duration = rospy.Duration(timeout)
self.timeout_target = None
self.wait_duration = wait_duration
self.given_action_topic = action_topic
self.max_delay = None
if max_delay > 0.0:
self.max_delay = rospy.Duration(max_delay)
try:
if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0):
# If topic is defined, set the client up on startup
self.action_client = ProxyActionClient({self.given_action_topic: ExecuteKnownTrajectoryAction},
self.wait_duration)
else:
self.given_action_topic = None # override ""
except Exception as e:
Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e)))
self.current_action_topic = self.given_action_topic
self.status_text = None
self.return_code = None
def __init__(self):
smach.State.__init__(self,
outcomes=['Success', 'Failure'],
input_keys=['args'])
self.topic = "go_to_yaw"
self.client = ProxyActionClient(
{self.topic, riptide_controllers.msg.GoToYawAction})
def __init__(self, direction):
'''
Constructor
'''
super(FootstepPlanRelativeState,
self).__init__(outcomes=['planned', 'failed'],
input_keys=['distance'],
output_keys=['plan_header'])
if not rospy.has_param("behavior/step_distance_forward"):
Logger.logerr(
"Need to specify parameter behavior/step_distance_forward at the parameter server"
)
return
if not rospy.has_param("behavior/step_distance_sideward"):
Logger.logerr(
"Need to specify parameter behavior/step_distance_sideward at the parameter server"
)
return
self._step_distance_forward = rospy.get_param(
"behavior/step_distance_forward")
self._step_distance_sideward = rospy.get_param(
"behavior/step_distance_sideward")
self._action_topic = '/vigir/footstep_manager/step_plan_request'
self._client = ProxyActionClient(
{self._action_topic: StepPlanRequestAction})
self._done = False
self._failed = False
self._direction = direction
def __init__(self, desired_tilt):
'''Constructor'''
super(TiltHeadState, self).__init__(outcomes=['done', 'failed'])
if not rospy.has_param("behavior/joint_controllers_name"):
Logger.logerr("Need to specify parameter behavior/joint_controllers_name at the parameter server")
return
controller_namespace = rospy.get_param("behavior/joint_controllers_name")
# self._configs['flor']['same'] = {
# 20: {'joint_name': 'neck_ry', 'joint_values': [+0.00], 'controller': 'neck_traj_controller'}, # max -40 degrees
# 21: {'joint_name': 'neck_ry', 'joint_values': [-40.0], 'controller': 'neck_traj_controller'},
# 22: {'joint_name': 'neck_ry', 'joint_values': [-20.0], 'controller': 'neck_traj_controller'},
# 23: {'joint_name': 'neck_ry', 'joint_values': [+20.0], 'controller': 'neck_traj_controller'},
# 24: {'joint_name': 'neck_ry', 'joint_values': [+40.0], 'controller': 'neck_traj_controller'},
# 25: {'joint_name': 'neck_ry', 'joint_values': [+60.0], 'controller': 'neck_traj_controller'} # max +60 degrees
# }
self._action_topic = "/" + controller_namespace + \
"/neck_traj_controller" + "/follow_joint_trajectory"
self._client = ProxyActionClient({self._action_topic: FollowJointTrajectoryAction})
# Convert desired position to radians
self._desired_tilt = math.radians(desired_tilt)
self._failed = False
def on_enter(self, userdata):
self.return_code = None
# Retrieve the relevant data
if self.given_action_topic == None:
# Not action topic define, check userdata
try :
self.current_action_topic = userdata.action_topic
except Exception as e:
Logger.logwarn('Failed to get relevant user data -\n%s' % (str(e)))
self.return_code = 'failed'
return
try:
if (self.client is None):
self.client = ProxyActionClient({self.current_action_topic: ClearOctomapAction},
self.wait_duration)
except Exception as e:
Logger.logwarn('Failed to set up the action client for %s\n%s' % (self.current_action_topic, str(e)))
self.return_code = 'failed'
return
try:
# Action Initialization
action_goal = ClearOctomapGoal()
self.client.send_goal(self.current_action_topic, action_goal)
if self.timeout_duration > rospy.Duration(0.0):
self.timeout_target = rospy.Time.now() + self.timeout_duration
else:
self.timeout_target = None
except Exception as e:
Logger.logwarn('Failed to send action goal for group - %s\n%s' % (self.name, str(e)))
self.return_code = 'failed'
def __init__(self, config_name='Home', move_group='arm', action_topic='/move_group', robot_name='m1n6s200',
position_topic='/m1n6s200_driver/joint_states', delta=1E-4):
'''
Constructor
'''
super(FeedbackSrdfStateToMoveit, self).__init__(outcomes=['reached', 'failed'])
self._position_topic = position_topic
self._delta = delta
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._planning_failed = False
self._control_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerror('Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
def __init__(self, topic):
"""Constructor"""
super(BigGateManeuverState,
self).__init__(outcomes=['Success', 'Failure'])
self._topic = topic
self.client = ProxyActionClient(
{self._topic: riptide_controllers.msg.GateManeuverAction})
def __init__(self, timeout=5.0, wait_duration=5, action_topic=None):
'''
Constructor
'''
super(ApplyPlanningSceneState, self).__init__(
input_keys=['action_topic'],
outcomes=['done', 'failed'])
self.client = None
self.timeout_duration = rospy.Duration(timeout)
self.wait_duration = wait_duration
self.action_topic = action_topic
if (self.action_topic is not None) and (len(self.action_topic) > 0):
# If topic is defined, set the client up on startup
self.client = ProxyActionClient({self.action_topic: ApplyPlanningSceneAction},
self.wait_duration)
else:
self.action_topic = None # override ""
self.moveit_client = None
try:
self.moveit_client = ProxyMoveItClient(None)
except Exception as e:
Logger.logerr(" %s - exception on initialization of ProxyMoveItClient \n%s"% (self.name, str(e)))
self.success = None
self.return_code = None
def __init__(self, timeout=5.0, wait_duration=0.001, action_topic=None):
'''
Constructor
'''
super(QueryPlannersState,
self).__init__(input_keys=['action_topic'],
outcomes=['done', 'failed'],
output_keys=['planner_interfaces'])
self.client = None
self.timeout_duration = rospy.Duration(timeout)
self.wait_duration = wait_duration
self.given_action_topic = action_topic
if (self.given_action_topic
is not None) and (len(self.given_action_topic) > 0):
# If topic is defined, set the client up on startup
self.client = ProxyActionClient(
{self.given_action_topic: QueryPlannersAction},
self.wait_duration)
else:
self.given_action_topic = None # handle empty string
self.current_action_topic = self.given_action_topic
self.planner_interfaces = list()
self.return_code = None
def __init__(self, gripper_cmd, time=5.0):
"""Constructor"""
super(GripperCommandState, self).__init__(outcomes=['done', 'failed'])
self._joint_names = [
'gripper_finger_joint_l', 'gripper_finger_joint_r'
]
self._gripper_joint_positions = {
# gripper_finger_joint_l, gripper_finger_joint_r
0: [0.001, 0.001],
1: [0.010, 0.010] #NOTE: 0.011 results in goal tolerance violation
}
self._gripper_cmd = gripper_cmd
self._time = time
self._action_topic = "/arm_1/gripper_controller/follow_joint_trajectory"
self._client = ProxyActionClient(
{self._action_topic: FollowJointTrajectoryAction})
self._done = False
self._failed = False
def __init__(self, move_group='whole_body', action_topic = '/move_group', orien_tolerance=True):
'''
Constructor
'''
super(hsr_MoveitToPoseGoalActionNew, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed'], output_keys=['move_status'], input_keys=['pose_goal','plan_num'])
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self.orien_tolerance = orien_tolerance
self._move_group = move_group
self._planning_failed = False
self._control_failed = False
self._success = False
self.count_num = None
self._tolerance = 0.01
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander(self._move_group)
self.planning_frame = group.get_planning_frame()
self.planning_frame = '/map'
self.eef_link = group.get_end_effector_link()
group_names = robot.get_group_names()
scene = moveit_commander.PlanningSceneInterface()
def __init__(self,
controller='motion/controller/joint_trajectory',
outcomes=[
'success', 'partial_movement', 'invalid_pose', 'failure'
],
input_keys=[],
*args,
**kwargs):
# Declare outcomes and output keys
super(ExecuteJointTrajectoryBase, self).__init__(outcomes=outcomes,
input_keys=input_keys,
*args,
**kwargs)
# Connect to action server.
self._controller = controller
self._client = ProxyActionClient({
self._controller:
FollowJointTrajectoryAction
}) # pass required clients as dict (topic: type)
# It may happen that the action client fails to send the action goal, if _outcome is set then execute() exits immediately.
self._outcome = None
# log_once support
self._logged = False
self._logfunc = {
'info': Logger.loginfo,
'warn': Logger.logwarn,
'err': Logger.logerr,
'hint': Logger.loghint
}
def __init__(self, distance=1, ReplanPeriod=0.5):
"""Constructor"""
super(SaraFollow, self).__init__(outcomes=['failed'],
input_keys=['ID'])
# Prepare the action client for move_base
self._action_topic = "/move_base"
self._client = ProxyActionClient({self._action_topic: MoveBaseAction})
# Initialise the subscribers
self.entities_topic = "/entities"
self._pose_topic = "/robot_pose"
self._sub = ProxySubscriberCached({
self._pose_topic: Pose,
self.entities_topic: Entities
})
# Get the parameters
self.targetDistance = distance
self.ReplanPeriod = ReplanPeriod
# Initialise the status variables
self.MyPose = None
self.lastPlanTime = 0
def __init__(self, topic):
"""Constructor"""
super(BigRollState, self).__init__(outcomes=['Success', 'Failure'],
input_keys=['angle'])
self._topic = topic
self.client = ProxyActionClient(
{self._topic: riptide_controllers.msg.GoToRollAction})
def __init__(self, topic, x, y):
"""Constructor"""
super(BigTranslateParameterState, self).__init__(outcomes=['Success', 'Failure'])
self._topic = topic
self._x = x
self._y = y
self.client = ProxyActionClient({self._topic: riptide_controllers.msg.MoveDistanceAction})
def __init__(self, config_name, move_group, action_topic = '/move_group', robot_name = ""):
'''
Constructor
'''
super(SrdfStateToMoveit, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed', 'param_error'])
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
self._action_topic = action_topic
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._planning_failed = False
self._control_failed = False
self._success = False
self._srdf_param = None
if rospy.has_param("/robot_description_semantic"):
self._srdf_param = rospy.get_param("/robot_description_semantic")
else:
Logger.logerror('Unable to get parameter: /robot_description_semantic')
self._param_error = False
self._srdf = None
def __init__(self,
move_group,
offset,
tool_link,
action_topic='/move_group'):
'''
Constructor
'''
super(MoveitToJointsDynState, self).__init__(
outcomes=['reached', 'planning_failed', 'control_failed'],
input_keys=['joint_values', 'joint_names'])
self._offset = offset
self._tool_link = tool_link
self._action_topic = action_topic
self._fk_srv_topic = '/compute_fk'
self._cartesian_srv_topic = '/compute_cartesian_path'
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
self._fk_srv = ProxyServiceCaller({self._fk_srv_topic: GetPositionFK})
self._cartesian_srv = ProxyServiceCaller(
{self._cartesian_srv_topic: GetCartesianPath})
self._current_group_name = move_group
self._joint_names = None
self._planning_failed = False
self._control_failed = False
self._success = False
self._traj_client = actionlib.SimpleActionClient(
'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
self._traj_client.wait_for_server()
self._traj_exec_result = None
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._config_name = userdata.config_name # Currently not used
self._robot_name = userdata.robot_name # Currently not used
self._move_group = userdata.move_group
self._action_topic = userdata.action_topic
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
userdata.action_topic = self._action_topic # Save action topic to output key
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
def __init__(self,
controller='motion/controller/joint_state_head',
operational=True,
resources=[],
sync=True):
super(SetOperational, self).__init__(outcomes=['done', 'failure'])
# Store topic parameter for later use.
self._controller = controller
self._operational = operational
self._sync = sync
self._resources = resources
# create proxies
self._action_client = ProxyActionClient(
{self._controller: SetOperationalAction})
# check parameters
if not isinstance(resources, list) or not all(
[isinstance(r, str) for r in resources]):
raise ValueError(
'SetOperational: resource must be list of strings')
if not isinstance(operational, bool) or not isinstance(sync, bool):
raise ValueError(
'SetOperational: sync and operational parameters must be bool.'
)
# error in enter hook
self._error = False
def __init__(self, components=1023, timeout=5.0, wait_duration=2.0, action_topic=None):
'''
Constructor
'''
super(GetPlanningSceneState, self).__init__(
input_keys=['action_topic'],
outcomes=['done', 'failed'],
output_keys=['scene'] )
self.client = None
self.components = components
self.timeout_duration = rospy.Duration(timeout)
self.wait_duration = wait_duration
self.given_action_topic = action_topic
if (self.given_action_topic is not None) and (len(self.given_action_topic) > 0):
# If topic is defined, set the client up on startup
self.client = ProxyActionClient({self.given_action_topic: GetPlanningSceneAction},
self.wait_duration)
else:
self.given_action_topic = None
self.current_action_topic = self.given_action_topic
self.scene = list()
self.return_code = None
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
self._joint_config = userdata.joint_values
self._joint_names = userdata.joint_names
self._sub = ProxySubscriberCached({self._position_topic: JointState})
self._current_position = []
self._client = ProxyActionClient({self._action_topic: MoveGroupAction})
# Action Initialization
action_goal = MoveGroupGoal()
action_goal.request.group_name = self._move_group
action_goal.request.allowed_planning_time = 1.0
goal_constraints = Constraints()
for i in range(len(self._joint_names)):
goal_constraints.joint_constraints.append(
JointConstraint(joint_name=self._joint_names[i],
position=self._joint_config[i],
weight=1.0))
action_goal.request.goal_constraints.append(goal_constraints)
try:
self._client.send_goal(self._action_topic, action_goal)
except Exception as e:
Logger.logwarn('Failed to send action goal for group: %s\n%s' %
(self._move_group, str(e)))
self._planning_failed = True
def __init__(
self,
output_topic="/bounding_box_2d_monitor",
rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color",
depth_topic="/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw",
camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info",
yolo_yaml="yolov3.yaml",
save_image=True,
z_offset=0.1,
MEMO="/spacoty/get_dataset"):
super(hsr_SpaCoTyGetDataset,
self).__init__(outcomes=['completed', 'failed'],
input_keys=['save_folder'])
# Create the action client when building the behavior.
# This will cause the behavior to wait for the client before starting execution
# and will trigger a timeout error if it is not available.
# Using the proxy client provides asynchronous access to the result and status
# and makes sure only one client is used, no matter how often this state is used in a behavior.
self._topic = {
"img": "bounding_box_2d",
"sw": "/spacoty/get_dataset",
"wrd": "/spacoty/place_name"
}
self._output_topic = output_topic
self._depth_topic = depth_topic
self._rgb_topic = rgb_topic
self._camera_info_topic = camera_info_topic
self._save_folder = "/root/HSR/catkin_ws/src/em_spco_tidy_up/training_data/"
self._save_image = save_image
self.camera_info = None
self.rgb_image = None
self.depth_image = None
self.z_offset = z_offset
# get object labels
# self._yolo_yaml_path = "/root/HSR/catkin_ws/src/hsr_launch/config/darknet_ros/"+yolo_yaml
# obj_yaml = rosparam.load_file(self._yolo_yaml_path)
# self.obj_class = obj_yaml[0][0]["yolo_model"]["detection_classes"]["names"]
self._yolo_yaml_path = "/root/HSR/catkin_ws/src/cv_detect_object/scripts/" + yolo_yaml
obj_yaml = rosparam.load_file(self._yolo_yaml_path)
self.obj_class = obj_yaml[0][0].values()
# pass required clients as dict (topic: type)
self._client = ProxyActionClient(
{self._topic["img"]: hsr_common.msg.BoundingBox2DAction})
self.bridge = cv_bridge.CvBridge()
# Subscriber
self.sub_sw = ProxySubscriberCached({self._topic["sw"]: Bool})
self.sub_word = ProxySubscriberCached({self._topic["wrd"]: String})
# define the dimension
self._data_dim = {"pose": 3, "object": len(self.obj_class), "word": 0}
# It may happen that the action client fails to send the action goal.
self._error = False
self._get_image_error = False
def __init__(self, topic, depth):
"""Constructor"""
super(BigDepthParameterState,
self).__init__(outcomes=['Success', 'Failure'])
self._topic = topic
self._depth = depth
self.client = ProxyActionClient(
{self._topic: riptide_controllers.msg.GoToDepthAction})
def __init__(self, topic):
"""Constructor"""
super(BigTranslateState,
self).__init__(outcomes=['Success', 'Failure'],
input_keys=['x', 'y'])
self._topic = topic
self.client = ProxyActionClient(
{self._topic: riptide_controllers.msg.MoveDistanceAction})
def __init__(self, topic, angle):
"""Constructor"""
super(BigRollParameterState,
self).__init__(outcomes=['Success', 'Failure'])
self._topic = topic
self._angle = angle
self.client = ProxyActionClient(
{self._topic: riptide_controllers.msg.GoToRollAction})
