Skip to content

Commit 6514304

Browse files
author
zhenyan
committed
adding a new state that can allow our gripper to move to a certain position
1 parent 8081484 commit 6514304

File tree

2 files changed

+161
-5
lines changed

2 files changed

+161
-5
lines changed

‎caro_skills_flexbe_states/caro_skills_flexbe_states/move_gripper_state.py‎

Lines changed: 157 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,157 @@
1+
from flexbe_core import EventState, Logger
2+
from flexbe_core.proxy import ProxyActionClient
3+
from gripper_msgs.action import Gripper
4+
from rclpy.duration import Duration
5+
class gripperMoveForward(EventState):
6+
"""
7+
This state navigates the robot to the given pose using NavigateToPose messages
8+
9+
Parameters
10+
-- timeout Maximum time allowed (seconds)
11+
-- action_topic Name of action to invoke
12+
13+
Outputs
14+
<= pose_reached Robot reached pose successful.
15+
<= failed Failed for some reason.
16+
<= canceled User canceled before completion.
17+
<= timeout The action has timed out.
18+
19+
User data
20+
># frame Frame of the goal pose
21+
># x_target X value of goal pose
22+
># y_target Y value of goal pose
23+
># z_target z value of goal pose
24+
"""
25+
def __init__(self, timeout,action_topic):
26+
27+
super().__init__(outcomes=['pose_reached', 'failed', 'canceled', 'timeout'],
28+
input_keys=['frame', 'x_target','y_target', 'z_target'],
29+
output_keys=[])
30+
self._timeout = Duration(seconds=timeout)
31+
self._timeout_sec = timeout
32+
self._topic = action_topic
33+
34+
# Create the action client when building the behavior.
35+
# Using the proxy client provides asynchronous access to the result and status
36+
# and makes sure only one client is used, no matter how often this state is used in a behavior.
37+
ProxyActionClient.initialize(gripperMoveForward._node)
38+
39+
self._client = ProxyActionClient({self._topic: Gripper},
40+
wait_duration=0.0) # pass required clients as dict (topic: type)
41+
42+
# It may happen that the action client fails to send the action goal.
43+
self._error = False
44+
self._return = None # Retain return value in case the outcome is blocked by operator
45+
self._start_time = None
46+
47+
def execute(self, userdata):
48+
"""
49+
Call this method periodically while the state is active.
50+
51+
If no outcome is returned, the state will stay active.
52+
"""
53+
# Check if the client failed to send the goal.
54+
if self._error:
55+
return 'failed'
56+
57+
if self._return is not None:
58+
# Return prior outcome in case transition is blocked by autonomy level
59+
return self._return
60+
61+
if self._node.get_clock().now().nanoseconds - self._start_time.nanoseconds > self._target_time.nanoseconds:
62+
# Normal completion, do not bother repeating the publish
63+
# We won't bother publishing a 0 command unless blocked (above)
64+
# so that we can chain multiple motions together
65+
self._return = 'timeout'
66+
return 'timeout'
67+
68+
# Normal operation
69+
if self._cmd_topic:
70+
Logger.localinfo(f"{self._name} : {self._twist}") # For initial debugging
71+
self._pub.publish(self._cmd_topic, self._twist)
72+
73+
return None
74+
75+
def on_enter(self, userdata):
76+
"""
77+
Call this method when the state becomes active.
78+
79+
i.e. a transition from another state to this one is taken.
80+
"""
81+
self._error = False
82+
self._return = None # reset the completion flag
83+
if 'x_target' not in userdata:
84+
self._error = True
85+
Logger.logwarn("MoveTotState requires userdata.target_x key!")
86+
return
87+
88+
if 'y_target' not in userdata:
89+
self._error = True
90+
Logger.logwarn("MoveTotState requires userdata.target_y key!")
91+
return
92+
93+
if 'z_target' not in userdata:
94+
self._error = True
95+
Logger.logwarn("MoveTotState requires userdata.target_yaw key!")
96+
return
97+
# create goal msg
98+
goal = Gripper.Goal()
99+
# Recording the start time to set rotation duration output
100+
self._start_time = self._node.get_clock().now()
101+
# goal.pose.header.stamp = self._start_time
102+
103+
Logger.logwarn("frame_id type %s. Expects a string.", type(userdata.frame).__name__)
104+
Logger.logwarn("frame_id = %s.", userdata.frame)
105+
Logger.logwarn("target_x is %s. Expects a float.", type(userdata.x_target).__name__)
106+
Logger.logwarn("target_x is %f.", userdata.x_target)
107+
Logger.logwarn("target_y is %s. Expects a float.", type(userdata.y_target).__name__)
108+
Logger.logwarn("target_y is %f.", userdata.y_target)
109+
Logger.logwarn("target_yaw is %s. Expects a float.", type(userdata.z_target).__name__)
110+
Logger.logwarn("target_yaw is %f.", userdata.z_target)
111+
112+
if isinstance(userdata.frame, str):
113+
goal.frame = userdata.frame
114+
else:
115+
Logger.logwarn(f"Invalid frame_id type: {type(userdata.frame).__name__}. Expected a string.")
116+
self._error = True
117+
return
118+
119+
120+
if isinstance(userdata.x_target, float):
121+
goal.x_target = userdata.x_target
122+
else:
123+
Logger.logwarn(f"Invalid target_x type: {type(userdata.x_target).__name__}. Expected float.")
124+
self._error = True
125+
return
126+
127+
if isinstance(userdata.y_target, float):
128+
goal.y_target = userdata.y_target
129+
else:
130+
Logger.logwarn(f"Invalid target_y type: {type(userdata.y_target).__name__}. Expected float.")
131+
self._error = True
132+
return
133+
134+
if isinstance(userdata.z_target, float):
135+
goal.z_target = userdata.z_target
136+
else:
137+
Logger.logwarn(f"Invalid target_z type: {type(userdata.z_target).__name__}. Expected float.")
138+
self._error = True
139+
return
140+
141+
# Send the goal.
142+
try:
143+
self._client.send_goal(self._topic, goal, wait_duration=self._timeout_sec)
144+
except Exception as exc: # pylint: disable=W0703
145+
# Since a state failure not necessarily causes a behavior failure,
146+
# it is recommended to only print warnings, not errors.
147+
# Using a linebreak before appending the error log enables the operator to collapse details in the GUI.
148+
Logger.logwarn(f"Failed to send the NavigateToPose command:\n {type(exc)} - {exc}")
149+
self._error = True
150+
151+
def on_exit(self, userdata):
152+
# Make sure that the action is not running when leaving this state.
153+
# A situation where the action would still be active is for example when the operator manually triggers an outcome.
154+
155+
if not self._client.has_result(self._topic):
156+
self._client.cancel(self._topic)
157+
Logger.loginfo('Cancelled active action goal.')

‎caro_skills_flexbe_states/package.xml‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -16,18 +16,17 @@
1616
<exec_depend>rclpy</exec_depend>
1717
<exec_depend>flexbe_core</exec_depend>
1818
<exec_depend>nav2_msgs</exec_depend>
19+
<exec_depend>python3-numpy</exec_depend>
20+
<exec_depend>flexbe_core</exec_depend>
21+
<exec_depend>gripper_msg</exec_depend>
1922
<exec_depend>tf_transformations</exec_depend>
2023
<exec_depend>tf</exec_depend>
21-
24+
<exec_depend>std_msgs</exec_depend>
2225
<test_depend>ament_copyright</test_depend>
2326
<test_depend>ament_flake8</test_depend>
2427
<test_depend>ament_pep257</test_depend>
2528
<test_depend>python3-pytest</test_depend>
2629

27-
<!-- Turtlesim is required for ExampleActionState test and demo >
28-
< And is therefore required for buildfarm tests >
29-
< Feel free to remove if you remove that test >
30-
< Turtlesim is NOT otherwise required by FlexBE -->
3130
<depend>geometry_msgs</depend>
3231
<depend>tf2_geometry_msgs</depend>
3332

0 commit comments

Comments
 (0)