added marker detection for the second camera

src/ezbot_descr_simul/CMakeLists.txt

@@ -33,7 +33,7 @@ if(BUILD_TESTING)
 endif()
 
 install(
-  DIRECTORY config description launch worlds meshes
+  DIRECTORY config description launch worlds meshes media
   DESTINATION share/${PROJECT_NAME}
 )
 
@@ -42,9 +42,13 @@ install(
   DESTINATION $ENV{HOME}/.gazebo/media/
 )
 
-
-install(DIRECTORY media/
-  DESTINATION share/${PROJECT_NAME}/media/
+install(
+  DIRECTORY media/
+  DESTINATION $ENV{HOME}/.gazebo/media/
 )
 
+install(PROGRAMS
+  launch/camerapubsub.py
+  DESTINATION lib/${PROJECT_NAME})
+
 ament_package()

src/ezbot_descr_simul/launch/camerapubsub.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+import cv2
+from cv2 import aruco
+import numpy as np
+
+class camerapubsub(Node):
+
+    def __init__(self):
+        super().__init__('camerapubsub')
+        self.publisher = self.create_publisher(Image
+, 'camera_namespace/camera2/computed_image', 10)
+        self.subscription = self.create_subscription(
+            Image,
+            'camera_namespace/camera2/image_raw',
+            self.image_callback,
+            10)
+        self.cv_bridge = CvBridge()
+        fx,fy=1280,1280
+        cx,cy=fx/2,fy/2
+
+        self.camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
+        self.distortion_coefficients = np.array([0, 0, 0, 0, 0])
+
+    def image_callback(self,msg):
+        marker_size=8
+        cv_image = self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        arucoParams=cv2.aruco.DetectorParameters_create()
+        arucoParams.adaptiveThreshWinSizeMin=141
+        arucoParams.adaptiveThreshWinSizeMax=251
+        arucoParams.adaptiveThreshWinSizeStep=20
+        arucoParams.adaptiveThreshConstant=4
+        #incorrect aruco detection with bigger markers 2811
+        markers, ids, regegted = cv2.aruco.detectMarkers(cv_image, cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50),parameters=arucoParams)
+
+        if ids is not None and len(ids) > 0:
+            for i in range(len(ids)):
+
+                cv2.aruco.drawDetectedMarkers(cv_image, markers, ids)
+                cv2.aruco.drawDetectedMarkers(cv_image, regegted, borderColor=(255,0,0))
+                [rvecs, tvec, objPoints]= cv2.aruco.estimatePoseSingleMarkers(markers[i], marker_size, self.camera_matrix, self.distortion_coefficients)
+                distance = tvec[0]
+                #text = f"ID: {ids[i]}, x : {distance[0]} y :{distance[1]} z: {distance[2]}"
+                cv2.putText(cv_image, f"{distance}", (int(markers[i][0, 0, 0]), int(markers[i][0, 0, 1]) - 10),
+                            cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
+        cv2.imshow('image',cv_image)
+        processed_msg = self.cv_bridge.cv2_to_imgmsg(cv_image, encoding='bgr8')
+        self.publisher.publish(processed_msg)
+
+def main(args=None):
+
+    rclpy.init(args=args)
+    camps = camerapubsub()
+    rclpy.spin(camps)
+    camps.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
+
+
+
+
+
+
+

src/ezbot_descr_simul/launch/launch_sim.launch.py

@@ -55,9 +55,12 @@ def generate_launch_description():
                         name='Pascameracontroller',
                         arguments=['-topic', 'robot_description',
                                    '-entity', 'camera',
-                                   '-x','1.0',
-                                   '-y','-1.0',
-                                   '-z', '1.0' ],
+                                   '-x','0.0',
+                                   '-y','-1.702',
+                                   '-z', '1.5' ,
+                                   '-R', '0.0',
+                                   '-P', '-0.55',
+                                   '-Y', '0.0'],
                         output='screen')
 
     joystick = IncludeLaunchDescription(

src/ezbot_descr_simul/package.xml

@@ -8,6 +8,14 @@
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>stg_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>OpenCV</depend>
+  <depend>rclpy</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/ezbot_static_camera/description/camera_static.xacro

@@ -7,27 +7,24 @@
         <origin xyz="0 0 0" rpy="0 1.57 0"/>
     </joint>
 
-    <link name="camera_link">
+    <link name="camera_link" type="fixed">
         <visual>
             <geometry>
                 <box size="0.010 0.03 0.03"/>
             </geometry>
         </visual>
-        <gazebo>
-            <static>true</static>
-            <gravity>false</gravity>
-        </gazebo>
+
     </link>
 
-    <gazebo reference="camera_link">
+    <gazebo reference="camera_link" type="fixed">
         <material>Gazebo/Black</material>
-        <gravity>false</gravity>
+        <static>true</static>
         <sensor name="camera2" type="camera">
             <pose> 0 0 0 0 0 0 </pose>
             <visualize>true</visualize>
             <update_rate>10</update_rate>
             <camera>
-                <horizontal_fov>1.089</horizontal_fov>
+                <horizontal_fov>2</horizontal_fov>
                 <image>
                     <format>R8G8B8</format>
                     <width>1280</width>
@@ -37,7 +34,9 @@
                     <near>0.05</near>
                     <far>8.0</far>
                 </clip>
+
             </camera>
+
             <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
                 <ros>
                     <namespace>camera_namespace</namespace>

src/ezbot_static_camera/description/robot.urdf.xacro

@@ -4,4 +4,5 @@
     <xacro:include filename="robot_core.xacro" />
     <xacro:include filename="camera_static.xacro" />
 
+
 </robot>

src/ezbot_static_camera/description/robot_core.xacro

@@ -8,19 +8,21 @@
         <color rgba="1 0.3 0.1 1"/>
     </material>
 
-    <link name="base_link">
+    <link name="base_link"></link>
 
-    </link>
+    <gazebo>
+        <static>true</static>
+    </gazebo>
 
     <!--Chassis-->
 
     <joint name= "chassis_joint" type="fixed">
         <parent link="base_link"/>
         <child link="chassis"/>
-        <origin xyz="0 -0.35 3"/>
+        <origin xyz="0 0 0"/>
     </joint>
 
-    <link name="chassis">
+    <link name="chassis" type="fixed">
         <visual>
             <origin xyz="0 0 0"/>
             <geometry>
@@ -40,8 +42,9 @@
         </xacro:inertial_box>
     </link>
 
-    <gazebo reference="chassis">
-    <gravity>false</gravity>
+    <gazebo reference="chassis" type="fixed">
+        <static>true</static>
+        <gravity>false</gravity>
         <material>Gazebo/Orange</material>
     </gazebo>