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

Bt sim node now broadcasts the workspace calibration

Browse Source
This commit is contained in:
Michel Breyer 2021-04-27 17:17:00 +02:00
parent 1b804a785f
commit 608c871afd
3 changed files with 22 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
bt_sim_node.py
View File

@ -14,6 +14,7 @@ import std_srvs.srv
from robot_utils.controllers import CartesianPoseController from robot_utils.controllers import CartesianPoseController
from robot_utils.ros.conversions import * from robot_utils.ros.conversions import *
from robot_utils.ros.tf import TransformTree
from simulation import BtPandaEnv from simulation import BtPandaEnv
@ -27,6 +28,7 @@ class BtSimNode:
self.sim = BtPandaEnv(gui=gui, sleep=False) self.sim = BtPandaEnv(gui=gui, sleep=False)
self.controller = CartesianPoseController(self.sim.arm) self.controller = CartesianPoseController(self.sim.arm)
self.cv_bridge = cv_bridge.CvBridge() self.cv_bridge = cv_bridge.CvBridge()
self.tf_tree = TransformTree()
self.reset_server = rospy.Service("reset", std_srvs.srv.Trigger, self.reset) self.reset_server = rospy.Service("reset", std_srvs.srv.Trigger, self.reset)
self.move_server = actionlib.SimpleActionServer( self.move_server = actionlib.SimpleActionServer(
"move", "move",
@ -41,6 +43,8 @@ class BtSimNode:
self.step_cnt = 0 self.step_cnt = 0
self.reset_requested = False self.reset_requested = False
self.move_server.start() self.move_server.start()
T_base_task = Transform(Rotation.identity(), np.r_[0.2, -0.1, 0.1])
self.tf_tree.broadcast_static(T_base_task, "panda_link0", "task")
def reset(self, req): def reset(self, req):
self.reset_requested = True self.reset_requested = True

20
launch/panda_visualization.rviz
View File

@ -61,6 +61,10 @@ Visualization Manager:
Expand Link Details: false Expand Link Details: false
Expand Tree: false Expand Tree: false
Link Tree Style: Links in Alphabetic Order Link Tree Style: Links in Alphabetic Order
cam_optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
panda_hand: panda_hand:
Alpha: 1 Alpha: 1
Show Axes: false Show Axes: false
@ -132,6 +136,8 @@ Visualization Manager:
Frame Timeout: 15 Frame Timeout: 15
Frames: Frames:
All Enabled: false All Enabled: false
cam_optical_frame:
Value: true
panda_hand: panda_hand:
Value: true Value: true
panda_leftfinger: panda_leftfinger:
@ -156,6 +162,8 @@ Visualization Manager:
Value: false Value: false
panda_rightfinger: panda_rightfinger:
Value: false Value: false
task:
Value: true
Marker Alpha: 1 Marker Alpha: 1
Marker Scale: 0.5 Marker Scale: 0.5
Name: TF Name: TF
@ -173,10 +181,14 @@ Visualization Manager:
panda_link7: panda_link7:
panda_link8: panda_link8:
panda_hand: panda_hand:
cam_optical_frame:
{}
panda_leftfinger: panda_leftfinger:
{} {}
panda_rightfinger: panda_rightfinger:
{} {}
task:
{}
Update Interval: 0 Update Interval: 0
Value: true Value: true
Enabled: true Enabled: true
@ -207,7 +219,7 @@ Visualization Manager:
Views: Views:
Current: Current:
Class: rviz/Orbit Class: rviz/Orbit
Distance: 1.3619381189346313 Distance: 1.3004181385040283
Enable Stereo Rendering: Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549 Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1 Stereo Focal Distance: 1
@ -223,9 +235,9 @@ Visualization Manager:
Invert Z Axis: false Invert Z Axis: false
Name: Current View Name: Current View
Near Clip Distance: 0.009999999776482582 Near Clip Distance: 0.009999999776482582
Pitch: 0.21979668736457825 Pitch: 0.2697962522506714
Target Frame: <Fixed Frame> Target Frame: <Fixed Frame>
Yaw: 5.333564758300781 Yaw: 5.108528137207031
Saved: ~ Saved: ~
Window Geometry: Window Geometry:
Displays: Displays:
@ -233,7 +245,7 @@ Window Geometry:
Height: 846 Height: 846
Hide Left Dock: false Hide Left Dock: false
Hide Right Dock: true Hide Right Dock: true
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection: Selection:
collapsed: false collapsed: false
Time: Time:

4
launch/simulation.launch
View File

@ -1,7 +1,7 @@
<?xml version="1.0" ?> <?xml version="1.0" ?>
<launch> <launch>
<!-- Arguments --> <!-- Arguments -->
<arg name="launch_rviz" default="false" /> <arg name="rviz" default="false" />
<!-- Load parameters. --> <!-- Load parameters. -->
<param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/panda_arm_hand.urdf.xacro" /> <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/panda_arm_hand.urdf.xacro" />
@ -11,5 +11,5 @@
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" /> <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<!-- Visualize the robot. --> <!-- Visualize the robot. -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/panda_visualization.rviz" if="$(arg launch_rviz)" /> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/panda_visualization.rviz" if="$(arg rviz)" />
</launch> </launch>