diff --git a/include/realsense_gazebo_plugin/RealSensePlugin.h b/include/realsense_gazebo_plugin/RealSensePlugin.h index fe8e61f..d204a7b 100644 --- a/include/realsense_gazebo_plugin/RealSensePlugin.h +++ b/include/realsense_gazebo_plugin/RealSensePlugin.h @@ -46,7 +46,7 @@ namespace gazebo public: ~RealSensePlugin(); // Documentation Inherited. - public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf, std::string prefix=""); /// \brief Callback for the World Update event. public: void OnUpdate(); diff --git a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h index 3cebd63..28cfe3e 100644 --- a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h +++ b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h @@ -42,6 +42,7 @@ namespace gazebo /// \brief A pointer to the ROS node. /// A node will be instantiated if it does not exist. protected: ros::NodeHandle* rosnode_; + public: std::string prefix; private: image_transport::ImageTransport* itnode_; protected: image_transport::CameraPublisher color_pub_, ir1_pub_, ir2_pub_, depth_pub_; diff --git a/launch/two_realsense_chessboard.rviz b/launch/two_realsense_chessboard.rviz new file mode 100644 index 0000000..62359f9 --- /dev/null +++ b/launch/two_realsense_chessboard.rviz @@ -0,0 +1,174 @@ +Panels: + - Class: rviz/Displays + Help Height: 278 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + Splitter Ratio: 0.6350210905075073 + Tree Height: 1317 + - 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: Camera +Preferences: + PromptSaveOnExit: true +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 + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /left_rs200/camera/color/image_raw + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Visibility: + Camera: true + Grid: true + TF: true + Value: true + Zoom Factor: 1 + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + left_rs200_color: + Value: true + right_rs200_color: + Value: true + world: + Value: true + Marker Scale: 0.30000001192092896 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + left_rs200_color: + {} + right_rs200_color: + {} + Update Interval: 0 + Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /right_rs200/camera/color/image_raw + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Visibility: + Camera: true + Grid: true + TF: true + Value: true + Zoom Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + 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 + Topic: /initialpose + - 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: 1.4991731643676758 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.008180774748325348 + Y: -0.010322336107492447 + Z: 0.20774713158607483 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5997961759567261 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.9690325260162354 + Saved: ~ +Window Geometry: + "&Camera": + collapsed: false + "&Displays": + collapsed: false + "&Time": + collapsed: false + C&amera: + collapsed: false + Height: 2027 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000473000006b8fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008001000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000073000003430000003601000003fb0000000c00430061006d00650072006101000003b700000374000000360100000300000001000003bc000006b8fc0200000006fb000000100044006900730070006c0061007900730100000073000006b80000011301000003fb0000000a005600690065007700730000000044000003ab0000011201000003fb0000000c00430061006d006500720061010000019a000001550000000000000000fb0000000c00430061006d006500720061010000016d000001820000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000e3fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000f000000007cfc0100000002fb0000000800540069006d0065010000000000000f000000052101000003fb0000000800540069006d00650100000000000004500000000000000000000006cf000006b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3840 + X: 0 + Y: 0 diff --git a/launch/two_realsense_chessboard_gazebo_spawn.launch b/launch/two_realsense_chessboard_gazebo_spawn.launch new file mode 100644 index 0000000..84801d5 --- /dev/null +++ b/launch/two_realsense_chessboard_gazebo_spawn.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/two_realsense_chessboard_rviz.launch b/launch/two_realsense_chessboard_rviz.launch new file mode 100644 index 0000000..86fb1db --- /dev/null +++ b/launch/two_realsense_chessboard_rviz.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/models/chessboard2/chessboard2.sdf b/models/chessboard2/chessboard2.sdf new file mode 100644 index 0000000..68cf422 --- /dev/null +++ b/models/chessboard2/chessboard2.sdf @@ -0,0 +1,65 @@ + + + + 0 + + + + 0.05 + + 0.000015 + 0.0 + 0.0 + 0.000015 + 0.0 + 0.0000108 + + + + + 0 0 0 0 0 0 + + + 0.46 0.46 0.04 + + + + + + 30.0 + 30.0 + 0 0 0 + + + + + 1000000.0 + 100.0 + 0.0025 + 1.0 + + + + + + + 0 0 0 0 0 0 + + + model://chessboard2/meshes/chessboard2.dae + + 9.2 9.2 0.8 + + + + + + 0.000000 + 0.000000 + + 0 + 0 + 1 + + + diff --git a/models/chessboard2/meshes/chessboard.svg b/models/chessboard2/meshes/chessboard.svg new file mode 100644 index 0000000..de6a028 --- /dev/null +++ b/models/chessboard2/meshes/chessboard.svg @@ -0,0 +1,911 @@ + + + + + + + + image/svg+xml + + + + + + + + + + + + + + 100 + 101 + 102 + 103 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/chessboard2/meshes/chessboard2.dae b/models/chessboard2/meshes/chessboard2.dae new file mode 100644 index 0000000..7826d08 --- /dev/null +++ b/models/chessboard2/meshes/chessboard2.dae @@ -0,0 +1,225 @@ + + + + + Blender User + Blender 2.77.0 commit date:2016-04-05, commit time:18:12, hash:abf6f08 + + 2016-09-12T13:44:38 + 2016-09-12T13:44:38 + + Z_UP + + + + + + + 49.13434 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 0 + + + + + + + + + 1 1 1 + 1 + 0 + 0.00111109 + + + + + 0.000999987 + 1 + 0.1 + 0.1 + 1 + 1 + 1 + 2 + 0 + 1 + 1 + 1 + 1 + 1 + 0 + 2880 + 2 + 30.002 + 1.000799 + 0.04999995 + 29.99998 + 1 + 2 + 0 + 0 + 1 + 1 + 1 + 1 + 8192 + 1 + 1 + 0 + 1 + 1 + 1 + 3 + 0 + 0 + 0 + 0 + 0 + 1 + 1 + 1 + 3 + 0.15 + 75 + 1 + 1 + 0 + 1 + 1 + 0 + + + + + + + chessboard2.png + + + + + + + + chessboard_png + + + + + chessboard_png-surface + + + + + + 1 1 1 1 + + + 0 0 0 1 + + + + + + 0.5 0.5 0.5 1 + + + 75 + + + 1 + + + + + + + + + + + + + + + + -0.02499997 -0.02499997 -0.02499997 -0.02499997 -0.02499997 0.02499997 -0.02499997 0.02499997 -0.02499997 -0.02499997 0.02499997 0.02499997 0.02499997 -0.02499997 -0.02499997 0.02499997 -0.02499997 0.02499997 0.02499997 0.02499997 -0.02499997 0.02499997 0.02499997 0.02499997 + + + + + + + + + + -1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1 + + + + + + + + + + 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.9999001 0.9998999 9.998e-5 9.998e-5 0.9999 9.998e-5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.9999001 0.9998999 1.00055e-4 0.9998999 9.998e-5 9.998e-5 + + + + + + + + + + + + + + + 3 3 3 3 3 3 3 3 3 3 3 3 +

