forked from LASR-at-Home/Base
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Revert "Spring clean 01 hotfix + slight cleanup (LASR-at-Home#141)"
This reverts commit a16f83b.
- Loading branch information
Showing
11 changed files
with
182 additions
and
133 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,53 +1,55 @@ | ||
certifi==2024.2.2 # via requests | ||
charset-normalizer==3.3.2 # via requests | ||
contourpy==1.1.1 # via matplotlib | ||
cycler==0.12.1 # via matplotlib | ||
certifi==2023.7.22 # via requests | ||
charset-normalizer==3.2.0 # via requests | ||
cmake==3.27.4.1 # via triton | ||
contourpy==1.1.0 # via matplotlib | ||
cycler==0.11.0 # via matplotlib | ||
dill==0.3.7 # via -r requirements.in | ||
filelock==3.13.1 # via torch, triton | ||
fonttools==4.49.0 # via matplotlib | ||
fsspec==2024.2.0 # via torch | ||
idna==3.6 # via requests | ||
importlib-resources==6.1.2 # via matplotlib | ||
jinja2==3.1.3 # via torch | ||
filelock==3.12.3 # via torch, triton | ||
fonttools==4.42.1 # via matplotlib | ||
idna==3.4 # via requests | ||
jinja2==3.1.2 # via torch | ||
kiwisolver==1.4.5 # via matplotlib | ||
markupsafe==2.1.5 # via jinja2 | ||
matplotlib==3.7.5 # via seaborn, ultralytics | ||
lit==16.0.6 # via triton | ||
markupsafe==2.1.3 # via jinja2 | ||
matplotlib==3.7.3 # via seaborn, ultralytics | ||
mpmath==1.3.0 # via sympy | ||
networkx==3.1 # via torch | ||
numpy==1.24.4 # via contourpy, matplotlib, opencv-python, pandas, scipy, seaborn, torchvision, ultralytics | ||
nvidia-cublas-cu12==12.1.3.1 # via nvidia-cudnn-cu12, nvidia-cusolver-cu12, torch | ||
nvidia-cuda-cupti-cu12==12.1.105 # via torch | ||
nvidia-cuda-nvrtc-cu12==12.1.105 # via torch | ||
nvidia-cuda-runtime-cu12==12.1.105 # via torch | ||
nvidia-cudnn-cu12==8.9.2.26 # via torch | ||
nvidia-cufft-cu12==11.0.2.54 # via torch | ||
nvidia-curand-cu12==10.3.2.106 # via torch | ||
nvidia-cusolver-cu12==11.4.5.107 # via torch | ||
nvidia-cusparse-cu12==12.1.0.106 # via nvidia-cusolver-cu12, torch | ||
nvidia-nccl-cu12==2.19.3 # via torch | ||
nvidia-nvjitlink-cu12==12.4.99 # via nvidia-cusolver-cu12, nvidia-cusparse-cu12 | ||
nvidia-nvtx-cu12==12.1.105 # via torch | ||
opencv-python==4.9.0.80 # via ultralytics | ||
packaging==23.2 # via matplotlib | ||
pandas==2.0.3 # via seaborn, ultralytics | ||
pillow==10.2.0 # via matplotlib, torchvision, ultralytics | ||
psutil==5.9.8 # via ultralytics | ||
numpy==1.25.2 # via contourpy, matplotlib, opencv-python, pandas, scipy, seaborn, torchvision, ultralytics | ||
nvidia-cublas-cu11==11.10.3.66 # via nvidia-cudnn-cu11, nvidia-cusolver-cu11, torch | ||
nvidia-cuda-cupti-cu11==11.7.101 # via torch | ||
nvidia-cuda-nvrtc-cu11==11.7.99 # via torch | ||
nvidia-cuda-runtime-cu11==11.7.99 # via torch | ||
nvidia-cudnn-cu11==8.5.0.96 # via torch | ||
nvidia-cufft-cu11==10.9.0.58 # via torch | ||
nvidia-curand-cu11==10.2.10.91 # via torch | ||
nvidia-cusolver-cu11==11.4.0.1 # via torch | ||
nvidia-cusparse-cu11==11.7.4.91 # via torch | ||
nvidia-nccl-cu11==2.14.3 # via torch | ||
nvidia-nvtx-cu11==11.7.91 # via torch | ||
opencv-python==4.8.0.76 # via ultralytics | ||
packaging==23.1 # via matplotlib | ||
pandas==2.1.0 # via seaborn, ultralytics | ||
pillow==10.0.0 # via matplotlib, torchvision, ultralytics | ||
psutil==5.9.5 # via ultralytics | ||
py-cpuinfo==9.0.0 # via ultralytics | ||
pyparsing==3.1.2 # via matplotlib | ||
python-dateutil==2.9.0.post0 # via matplotlib, pandas | ||
pytz==2024.1 # via pandas | ||
pyparsing==3.1.1 # via matplotlib | ||
python-dateutil==2.8.2 # via matplotlib, pandas | ||
pytz==2023.3.post1 # via pandas | ||
pyyaml==6.0.1 # via ultralytics | ||
requests==2.31.0 # via ultralytics | ||
scipy==1.10.1 # via ultralytics | ||
seaborn==0.13.2 # via ultralytics | ||
requests==2.31.0 # via torchvision, ultralytics | ||
scipy==1.11.2 # via ultralytics | ||
seaborn==0.12.2 # via ultralytics | ||
six==1.16.0 # via python-dateutil | ||
sympy==1.12 # via torch | ||
torch==2.2.1 # via torchvision, ultralytics | ||
torchvision==0.17.1 # via ultralytics | ||
tqdm==4.66.2 # via ultralytics | ||
triton==2.2.0 # via torch | ||
typing-extensions==4.10.0 # via torch | ||
tzdata==2024.1 # via pandas | ||
torch==2.0.1 # via torchvision, triton, ultralytics | ||
torchvision==0.15.2 # via ultralytics | ||
tqdm==4.66.1 # via ultralytics | ||
triton==2.0.0 # via torch | ||
typing-extensions==4.7.1 # via filelock, torch | ||
tzdata==2023.3 # via pandas | ||
ultralytics==8.0.168 # via -r requirements.in | ||
urllib3==2.2.1 # via requests | ||
zipp==3.17.0 # via importlib-resources | ||
urllib3==2.0.4 # via requests | ||
wheel==0.41.2 # via nvidia-cublas-cu11, nvidia-cuda-cupti-cu11, nvidia-cuda-runtime-cu11, nvidia-curand-cu11, nvidia-cusparse-cu11, nvidia-nvtx-cu11 | ||
|
||
# The following packages are considered to be unsafe in a requirements file: | ||
# setuptools |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,3 +1,4 @@ | ||
#!/usr/bin/env python3 | ||
import rospy | ||
import smach | ||
|
||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,18 +1,31 @@ | ||
import smach_ros | ||
#!/usr/bin/env python3 | ||
|
||
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal | ||
from geometry_msgs.msg import PoseStamped | ||
from std_msgs.msg import Header | ||
import rospy | ||
import smach | ||
from tiago_controllers.controllers import Controllers | ||
from geometry_msgs.msg import Point, Quaternion, Pose | ||
|
||
|
||
class GoToLocation(smach_ros.SimpleActionState): | ||
class GoToLocation(smach.State): | ||
|
||
def __init__(self): | ||
super(GoToLocation, self).__init__( | ||
"move_base", | ||
MoveBaseAction, | ||
goal_cb=lambda ud, _: MoveBaseGoal( | ||
target_pose=PoseStamped(pose=ud.location, header=Header(frame_id="map")) | ||
), | ||
input_keys=["location"], | ||
) | ||
smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['location']) | ||
self.controllers = Controllers() | ||
|
||
def execute(self, userdata): | ||
try: | ||
status = self.controllers.base_controller.sync_to_pose(userdata.location) | ||
if status: | ||
return 'succeeded' | ||
return 'failed' | ||
except rospy.ERROR as e: | ||
rospy.logwarn(f"Unable to go to location. {userdata.location} -> ({str(e)})") | ||
return 'failed' | ||
|
||
if __name__ == '__main__': | ||
rospy.init_node('go_to_location') | ||
sm = smach.StateMachine(outcomes=['succeeded', 'failed']) | ||
loc = rospy.get_param('/living_room/table/location') | ||
sm.userdata.location = Pose(position=Point(**loc['position']), orientation=Quaternion(**loc['orientation'])) | ||
with sm: | ||
smach.StateMachine.add('GoToLocation', GoToLocation(), transitions={'succeeded': 'succeeded', 'failed': 'failed'}) | ||
sm.execute() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,28 +1,30 @@ | ||
import rospy | ||
import smach_ros | ||
|
||
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal | ||
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion | ||
from std_msgs.msg import Header | ||
#!/usr/bin/env python3 | ||
|
||
import rospy | ||
import smach | ||
from tiago_controllers.controllers import Controllers | ||
from geometry_msgs.msg import Point, Quaternion, Pose | ||
|
||
class GoToSemanticLocation(smach_ros.SimpleActionState): | ||
class GoToSemanticLocation(smach.State): | ||
|
||
def __init__(self): | ||
super(GoToSemanticLocation, self).__init__( | ||
"move_base", | ||
MoveBaseAction, | ||
goal_cb=lambda ud, _: MoveBaseGoal( | ||
target_pose=PoseStamped( | ||
pose=self._to_pose(rospy.get_param(f"{ud.location}/location")), | ||
header=Header(frame_id="map"), | ||
) | ||
), | ||
input_keys=["location"], | ||
) | ||
smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['location']) | ||
self.controllers = Controllers() | ||
|
||
def _to_pose(self, location): | ||
return Pose( | ||
position=Point(**location["position"]), | ||
orientation=Quaternion(**location["orientation"]), | ||
) | ||
def execute(self, userdata): | ||
loc = rospy.get_param(f"{userdata.location}/location") | ||
try: | ||
status = self.controllers.base_controller.sync_to_pose(Pose(position=Point(**loc['position']), orientation=Quaternion(**loc['orientation']))) | ||
if status: | ||
return 'succeeded' | ||
return 'failed' | ||
except rospy.ERROR as e: | ||
rospy.logwarn(f"Unable to go to location. {loc} -> ({str(e)})") | ||
return 'failed' | ||
if __name__ == '__main__': | ||
rospy.init_node('go_to_semantic_location') | ||
sm = smach.StateMachine(outcomes=['succeeded', 'failed']) | ||
sm.userdata.location = '/living_room/table' | ||
with sm: | ||
smach.StateMachine.add('GoToSemanticLocation', GoToSemanticLocation(), transitions={'succeeded': 'succeeded', 'failed': 'failed'}) | ||
sm.execute() |
Oops, something went wrong.