hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Publish rendered cam images

Browse Source
This commit is contained in:
Michel Breyer 2021-04-27 11:45:57 +02:00
parent 19b57ea164
commit bed1965643
3 changed files with 24 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
bt_sim_node.py
View File

@ -3,12 +3,13 @@
import argparse
import actionlib
import cv_bridge
import numpy as np
import rospy
import franka_gripper.msg
from geometry_msgs.msg import Pose
from sensor_msgs.msg import JointState
from sensor_msgs.msg import JointState, Image
import std_srvs.srv
from robot_tools.controllers import CartesianPoseController
@ -17,23 +18,25 @@ from robot_tools.ros import *
from simulation import BtPandaEnv
CONTROLLER_UPDATE_RATE = 60
JOINT_STATE_PUBLISHER_RATE = 60
JOINT_PUBLISH_RATE = 60
CAM_PUBLISH_RATE = 5
class BtSimNode:
def __init__(self, gui):
self.sim = BtPandaEnv(gui=gui, sleep=False)
self.controller = CartesianPoseController(self.sim.arm)
self.cv_bridge = cv_bridge.CvBridge()
self.reset_server = rospy.Service("reset", std_srvs.srv.Trigger, self.reset)
self.move_server = actionlib.SimpleActionServer(
"move",
franka_gripper.msg.MoveAction,
execute_cb=self.move,
auto_start=False,
)
self.joint_state_pub = rospy.Publisher(
"joint_states", JointState, queue_size=10
self.move,
False,
)
self.joint_pub = rospy.Publisher("joint_states", JointState, queue_size=10)
self.color_pub = rospy.Publisher("/cam/color/image_raw", Image, queue_size=10)
self.depth_pub = rospy.Publisher("/cam/depth/image_raw", Image, queue_size=10)
rospy.Subscriber("target", Pose, self.target_pose_cb)
self.step_cnt = 0
self.reset_requested = False
@ -60,8 +63,10 @@ class BtSimNode:
def handle_updates(self):
if self.step_cnt % int(self.sim.rate / CONTROLLER_UPDATE_RATE) == 0:
self.controller.update()
if self.step_cnt % int(self.sim.rate / JOINT_STATE_PUBLISHER_RATE) == 0:
if self.step_cnt % int(self.sim.rate / JOINT_PUBLISH_RATE) == 0:
self.publish_joint_state()
if self.step_cnt % int(self.sim.rate / CAM_PUBLISH_RATE) == 0:
self.publish_cam_imgs()
def publish_joint_state(self):
q, dq = self.sim.arm.get_state()
@ -74,7 +79,14 @@ class BtSimNode:
]
msg.position = np.r_[q, 0.5 * width, 0.5 * width]
msg.velocity = dq
self.joint_state_pub.publish(msg)
self.joint_pub.publish(msg)
def publish_cam_imgs(self):
color, depth = self.sim.camera.update()
color_msg = self.cv_bridge.cv2_to_imgmsg(color, encoding="rgb8")
self.color_pub.publish(color_msg)
depth_msg = self.cv_bridge.cv2_to_imgmsg(depth)
self.depth_pub.publish(depth_msg)
def move(self, goal):
self.sim.gripper.move(goal.width)

2
launch/simulation.launch
View File

@ -4,7 +4,7 @@
<arg name="launch_rviz" default="false" />
<!-- Load parameters. -->
<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
<param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/panda_arm_hand.urdf.xacro" />
<!-- Bringup the robot -->
<node pkg="active_grasp" type="bt_sim_node.py" name="simulation" args="--gui" output="screen" />

4
simulation.py
View File

@ -8,7 +8,8 @@ class BtPandaEnv(BtBaseEnv):
def __init__(self, gui=True, sleep=True):
super().__init__(gui, sleep)
self.arm = BtPandaArm()
self.gripper = BtPandaGripper()
self.gripper = BtPandaGripper(self.arm)
self.camera = BtCamera(self.arm, 9, 320, 240, 1.047, 0.1, 1.0)
self.T_W_B = Transform(Rotation.identity(), np.r_[-0.6, 0.0, 0.4])
self.load_table()
self.load_robot()
@ -29,7 +30,6 @@ class BtPandaEnv(BtBaseEnv):
def load_robot(self):
self.arm.load(self.T_W_B)
self.gripper.uid = self.arm.uid
def load_objects(self):
p.loadURDF("cube_small.urdf", [-0.2, 0.0, 0.8])