3 0 0 0 0 1 1 0 2 7 1 3 2 1 4 3 1 5 5 2 6 6 2 7 7 2 8 1 3 9 4 3 10 5 3 11 2 4 12 4 4 13 0 4 14 7 5 15 1 5 16 5 5 17 3 0 18 2 0 19 0 0 20 7 1 21 6 1 22 2 1 23 5 2 24 4 2 25 6 2 26 1 3 27 0 3 28 4 3 29 2 4 30 6 4 31 4 4 32 7 5 33 3 5 34 1 5 35

+ + + + + + + + + 0.6858805 -0.3173701 0.6548619 7.481132 0.7276338 0.3124686 -0.6106656 -6.50764 -0.01081678 0.8953432 0.4452454 5.343665 0 0 0 1 + + + + -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 + + + + 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 + + + + + + + + + + + + + + + + diff --git a/models/chessboard2/meshes/chessboard2.png b/models/chessboard2/meshes/chessboard2.png new file mode 100644 index 0000000..e01073b Binary files /dev/null and b/models/chessboard2/meshes/chessboard2.png differ diff --git a/models/chessboard2/meshes/chessboard2a.png b/models/chessboard2/meshes/chessboard2a.png new file mode 100644 index 0000000..c9a1391 Binary files /dev/null and b/models/chessboard2/meshes/chessboard2a.png differ diff --git a/models/chessboard2/model.config b/models/chessboard2/model.config new file mode 100644 index 0000000..0dea2aa --- /dev/null +++ b/models/chessboard2/model.config @@ -0,0 +1,15 @@ + + + chessboard2 + 0.1.0 + + Jan Rosell + jan.rosell@upc.edu + + chessboard2.sdf + + Chessboard + + + + diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index 29fc2cd..9d3a2e5 100644 --- a/src/RealSensePlugin.cpp +++ b/src/RealSensePlugin.cpp @@ -50,7 +50,7 @@ RealSensePlugin::~RealSensePlugin() } ///////////////////////////////////////////////// -void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf, std::string prefix) { // Output the name of the model std::cout << std::endl @@ -69,15 +69,15 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) // Get Cameras Renderers this->depthCam = std::dynamic_pointer_cast( - smanager->GetSensor(DEPTH_CAMERA_NAME))->DepthCamera(); + smanager->GetSensor(prefix+DEPTH_CAMERA_NAME))->DepthCamera(); this->ired1Cam = std::dynamic_pointer_cast( - smanager->GetSensor(IRED1_CAMERA_NAME)) + smanager->GetSensor(prefix+IRED1_CAMERA_NAME)) ->Camera(); this->ired2Cam = std::dynamic_pointer_cast( - smanager->GetSensor(IRED2_CAMERA_NAME)) + smanager->GetSensor(prefix+IRED2_CAMERA_NAME)) ->Camera(); this->colorCam = std::dynamic_pointer_cast( - smanager->GetSensor(COLOR_CAMERA_NAME)) + smanager->GetSensor(prefix+COLOR_CAMERA_NAME)) ->Camera(); // Check if camera renderers have been found successfuly diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index aecd06f..ec4e873 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -1,6 +1,8 @@ #include "realsense_gazebo_plugin/gazebo_ros_realsense.h" #include + + namespace { std::string extractCameraName(const std::string& name); @@ -32,7 +34,8 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) } ROS_INFO("Realsense Gazebo ROS plugin loading."); - RealSensePlugin::Load(_model, _sdf); + prefix = this->GetHandle()+"_"; + RealSensePlugin::Load(_model, _sdf, prefix); this->rosnode_ = new ros::NodeHandle(this->GetHandle()); @@ -63,7 +66,7 @@ void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, const auto image_pub = camera_publishers.at(camera_id); // copy data into image - this->image_msg_.header.frame_id = camera_id; + this->image_msg_.header.frame_id = prefix+camera_id; this->image_msg_.header.stamp.sec = current_time.sec; this->image_msg_.header.stamp.nsec = current_time.nsec; @@ -101,7 +104,7 @@ void GazeboRosRealsense::OnNewDepthFrame() RealSensePlugin::OnNewDepthFrame(); // copy data into image - this->depth_msg_.header.frame_id = COLOR_CAMERA_NAME; + this->depth_msg_.header.frame_id = prefix+COLOR_CAMERA_NAME; this->depth_msg_.header.stamp.sec = current_time.sec; this->depth_msg_.header.stamp.nsec = current_time.nsec; diff --git a/urdf/chessboard_cameras_cell.urdf.xacro b/urdf/chessboard_cameras_cell.urdf.xacro new file mode 100644 index 0000000..ccee81a --- /dev/null +++ b/urdf/chessboard_cameras_cell.urdf.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/urdf/realsense-RS200.macro.xacro b/urdf/realsense-RS200.macro.xacro index 24e09d0..9d3a928 100644 --- a/urdf/realsense-RS200.macro.xacro +++ b/urdf/realsense-RS200.macro.xacro @@ -83,7 +83,7 @@ - + @@ -103,13 +103,14 @@ - + 0 -0.046 0.004 0 0 0 - 1.047 + 1.211258501 + 1.777777778 - 640 - 480 + 1920 + 1080 RGB_INT8 @@ -126,13 +127,14 @@ 60 1 - + 0 -0.06 0.004 0 0 0 - 1.047 + 1.211258501 + 1.777777778 - 640 - 480 + 1920 + 1080 L_INT8 @@ -149,13 +151,14 @@ 60 0 - + 0 0.01 0.004 0 0 0 - 1.047 + 1.211258501 + 1.777777778 - 640 - 480 + 1920 + 1080 L_INT8 @@ -172,13 +175,14 @@ 60 0 - + 0 -0.03 0.004 0 0 0 - 1.047 + 1.211258501 + 1.777777778 - 640 - 480 + 1920 + 1080 0.1