From 6658b8c7f0045a9c2d34a8f41fe743812df16700 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Wed, 7 Jul 2021 16:29:50 +0200 Subject: [PATCH] Add setup.py file --- CMakeLists.txt | 2 ++ README.md | 12 ------- active_grasp/__init__.py | 0 controller.py => active_grasp/controller.py | 10 ++++-- policies.py => active_grasp/policies.py | 20 ++++++++---- simulation.py => active_grasp/simulation.py | 2 +- utils.py => active_grasp/utils.py | 0 launch/active_grasp.rviz | 36 ++++++++++----------- bt_sim_node.py => scripts/bt_sim_node.py | 11 +++---- sim_grasp.py => scripts/run.py | 21 ++---------- setup.py | 12 +++++++ 11 files changed, 61 insertions(+), 65 deletions(-) create mode 100644 active_grasp/__init__.py rename controller.py => active_grasp/controller.py (86%) rename policies.py => active_grasp/policies.py (95%) rename simulation.py => active_grasp/simulation.py (99%) rename utils.py => active_grasp/utils.py (100%) rename bt_sim_node.py => scripts/bt_sim_node.py (96%) rename sim_grasp.py => scripts/run.py (51%) create mode 100644 setup.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c009bf..a8d5ae4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,8 @@ find_package(catkin REQUIRED COMPONENTS message_generation ) +catkin_python_setup() + add_message_files( FILES AABBox.msg diff --git a/README.md b/README.md index ca6fc38..182f2df 100644 --- a/README.md +++ b/README.md @@ -1,13 +1 @@ # active_grasp - -First, run the simulation - -``` -roslaunch active_grasp simulation.launch -``` - -Then you can run a policy. - -``` -python3 run.py ... -``` diff --git a/active_grasp/__init__.py b/active_grasp/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/controller.py b/active_grasp/controller.py similarity index 86% rename from controller.py rename to active_grasp/controller.py index d1d7a19..e355c91 100644 --- a/controller.py +++ b/active_grasp/controller.py @@ -1,10 +1,10 @@ import numpy as np import rospy -from robot_utils.ros import tf +from active_grasp.srv import Reset, ResetRequest +from active_grasp.utils import * from robot_utils.ros.panda import PandaGripperClient from robot_utils.spatial import Rotation, Transform -from utils import CartesianPoseControllerClient class GraspController: @@ -12,6 +12,7 @@ class GraspController: self.policy = policy self.controller = CartesianPoseControllerClient() self.gripper = PandaGripperClient() + self.reset_env = rospy.ServiceProxy("reset", Reset) self.load_parameters() def load_parameters(self): @@ -24,7 +25,10 @@ class GraspController: self.execute_grasp(grasp) def reset(self): - raise NotImplementedError + req = ResetRequest() + res = self.reset_env(req) + rospy.sleep(1.0) # wait for states to be updated + return from_bbox_msg(res.bbox) def explore(self, bbox): self.policy.activate(bbox) diff --git a/policies.py b/active_grasp/policies.py similarity index 95% rename from policies.py rename to active_grasp/policies.py index 43eb1e2..92c41b3 100644 --- a/policies.py +++ b/active_grasp/policies.py @@ -18,12 +18,20 @@ from vgn.utils import * def get_policy(name): if name == "single-view": - return SingleViewBaseline() + return SingleView() else: raise ValueError("{} policy does not exist.".format(name)) -class BasePolicy: +class Policy: + def activate(self, bbox): + raise NotImplementedError + + def update(self): + raise NotImplementedError + + +class BasePolicy(Policy): def __init__(self): self.cv_bridge = cv_bridge.CvBridge() self.vgn = VGN(Path(rospy.get_param("vgn/model"))) @@ -34,6 +42,8 @@ class BasePolicy: self.connect_to_camera() self.connect_to_rviz() + self.rate = 2 + def load_parameters(self): self.base_frame = rospy.get_param("~base_frame_id") self.task_frame = rospy.get_param("~frame_id") @@ -150,15 +160,11 @@ class BasePolicy: self.path_pub.publish(MarkerArray([spheres, lines])) -class SingleViewBaseline(BasePolicy): +class SingleView(BasePolicy): """ Process a single image from the initial viewpoint. """ - def __init__(self): - super().__init__() - self.rate = 1 - def update(self): self.integrate_latest_image() self.draw_scene_cloud() diff --git a/simulation.py b/active_grasp/simulation.py similarity index 99% rename from simulation.py rename to active_grasp/simulation.py index 682d29d..ac53370 100644 --- a/simulation.py +++ b/active_grasp/simulation.py @@ -2,10 +2,10 @@ from pathlib import Path import pybullet as p import rospkg +from active_grasp.utils import * from robot_utils.bullet import * from robot_utils.controllers import CartesianPoseController from robot_utils.spatial import Rotation, Transform -from utils import * class Simulation(BtSim): diff --git a/utils.py b/active_grasp/utils.py similarity index 100% rename from utils.py rename to active_grasp/utils.py diff --git a/launch/active_grasp.rviz b/launch/active_grasp.rviz index 55638e3..7bd4919 100644 --- a/launch/active_grasp.rviz +++ b/launch/active_grasp.rviz @@ -173,23 +173,23 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - panda_link0: - panda_link1: - panda_link2: - panda_link3: - panda_link4: - panda_link5: - panda_link6: - panda_link7: - panda_link8: - panda_hand: - cam_optical_frame: - {} - panda_leftfinger: - {} - panda_rightfinger: - {} world: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + cam_optical_frame: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} task: {} Update Interval: 0 @@ -320,7 +320,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.1227775812149048 + Distance: 1.5774216651916504 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -357,4 +357,4 @@ Window Geometry: collapsed: true Width: 1275 X: 61 - Y: 200 + Y: 163 diff --git a/bt_sim_node.py b/scripts/bt_sim_node.py similarity index 96% rename from bt_sim_node.py rename to scripts/bt_sim_node.py index f65e81f..fc46646 100755 --- a/bt_sim_node.py +++ b/scripts/bt_sim_node.py @@ -8,13 +8,12 @@ from geometry_msgs.msg import PoseStamped import numpy as np import rospy from sensor_msgs.msg import JointState, Image, CameraInfo -import std_srvs.srv as std_srvs import tf2_ros -import active_grasp.srv +from active_grasp.srv import Reset, ResetResponse +from active_grasp.simulation import Simulation +from active_grasp.utils import * from robot_utils.ros.conversions import * -from simulation import Simulation -from utils import * class BtSimNode: @@ -31,7 +30,7 @@ class BtSimNode: self.broadcast_transforms() def advertise_services(self): - rospy.Service("reset", active_grasp.srv.Reset, self.reset) + rospy.Service("reset", Reset, self.reset) def broadcast_transforms(self): self.static_broadcaster = tf2_ros.StaticTransformBroadcaster() @@ -47,7 +46,7 @@ class BtSimNode: self.reset_requested = True rospy.sleep(1.0) # wait for the latest sim step to finish bbox = self.sim.reset() - res = active_grasp.srv.ResetResponse(to_bbox_msg(bbox)) + res = ResetResponse(to_bbox_msg(bbox)) self.step_cnt = 0 self.reset_requested = False return res diff --git a/sim_grasp.py b/scripts/run.py similarity index 51% rename from sim_grasp.py rename to scripts/run.py index 4dc7cb9..31f5689 100644 --- a/sim_grasp.py +++ b/scripts/run.py @@ -1,23 +1,8 @@ import argparse import rospy - -from active_grasp.srv import Reset, ResetRequest -from controller import GraspController -from policies import get_policy -from utils import * - - -class SimGraspController(GraspController): - def __init__(self, policy): - super().__init__(policy) - self.reset_sim = rospy.ServiceProxy("reset", Reset) - - def reset(self): - req = ResetRequest() - res = self.reset_sim(req) - rospy.sleep(1.0) # wait for states to be updated - return from_bbox_msg(res.bbox) +from active_grasp.controller import GraspController +from active_grasp.policies import get_policy def create_parser(): @@ -41,7 +26,7 @@ def main(): parser = create_parser() args = parser.parse_args() policy = get_policy(args.policy) - controller = SimGraspController(policy) + controller = GraspController(policy) while True: controller.run() diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..b0e9a57 --- /dev/null +++ b/setup.py @@ -0,0 +1,12 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD! + +from distutils.core import setup + +from catkin_pkg.python_setup import generate_distutils_setup + +# Fetch values from package.xml. +setup_args = generate_distutils_setup( + packages=["active_grasp"], +) + +setup(**setup_args)