diff --git a/jsk_fetch_robot/README.md b/jsk_fetch_robot/README.md index 7219f133c2..0edec46a8d 100644 --- a/jsk_fetch_robot/README.md +++ b/jsk_fetch_robot/README.md @@ -101,6 +101,11 @@ roslaunch fetch_gazebo simulation.launch roslaunch fetch_moveit_config move_group.launch ``` +Roseus script can be executed on Gazebo. The whole demo is in `jsk_fetch_gazebo_demo/launch/demo.launch` +```bash +roslaunch jsk_fetch_gazebo_demo demo.launch +``` + ## Fetcheus APIs ### Common diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt b/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt new file mode 100644 index 0000000000..f8ab04eead --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_fetch_gazebo_demo) + +find_package(catkin) +catkin_package() + +install(DIRECTORY euslisp launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) + +if(CATKIN_ENABLE_TESTING) + if ("$ENV{ROS_DISTRO}" STRGREATER "lunar") # >= melodic + find_package(catkin REQUIRED COMPONENTS rostest roslaunch) + add_rostest(test/jsk_fetch_gazebo_demo.test) + add_rostest(test/grasping_objects.test) + roslaunch_add_file_check(launch/demo.launch) + endif() +endif() diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/README.md b/jsk_fetch_robot/jsk_fetch_gazebo_demo/README.md new file mode 100644 index 0000000000..3f1867338c --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/README.md @@ -0,0 +1,57 @@ +jsk_fetch_gazebo_demo +===================== + +- This demo is roseus version of [fetch\_gazebo\_demo](https://github.com/fetchrobotics/fetch_gazebo/tree/gazebo9/fetch_gazebo_demo). +- You can learn how to use roseus and how ROS components are executed. (e.g. pointcloud, navigation, motion planning, ... etc) + +## Usage +- Build this package like [jsk\_fetch\_startup README](https://github.com/jsk-ros-pkg/jsk_robot/tree/master/jsk_fetch_robot#setup-environment) +```bash +mkdir -p catkin_ws/src +cd catkin_ws/src +git clone https://github.com/jsk-ros-pkg/jsk_robot.git +rosdep install --from-paths . --ignore-src -y -r +cd .. +catkin config +catkin build jsk_fetch_gazebo_demo +source devel/setup.bash +``` + +- Launch demo +```bash +source devel/setup.bash +roslaunch jsk_fetch_gazebo_demo demo.launch +``` + +NOTE +- fetch\_gazebo is compatible with ROS indigo and melodic, so this repo is compatible with ROS indigo and melodic, too. +- Fetch in Gazebo9 (melodic) is somehow dark, but it seems no problem. + +## Demo +#### Start of demo +Fetch generates the costmap (blue and purple thick lines) based on the known room map. AMCL particle (red arrows) is scattered at the beginning. + +|Gazebo|Rviz| +|---|---| +|![](https://user-images.githubusercontent.com/19769486/78505523-f9f7c180-77ae-11ea-997f-379fdbf94c89.jpg)|![](https://user-images.githubusercontent.com/19769486/78505541-1ac01700-77af-11ea-98eb-e6c1b3e9caaf.png)| + +#### Move to table using navigation stacks +While fetch is moving, AMCL particle (red arrows) come together by comparing laser data (red dotted line) and room map. Navigation path (green line) is calculated based on the costmap. + +|Gazebo|Rviz| +|---|---| +|![](https://user-images.githubusercontent.com/19769486/78506252-565ce000-77b3-11ea-874d-fd1b966b7d15.jpg)|![](https://user-images.githubusercontent.com/19769486/78505555-3a573f80-77af-11ea-9ad3-e99fa06382be.png)| + +#### Recognize obstacles by pointcloud +Fetch generates collision obstacles (green blocks) based on the table and cube pointcloud. + +|Gazebo|Rviz| +|---|---| +|![](https://user-images.githubusercontent.com/19769486/78505574-5a86fe80-77af-11ea-803c-d4c45bc4d84d.jpg)|![](https://user-images.githubusercontent.com/19769486/78506589-96bd5d80-77b5-11ea-8220-24e5647998cc.png)| + +#### Grasp cube +Fetch solves IK, moves the arm while avoiding obstacles and grasps the blue cube. + +|Gazebo|Rviz| +|---|---| +|![](https://user-images.githubusercontent.com/19769486/78505616-9ae67c80-77af-11ea-87d8-dcb0a5714c78.jpg)|![](https://user-images.githubusercontent.com/19769486/78505631-a9cd2f00-77af-11ea-9410-773d85e8081e.png)| diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/config/demo.rviz b/jsk_fetch_robot/jsk_fetch_gazebo_demo/config/demo.rviz new file mode 100644 index 0000000000..cfe71c403a --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/config/demo.rviz @@ -0,0 +1,690 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.684211015701294 + Tree Height: 806 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +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: + Value: true + - 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 + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bellows_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bellows_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + estop_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_rgb_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_fixed_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + upperarm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_roll_link: + 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 + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /head_camera/depth_registered/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /base_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: GlobalCostmap + Topic: /move_base/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: LocalCostmap + Topic: /move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /particlecloud + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Global_plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/TrajectoryPlannerROS/global_plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local_plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/TrajectoryPlannerROS/local_plan + Unreliable: false + Value: true + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: false + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + 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 + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /move_group/display_planned_path + 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: arm + Query Goal State: true + Query Start State: true + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + 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 + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: false + Velocity_Scaling_Factor: 1 + - 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.20000000298023224 + 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 + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bellows_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bellows_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + estop_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_rgb_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_fixed_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + upperarm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 15.662784576416016 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.3286843299865723 + Y: 4.351048469543457 + Z: -0.9780475497245789 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.47539815306663513 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.938580513000488 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1025 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000017800000363fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670000000184000001b50000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001c1000001df0000019b00ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005bf0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1853 + X: 67 + Y: 27 diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/euslisp/demo.l b/jsk_fetch_robot/jsk_fetch_gazebo_demo/euslisp/demo.l new file mode 100755 index 0000000000..e379ceff5c --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/euslisp/demo.l @@ -0,0 +1,242 @@ +#!/usr/bin/env roseus + +;; This script is roseus version of demo.py in fetch_gazebo_demo +;; https://github.com/fetchrobotics/fetch_gazebo/blob/gazebo9/fetch_gazebo_demo/scripts/demo.py + +(require "package://fetcheus/fetch-interface.l") +;; hotfix for https://github.com/jsk-ros-pkg/jsk_robot/pull/1213#issuecomment-612976152, see https://github.com/jsk-ros-pkg/jsk_roseus/issues/574 +(make-package "GRASPING_MSGS") +(load "package://roseus/ros/grasping_msgs/msg/Object.l") +;; +(ros::load-ros-manifest "grasping_msgs") + +;; Remove (shape-msg->xxx) functions after https://github.com/jsk-ros-pkg/jsk_roseus/pull/640 is released +(defun shape-msg->shape (msg) + "Convert shape_msgs::SolidPrimitive to euslisp object" + (let ((type (send msg :type))) + (cond + ((eq type shape_msgs::SolidPrimitive::*BOX*) + (shape-msg->cube msg)) + ((eq type shape_msgs::SolidPrimitive::*SPHERE*) + (shape-msg->sphere msg)) + ((eq type shape_msgs::SolidPrimitive::*CYLINDER*) + (shape-msg->cylinder msg)) + (t + (error "unknown type ~A" type))))) +(defun shape-msg->cube (msg) + "Convert shape_msgs::SolidPrimitive to euslisp cube object" + (let* ((scale (send msg :dimensions)) + (cb (make-cube (* (elt scale 0) 1000) + (* (elt scale 1) 1000) + (* (elt scale 2) 1000)))) + cb)) +(defun shape-msg->sphere (msg) + "Convert shape_msgs::SolidPrimitive to euslisp sphere object" + (let* ((scale (send msg :dimensions)) + (cp (make-sphere (* (elt scale 0) 1000)))) + cp)) +(defun shape-msg->cylinder (msg) + "Convert shape_msgs::SolidPrimitive to euslisp cylinder object" + (let* ((scale (send msg :dimensions)) + (height (* (elt scale 0) 1000)) + (radius (* (elt scale 1) 1000)) + (cyl (make-cylinder radius height))) + cyl)) + +(defclass grasping-client + :super propertied-object + :slots (find-client objects support-surfaces) + ) + +(defmethod grasping-client + (:init (&key (find-topic "basic_grasping_perception/find_objects")) + (let () + (ros::ros-info (format nil "Waiting for ~A ..." find-topic)) + (setq find-client (instance ros::simple-action-client :init + find-topic + grasping_msgs::FindGraspableObjectsAction)) + (send find-client :wait-for-server))) + (:update-scene () + (let ((goal (instance grasping_msgs::FindGraspableObjectsGoal :init)) + (map-to-frame (send *tfl* :lookup-transform "map" "base_link" (ros::time 0))) + find-result primitive-pose scene-object height) + ;; Find objects + (send goal :plan_grasps t) + (send find-client :send-goal goal) + (send find-client :wait-for-result :timeout 5.0) + (setq find-result (send find-client :get-result)) + ;; Remove previous objects + (send *co* :wipe-all) + ;; Insert objects to scene + (dolist (obj (send find-result :objects)) + (setq primitive-pose + (ros::tf-pose->coords (car (send (send obj :object) :primitive_poses)))) + (setq scene-object + (shape-msg->shape (car (send (send obj :object) :primitives)))) + (send scene-object :transform primitive-pose :world) + (send scene-object :transform map-to-frame :world) + (send *co* :add-object scene-object :frame-id "map")) + (dolist (obj (send find-result :support_surfaces)) + (setq primitive-pose + (ros::tf-pose->coords (car (send obj :primitive_poses)))) + (setq height (elt (send primitive-pose :worldpos) 2)) + (setq scene-object + (shape-msg->cube (car (send obj :primitives)))) + (setq scene-object + (make-cube (x-of-cube scene-object) + (* 1.5 1000) + (+ (z-of-cube scene-object) height))) + (send scene-object :transform primitive-pose :world) + (send scene-object :transform map-to-frame :world) + (send scene-object :translate (float-vector 0 0 (/ height -2.0)) :world) + (send *co* :add-object scene-object :frame-id "map")) + ;; Store for grasping + (setq objects (send find-result :objects)) + (setq support-surfaces (send find-result :support_surfaces)))) + (:get-graspable-cube () + (let (graspable) + (dolist (obj objects) + (setq continue-flag nil) + ;; need grasps + (if (or (< (length (send obj :grasps)) 1) continue-flag) + (setq continue-flag t)) + ;; check size + (if (or (< (elt (send (car (send (send obj :object) :primitives)) :dimensions) 0) 0.05) + (> (elt (send (car (send (send obj :object) :primitives)) :dimensions) 0) 0.07) + (< (elt (send (car (send (send obj :object) :primitives)) :dimensions) 0) 0.05) + (> (elt (send (car (send (send obj :object) :primitives)) :dimensions) 0) 0.07) + (< (elt (send (car (send (send obj :object) :primitives)) :dimensions) 0) 0.05) + (> (elt (send (car (send (send obj :object) :primitives)) :dimensions) 0) 0.07) + continue-flag) + (setq continue-flag t)) + ;; has to be on table + (if (or (< (send (car (send (send obj :object) :primitive_poses)) :position :z) 0.5) + continue-flag) + (setq continue-flag t)) + (unless continue-flag + (return-from :get-graspable-cube (list (send obj :object) (send obj :grasps))))) + ;; nothing detected + (return-from :get-graspable-cube (list nil nil)))) + ) + +(defun main () + ;; Create *ri* to use fetch with MoveIt! and Navigation stack + (fetch-init) + + ;; To initialie tfl/co, we need node initialiation. see https://github.com/jsk-ros-pkg/jsk_robot/pull/1218/ + (unless (boundp '*tfl*) + (setq *tfl* (instance ros::transform-listener :init))) + (unless (boundp '*co*) + (setq *co* (instance collision-object-publisher :init))) + + ;; Move the base to be in front of the table + ;; Demonstrates the use of the navigation stack + (ros::ros-info "Moving to table...") + (send *ri* :move-to (make-coords :pos #f(2250 3118 0)) :frame-id "map") + (send *ri* :move-to (make-coords :pos #f(2750 3118 0)) :frame-id "map") + + ;; Raise the torso using just a controller + (ros::ros-info "Raising torso...") + (send *fetch* :reset-pose) + (send *fetch* :torso :waist-z :joint-angle 400) + (send *ri* :angle-vector (send *fetch* :angle-vector)) + (send *ri* :wait-interpolation) + + ;; Point the head at the cube we want to pick + (setq frame-to-map (send *tfl* :lookup-transform "base_link" "map" (ros::time 0))) + (send *fetch* :look-at-target + (send (make-coords :pos #f(3700 3180 0)) :transform frame-to-map :world)) + (send *ri* :angle-vector (send *fetch* :angle-vector)) + (send *ri* :wait-interpolation) + + ;; Get block to pick + (setq grasping-client (instance grasping-client :init)) + (while t + (ros::ros-info "Picking object...") + (send grasping-client :update-scene) + (setq ret (send grasping-client :get-graspable-cube)) + (setq cube (car ret)) + (setq grasps (cadr ret)) + (if cube + (progn + ;; Pick the block + ;; NOTE We do not use pick function like demo.py in fetch_gazebo_demo + (setq primitive-pose + (ros::tf-pose->coords (car (send cube :primitive_poses)))) + (setq scene-object + (shape-msg->shape (car (send cube :primitives)))) + (send scene-object :transform primitive-pose :world) + (send scene-object :rotate pi/2 :y :local) + (send *fetch* :inverse-kinematics (send scene-object :copy-worldcoords) :use-torso nil) + (send *fetch* :rarm :move-end-pos #f(0 0 150) :world :use-torso nil) + (send *ri* :angle-vector (send *fetch* :angle-vector) 10000) + (send *ri* :wait-interpolation) + (send *ri* :stop-grasp :wait t) + (send *fetch* :rarm :move-end-pos #f(0 0 -120) :world :use-torso nil) + (send *ri* :angle-vector (send *fetch* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *ri* :start-grasp :wait t) + (return)) + ;; TODO end + (ros::ros-warn "Perception failed."))) + + ;; Intermediate pose for MoveIt planning and avoid collision between table and cube + (send *fetch* :rarm :move-end-pos #f(0 500 100) :world :use-torso nil) + (send *ri* :angle-vector (send *fetch* :angle-vector) 5000) + (send *ri* :wait-interpolation) + + ;; Tuck the arm + (send *fetch* :reset-pose) + (send *ri* :angle-vector (send *fetch* :angle-vector) 5000 :arm-controller) + (send *ri* :wait-interpolation) + + ;; Lower torso + (send *ri* :angle-vector (send *fetch* :angle-vector) 2000 :torso-controller) + (send *ri* :wait-interpolation) + (send *co* :wipe-all) + + ;; Move to second table + (ros::ros-info "Moving to second table...") + (send *ri* :move-to (make-coords :pos #f(-3530 3750 0) :rpy (float-vector pi/2 0 0)) :frame-id "map") + (send *ri* :move-to (make-coords :pos #f(-3530 4150 0) :rpy (float-vector pi/2 0 0)) :frame-id "map") + + ;; Raise the torso using just a controller + (ros::ros-info "Raising torso...") + (send *fetch* :torso :waist-z :joint-angle 400) + (send *ri* :angle-vector (send *fetch* :angle-vector)) + (send *ri* :wait-interpolation) + + ;; Place the block + ;; NOTE We do not use place function like demo.py in fetch_gazebo_demo + (setq frame-to-map (send *tfl* :lookup-transform "base_link" "map" (ros::time 0))) + (send *fetch* :look-at-target + (send (make-coords :pos #f(-3000 5000 0)) :transform frame-to-map :world)) + (send *ri* :angle-vector (send *fetch* :angle-vector)) + (send *ri* :wait-interpolation) + (while t + (ros::ros-info "Placing object...") + (send grasping-client :update-scene) + (send *fetch* :inverse-kinematics (send scene-object :copy-worldcoords) :use-torso nil) + (send *fetch* :rarm :move-end-pos #f(0 0 150) :world :use-torso nil) + (send *ri* :angle-vector (send *fetch* :angle-vector) 10000) ;; move arm slowly not to drop cube + (send *ri* :wait-interpolation) + (send *fetch* :rarm :move-end-pos #f(0 0 -120) :world :use-torso nil) + (send *ri* :angle-vector (send *fetch* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *ri* :stop-grasp :wait t) + (return)) + ;; NOTE end + + ;; Intermediate pose for MoveIt planning and avoid collision between table and cube + (send *fetch* :rarm :move-end-pos #f(0 500 100) :world :use-torso nil) + (send *ri* :angle-vector (send *fetch* :angle-vector) 5000) + (send *ri* :wait-interpolation) + + ;; Tuck the arm, lower the torso + (send *fetch* :reset-pose) + (send *ri* :angle-vector (send *fetch* :angle-vector) 5000 :arm-controller) + (send *ri* :wait-interpolation) + (send *co* :wipe-all) + + (exit) + ) diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/launch/demo.launch b/jsk_fetch_robot/jsk_fetch_gazebo_demo/launch/demo.launch new file mode 100644 index 0000000000..2cf4ceae75 --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/launch/demo.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /move_base/local_costmap/inflater/inflation_radius: 0.3 + /move_base/global_costmap/inflater/inflation_radius: 0.3 + + /amcl/initial_pose_x: 0.0 + /amcl/initial_pose_y: 0.0 + /amcl/initial_pose_a: 0.0 + + + + + diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/package.xml b/jsk_fetch_robot/jsk_fetch_gazebo_demo/package.xml new file mode 100644 index 0000000000..9f07e7a3c1 --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/package.xml @@ -0,0 +1,30 @@ + + + jsk_fetch_gazebo_demo + 0.0.1 + Fetch gazebo demo for roseus implementation + + Naoya Yamaguchi + + BSD + + catkin + + fetch_gazebo_demo + fetcheus + + + + + robot_state_publisher + topic_tools + + simple_grasping + fetch_gazebo + rviz + fetch_moveit_config + + rostest + roslaunch + + diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/grasping_objects.bag b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/grasping_objects.bag new file mode 100644 index 0000000000..e4b00cfc27 Binary files /dev/null and b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/grasping_objects.bag differ diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/grasping_objects.test b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/grasping_objects.test new file mode 100644 index 0000000000..38f3f284e6 --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/grasping_objects.test @@ -0,0 +1,9 @@ + + + + + + + + diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/jsk_fetch_gazebo_demo.test b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/jsk_fetch_gazebo_demo.test new file mode 100644 index 0000000000..87f49bc002 --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/jsk_fetch_gazebo_demo.test @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-demo.l b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-demo.l new file mode 100644 index 0000000000..8908769e1a --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-demo.l @@ -0,0 +1,35 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +(load "package://jsk_fetch_gazebo_demo/euslisp/demo.l") + +(deftest init-and-move () + (let ((pos0 #f(2250 3118 0)) pos1) + ;; Create *ri* to use fetch with MoveIt! and Navigation stack + (fetch-init) + + ;; Move the base to be in front of the table + ;; Demonstrates the use of the navigation stack + (setq pos1 (send (send *ri* :state :worldcoords "map") :worldpos)) + (ros::ros-warn " goal position: ~A (diff:~A)" pos1 (norm (v- pos1 pos0))) + (ros::ros-warn "Moving to table...") + ;; first time + (send *ri* :move-to (make-coords :pos pos0) :frame-id "map") + (setq pos1 (send (send *ri* :state :worldcoords "map") :worldpos)) + (ros::ros-warn " goal position: ~A (diff:~A)" pos1 (norm (v- pos1 pos0))) + (assert (< (norm (v- pos1 pos0)) 500) + (format nil "go-pos moves incorrectly : ~A" (norm (v- pos0 pos1)))) + ;; second time + (send *ri* :move-to (make-coords :pos pos0) :frame-id "map") + (setq pos1 (send (send *ri* :state :worldcoords "map") :worldpos)) + (ros::ros-warn " goal position: ~A (diff:~A)" pos1 (norm (v- pos1 pos0))) + (assert (< (norm (v- pos1 pos0)) 500) + (format nil "go-pos moves incorrectly : ~A" (norm (v- pos0 pos1)))) + )) + +(run-all-tests) +(exit) + + diff --git a/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-grasping-objects.l b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-grasping-objects.l new file mode 100644 index 0000000000..5396effa0e --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_gazebo_demo/test/test-grasping-objects.l @@ -0,0 +1,51 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +;; force set *co* to aovid loading (instance collision-object-publisher :init))) that wait for '/apply_planning_scene' serice +(defclass dummy-collision-object-publisher :super propertied-object) +(defmethod dummy-collision-object-publisher + (:init () self) + (:wipe-all () t) + ) +(ros::roseus "test_grasping_objects") +(setq *tfl* (instance ros::transform-listener :init)) +(setq *co* (instance dummy-collision-object-publisher :init)) +;; fake action goal/result id, skip wait-for-server +(in-package "ROS") +(defmethod ros::simple-action-client + (:wait-for-server (&optional (timeout nil)) t) +(defmethod actionlib-comm-state + (:update-result + (msg) + (setq latest-goal-status (send msg :status)) + (setq latest-result msg) + (ros::ros-info "[~A] update-result goal id: ~A ~A~%" (send self :name) (send msg :status :goal_id :id) state) + (if action-goal (send msg :status :goal_id :id (send action-goal :goal_id :id))) + (ros::ros-info "[~A] replaced goal id: ~A ~A~%" (send self :name) (send msg :status :goal_id :id) state) + (send self :update-status + (instance actionlib_msgs::GoalStatusArray :init :status_list (list (send msg :status)))) + (setq state 'ros::*comm-state-done*)) + (ros::ros-info "[~A] update-result state: ~A ~A" (send self :name) (send msg :status :goal_id :id) state) + )) +(in-package "USER") +;; +(load "package://jsk_fetch_gazebo_demo/euslisp/demo.l") + +(deftest grasp-client () + (let (ret cube grasps) + (setq grasping-client (instance grasping-client :init + :find-topic "grasp_objects_test/basic_grasping_perception/find_objects")) + (ros::ros-warn "Picking object...") + (send grasping-client :update-scene) + (setq ret (send grasping-client :get-graspable-cube)) + (ros::ros-warn "Graspable object results: ~A" ret) + (setq cube (car ret)) + (setq grasps (cadr ret)) + (ros::ros-warn "Graspable cube ~A / grasps" cube grasps) + )) + +(run-all-tests) +(exit) +