Skip to content

Commit f5fe263

Browse files
authored
Merge branch 'main' into validate_time_input
2 parents 60af8d8 + 715be2c commit f5fe263

File tree

3 files changed

+20
-5
lines changed

3 files changed

+20
-5
lines changed

CHANGELOG.rst

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,11 +13,14 @@ Unreleased
1313
**Added**
1414

1515
* Added function to set the default timeout value.
16+
* Added ROS host and port parameters to the command-line interface.
1617

1718
**Changed**
1819

1920
**Fixed**
2021

22+
* Fixed #87 where a goal could be marked as terminal on result alone rather
23+
than both result and status.
2124
* Ensure input of ``Time`` is always two integers.
2225

2326
**Deprecated**

src/roslibpy/__main__.py

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,8 @@ def main():
9999
import argparse
100100

101101
parser = argparse.ArgumentParser(description='roslibpy command-line utility')
102+
parser.add_argument('-r', '--ros-host', type=str, help='ROS host name or IP address', default='localhost')
103+
parser.add_argument('-p', '--ros-port', type=int, help='ROS bridge port', default=9090)
102104

103105
commands = parser.add_subparsers(help='commands')
104106
commands.dest = 'command'
@@ -186,7 +188,7 @@ def main():
186188

187189
# Invoke
188190
args = parser.parse_args()
189-
ros = roslibpy.Ros('localhost', 9090)
191+
ros = roslibpy.Ros(args.ros_host, args.ros_port)
190192

191193
try:
192194
ros.run()

src/roslibpy/actionlib.py

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -120,6 +120,8 @@ def send(self, result_callback=None, timeout=None):
120120
if result_callback:
121121
self.on('result', result_callback)
122122

123+
self.status = {'status': GoalStatus.PENDING}
124+
123125
self.action_client.goal_topic.publish(self.goal_message)
124126
if timeout:
125127
self.action_client.ros.call_later(timeout, self._trigger_timeout)
@@ -150,22 +152,32 @@ def _trigger_timeout(self):
150152

151153
def _set_status(self, status):
152154
self.status = status
155+
if self.is_finished:
156+
self.wait_result.set()
153157

154158
def _set_result(self, result):
155159
self.result = result
156-
self.wait_result.set()
160+
if self.is_finished:
161+
self.wait_result.set()
157162

158163
def _set_feedback(self, feedback):
159164
self.feedback = feedback
160165

166+
@property
167+
def is_active(self):
168+
if self.status is None:
169+
return False
170+
return (self.status['status'] == GoalStatus.ACTIVE or
171+
self.status['status'] == GoalStatus.PENDING)
172+
161173
@property
162174
def is_finished(self):
163175
"""Indicate if the goal is finished or not.
164176
165177
Returns:
166178
bool: True if finished, False otherwise.
167179
"""
168-
return self.result is not None
180+
return self.result is not None and not self.is_active
169181

170182

171183
class ActionClient(EventEmitterMixin):
@@ -236,15 +248,13 @@ def _on_feedback_message(self, message):
236248
goal = self.goals.get(goal_id, None)
237249

238250
if goal:
239-
goal.emit('status', message['status'])
240251
goal.emit('feedback', message['feedback'])
241252

242253
def _on_result_message(self, message):
243254
goal_id = message['status']['goal_id']['id']
244255
goal = self.goals.get(goal_id, None)
245256

246257
if goal:
247-
goal.emit('status', message['status'])
248258
goal.emit('result', message['result'])
249259

250260
def add_goal(self, goal):

0 commit comments

Comments
 (0)