diff --git a/config/moveit_with_camera.rviz b/config/moveit_with_camera.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..34f81227c7907329d9ddcc702fdff951a40a3ea4
--- /dev/null
+++ b/config/moveit_with_camera.rviz
@@ -0,0 +1,735 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 84
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /MotionPlanning1
+ - /Tag Detection Camera1
+ Splitter Ratio: 0.5
+ Tree Height: 357
+ - Class: rviz/Help
+ Name: Help
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Displays
+ Help Height: 70
+ Name: Displays
+ Property Tree Widget:
+ Expanded: ~
+ Splitter Ratio: 0.5
+ Tree Height: 822
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Acceleration_Scaling_Factor: 0.1
+ Class: moveit_rviz_plugin/MotionPlanning
+ Enabled: true
+ Move Group Namespace: ""
+ MoveIt_Allow_Approximate_IK: false
+ MoveIt_Allow_External_Program: false
+ MoveIt_Allow_Replanning: false
+ MoveIt_Allow_Sensor_Positioning: false
+ MoveIt_Planning_Attempts: 10
+ MoveIt_Planning_Time: 5
+ MoveIt_Use_Cartesian_Path: false
+ MoveIt_Use_Constraint_Aware_IK: false
+ MoveIt_Workspace:
+ Center:
+ X: 0
+ Y: 0
+ Z: 0
+ Size:
+ X: 2
+ Y: 2
+ Z: 2
+ Name: MotionPlanning
+ Planned Path:
+ Color Enabled: false
+ Interrupt Display: false
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Loop Animation: false
+ Robot Alpha: 0.5
+ Robot Color: 150; 50; 150
+ Show Robot Collision: false
+ Show Robot Visual: false
+ Show Trail: false
+ State Display Time: 0.05 s
+ Trail Step Size: 1
+ Trajectory Topic: move_group/display_planned_path
+ Use Sim Time: false
+ Planning Metrics:
+ Payload: 1
+ Show Joint Torques: false
+ Show Manipulability: false
+ Show Manipulability Index: false
+ Show Weight Limit: false
+ TextHeight: 0.07999999821186066
+ Planning Request:
+ Colliding Link Color: 255; 0; 0
+ Goal State Alpha: 1
+ Goal State Color: 250; 128; 0
+ Interactive Marker Size: 0
+ Joint Violation Color: 255; 0; 255
+ Planning Group: panda_arm
+ Query Goal State: false
+ Query Start State: false
+ Show Workspace: false
+ Start State Alpha: 1
+ Start State Color: 0; 255; 0
+ Planning Scene Topic: move_group/monitored_planning_scene
+ Robot Description: robot_description
+ Scene Geometry:
+ Scene Alpha: 1
+ Scene Color: 50; 230; 50
+ Scene Display Time: 0.009999999776482582
+ Show Scene Geometry: true
+ Voxel Coloring: Z-Axis
+ Voxel Rendering: Occupied Voxels
+ Scene Robot:
+ Attached Body Color: 150; 50; 150
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Robot Alpha: 1
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Value: true
+ Velocity_Scaling_Factor: 0.1
+ - Class: rviz/Camera
+ Enabled: false
+ Image Rendering: background and overlay
+ Image Topic: ""
+ Name: Camera
+ Overlay Alpha: 0.5
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: false
+ Visibility:
+ "": true
+ Camera Image: true
+ Grid: true
+ Image: true
+ MotionPlanning: true
+ PlanningScene: true
+ RobotModel: true
+ Tag Detection Camera: true
+ Value: true
+ Zoom Factor: 1
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: moveit_rviz_plugin/PlanningScene
+ Enabled: true
+ Move Group Namespace: ""
+ Name: PlanningScene
+ Planning Scene Topic: move_group/monitored_planning_scene
+ Robot Description: robot_description
+ Scene Geometry:
+ Scene Alpha: 0.8999999761581421
+ Scene Color: 50; 230; 50
+ Scene Display Time: 0.009999999776482582
+ Show Scene Geometry: true
+ Voxel Coloring: Z-Axis
+ Voxel Rendering: Occupied Voxels
+ Scene Robot:
+ Attached Body Color: 150; 50; 150
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_hand_tcp:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7_sc:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Robot Alpha: 1
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Value: true
+ - Class: rviz/Image
+ Enabled: false
+ Image Topic: /tag_detections_image
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Tag Detection Camera
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: compressed
+ Unreliable: false
+ Value: false
+ - Class: rviz/Image
+ Enabled: false
+ Image Topic: /usb_cam/image_color
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Camera Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: compressed
+ Unreliable: false
+ Value: false
+ - Class: rviz/Image
+ Enabled: false
+ Image Topic: /usb_cam/image_rect
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: false
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /usb_cam/image_rect_color
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: panda_link0
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 1.9689866304397583
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.75
+ Focal Point:
+ X: -0.06553632020950317
+ Y: 0.18562549352645874
+ Z: 0.22021731734275818
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.2799999713897705
+ Target Frame: panda_link0
+ Yaw: 6.244957447052002
+ Saved: ~
+Window Geometry:
+ Camera:
+ collapsed: false
+ Camera Image:
+ collapsed: false
+ Displays:
+ collapsed: false
+ Height: 1043
+ Help:
+ collapsed: false
+ Hide Left Dock: false
+ Hide Right Dock: false
+ Image:
+ collapsed: false
+ MotionPlanning:
+ collapsed: false
+ MotionPlanning - Trajectory Slider:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd000000020000000000000310000003b9fc020000000cfb000000100044006900730070006c006100790073000000003d000003b9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000003d000003b9000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000001600fffffffb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000010c000001ce0000017d00fffffffb000000100044006900730070006c006100790073000000003d000003b9000000c900fffffffb0000000c00430061006d006500720061000000028f000001670000000000000000fb0000000a0049006d0061006700650000000178000000840000001600fffffffb0000000a0049006d0061006700650300000dd8000003610000037600000209fb0000000a0049006d0061006700650300000e310000038b00000310000001d40000000100000303000003b9fc0200000002fb0000001800430061006d00650072006100200049006d0061006700650200000d09000002ab00000280000001e0fb0000002800540061006700200044006500740065006300740069006f006e002000430061006d0065007200610000000214000001e20000001600ffffff00000780000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Tag Detection Camera:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1920
+ X: 2560
+ Y: 360
diff --git a/launch/cell-controllers/robot-cell_ceti-table-1-emptyworld-grey.launch b/launch/cell-controllers/robot-cell_ceti-table-1-emptyworld-grey.launch
index 1f04aab4242993eddddc5c31b14f6ca8b0fe776a..6198ca4a26b2d807b0cef1615827ac76ad7bbc76 100644
--- a/launch/cell-controllers/robot-cell_ceti-table-1-emptyworld-grey.launch
+++ b/launch/cell-controllers/robot-cell_ceti-table-1-emptyworld-grey.launch
@@ -1,4 +1,5 @@
<launch>
+ <arg name="rviz_config" default="$(find ccf_immersive_sorting)/config/moveit_with_camera.rviz" />
<!-- MQTT server for communication with client cells -->
<arg name="mqtt_server" default="tcp://localhost:1883" doc="address of the mqtt server for the connection to the controller"/>
@@ -6,8 +7,7 @@
<!-- robot IP used by the robot controller -->
<arg name="robot_ip" default="172.31.1.13" doc="ip address if the robot"/>
- <include file="$(find ccf)/launch/robot_setup.launch">
- <arg name="robot_ip" value="$(arg robot_ip)"/>
+ <include file="$(find ccf)/launch/robot_setup.launch" pass_all_args="true">
<arg name="load_gripper" value="true"/>
</include>
diff --git a/launch/object-localization/object_locator.launch b/launch/object-localization/object_locator.launch
deleted file mode 100644
index ccf17258b1417d97a0e9048ffbc5e3d7563c35b6..0000000000000000000000000000000000000000
--- a/launch/object-localization/object_locator.launch
+++ /dev/null
@@ -1,3 +0,0 @@
-<launch>
- <node pkg="ccf_immersive_sorting" type="object_locator" name="object_locator"/>
-</launch>
diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp
index aed507d1b7a8362812a5f547f04f15f7779ec12d..7c99df08a4093c2015618e5d827f69cb26dada6d 100644
--- a/src/moveit_sorting_controller.cpp
+++ b/src/moveit_sorting_controller.cpp
@@ -58,6 +58,10 @@ int main(int argc, char** argv)
ros::WallDuration(5).sleep(); // wait 5 secs to init scene (to give moveit time to load)
+ ros::Timer timer = n.createTimer(ros::Duration(3), [&controller](const ros::TimerEvent& event) {
+ controller.sendScene();
+ }); // send a scene every three seconds
+
controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/con"
"fig/"
"conf"