changes: ar_pose -> ar_marker_alvar package

LeonJung · LeonJung · commit 24c7c483c987 · 2018-04-16T17:56:31.000+09:00
diff --git a/turtlebot3_automatic_parking_vision/CMakeLists.txt b/turtlebot3_automatic_parking_vision/CMakeLists.txt
@@ -13,7 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   geometry_msgs
   nav_msgs
-  ar_pose
+  ar_track_alvar_msgs
+  ar_track_alvar
 )
 
 ################################################################################
@@ -28,7 +29,7 @@ find_package(catkin REQUIRED COMPONENTS
 # Catkin specific configuration
 ################################################################################
 catkin_package(
-  CATKIN_DEPENDS rospy std_msgs sensor_msgs geometry_msgs nav_msgs ar_pose
+  CATKIN_DEPENDS rospy std_msgs sensor_msgs geometry_msgs nav_msgs ar_track_alvar ar_track_alvar_msgs
 )
 
 ################################################################################
diff --git a/turtlebot3_automatic_parking_vision/launch/turtlebot3_automatic_parking_vision.launch b/turtlebot3_automatic_parking_vision/launch/turtlebot3_automatic_parking_vision.launch
@@ -19,18 +19,16 @@
     args="-d $(find turtlebot3_automatic_parking_vision)/rviz/turtlebot3_automatic_parking_vision.rviz"/>
   <node pkg="tf" type="static_transform_publisher" name="camera_rgb_optical_frame_to_cam" 
     args="0 0 0 0 0 0 camera_rgb_optical_frame camera 10" />
-  <node name="ar_pose" pkg="ar_pose" type="ar_single" respawn="false"
-    output="screen">
-    <remap from="/camera/image_raw" to="/raspicam_node/image" />
-    <remap from="/camera/camera_info" to="/raspicam_node/camera_info"/>
-    <param name="marker_pattern" type="string"
-      value="$(find ar_pose)/data/4x4/4x4_55.patt"/>
-    <param name="marker_width" type="double" value="60.0"/>
-    <param name="marker_center_x" type="double" value="0.0"/>
-    <param name="marker_center_y" type="double" value="0.0"/>
-    <param name="threshold" type="int" value="100"/>
-    <param name="use_history" type="bool" value="true"/>
-  </node>
+
+	<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
+		<param name="marker_size"           type="double" value="8.0" />
+		<param name="max_new_marker_error"  type="double" value="0.08" />
+		<param name="max_track_error"       type="double" value="0.2" />
+		<param name="output_frame"          type="string" value="/base_footprint" />
+
+		<remap from="camera_image"  to="/raspicam_node/image" />
+		<remap from="camera_info"   to="/raspicam_node/camera_info" />
+	</node>
 
   <node pkg="turtlebot3_automatic_parking_vision" type="automatic_parking_vision" name="automatic_parking_vision" />
 </launch>
diff --git a/turtlebot3_automatic_parking_vision/nodes/automatic_parking_vision b/turtlebot3_automatic_parking_vision/nodes/automatic_parking_vision
@@ -24,16 +24,19 @@ import numpy as np
 import tf
 from enum import Enum
 from nav_msgs.msg import Odometry
-from ar_pose.msg import ARMarker
+from ar_track_alvar_msgs.msg import AlvarMarkers
+# from ar_pose.msg import ARMarker
 from geometry_msgs.msg import Twist
 from tf.transformations import euler_from_quaternion, quaternion_from_euler
 import math
 import time
 
+MARKER_ID_DETECTION = 17
+
 class AutomaticParkingVision():
     def __init__(self):
         self.sub_odom_robot = rospy.Subscriber('/odom', Odometry, self.cbGetRobotOdom, queue_size = 1)
-        self.sub_info_marker = rospy.Subscriber('/ar_pose_marker', ARMarker, self.cbGetMarkerOdom, queue_size = 1)
+        self.sub_info_marker = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.cbGetMarkerOdom, queue_size = 1)
 
         self.pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
 
@@ -90,15 +93,17 @@ class AutomaticParkingVision():
 
         self.robot_2d_theta = self.total_robot_2d_theta
 
-    def cbGetMarkerOdom(self, marker_odom_msg):
-        if self.is_marker_pose_received == False:
-            self.is_marker_pose_received = True 
+    def cbGetMarkerOdom(self, markers_odom_msg):
+        for marker_odom_msg in markers_odom_msg.markers:
+            if marker_odom_msg.id == MARKER_ID_DETECTION:
+                if self.is_marker_pose_received == False:
+                    self.is_marker_pose_received = True
 
-        pos_x, pos_y, theta = self.fnGet2DMarkerPose(marker_odom_msg)
+                pos_x, pos_y, theta = self.fnGet2DMarkerPose(marker_odom_msg)
 
-        self.marker_2d_pose_x = pos_x
-        self.marker_2d_pose_y = pos_y
-        self.marker_2d_theta = theta - math.pi
+                self.marker_2d_pose_x = pos_x
+                self.marker_2d_pose_y = pos_y
+                self.marker_2d_theta = theta - math.pi
 
     def fnParking(self):
         if self.current_parking_sequence == self.ParkingSequence.searching_parking_lot.value:
@@ -150,7 +155,11 @@ class AutomaticParkingVision():
             return True
 
     def fnSeqChangingDirection(self):
-        desired_angle_turn = math.atan2(self.marker_2d_pose_y - 0, self.marker_2d_pose_x - 0)
+        desired_angle_turn = -1. *  math.atan2(self.marker_2d_pose_y - 0, self.marker_2d_pose_x - 0)
+
+        # rospy.loginfo("desired_angle_turn %f self.marker_2d_pose_x %f self.marker_2d_pose_y %f"
+        # , desired_angle_turn, self.marker_2d_pose_x, self.marker_2d_pose_y)
+
         self.fnTurn(desired_angle_turn)
         
         if abs(desired_angle_turn) < 0.01:
@@ -170,13 +179,15 @@ class AutomaticParkingVision():
                 self.initial_marker_pose_x = self.marker_2d_pose_x
 
             if self.initial_marker_pose_theta < 0.0:
-                desired_angle_turn = (math.pi / 2.0) + self.initial_marker_pose_theta + (self.robot_2d_theta - self.initial_robot_pose_theta)
+                desired_angle_turn = (math.pi / 2.0) + self.initial_marker_pose_theta - (self.robot_2d_theta - self.initial_robot_pose_theta)
             elif self.initial_marker_pose_theta > 0.0:
-                desired_angle_turn = -(math.pi / 2.0) + self.initial_marker_pose_theta + (self.robot_2d_theta - self.initial_robot_pose_theta)
+                desired_angle_turn = -(math.pi / 2.0) + self.initial_marker_pose_theta - (self.robot_2d_theta - self.initial_robot_pose_theta)
 
             # rospy.loginfo("desired_angle_turn %f self.initial_marker_pose_theta %f self.robot_2d_theta %f self.initial_robot_pose_theta %f"
             # , desired_angle_turn, self.initial_marker_pose_theta, self.robot_2d_theta, self.initial_robot_pose_theta)
 
+            desired_angle_turn = -1. * desired_angle_turn
+
             self.fnTurn(desired_angle_turn)
 
             if abs(desired_angle_turn) < 0.05:
@@ -221,10 +232,10 @@ class AutomaticParkingVision():
 
     def fnSeqParking(self):
         desired_angle_turn = math.atan2(self.marker_2d_pose_y - 0, self.marker_2d_pose_x - 0)
-        self.fnTrackMarker(desired_angle_turn)
+        self.fnTrackMarker(-desired_angle_turn)
         
         print self.marker_2d_pose_x
-        if abs(self.marker_2d_pose_x) < 0.13:
+        if abs(self.marker_2d_pose_x) < 0.22:
             self.fnStop()
             return True
         else:
@@ -293,16 +304,20 @@ class AutomaticParkingVision():
         return pos_x, pos_y, theta
 
     def fnGet2DMarkerPose(self, marker_odom_msg):
-        quaternion = (marker_odom_msg.pose.pose.orientation.z, marker_odom_msg.pose.pose.orientation.x, marker_odom_msg.pose.pose.orientation.y, marker_odom_msg.pose.pose.orientation.w)
+        quaternion = (marker_odom_msg.pose.pose.orientation.x, marker_odom_msg.pose.pose.orientation.y, marker_odom_msg.pose.pose.orientation.z, marker_odom_msg.pose.pose.orientation.w)
         theta = tf.transformations.euler_from_quaternion(quaternion)[2]
 
+        theta = theta + np.pi / 2.
+        # rospy.loginfo("theta : %f", theta)
+
+
         if theta < 0:
             theta = theta + np.pi * 2
         if theta > np.pi * 2:
             theta = theta - np.pi * 2
 
-        pos_x = marker_odom_msg.pose.pose.position.z
-        pos_y = marker_odom_msg.pose.pose.position.x
+        pos_x = marker_odom_msg.pose.pose.position.x
+        pos_y = marker_odom_msg.pose.pose.position.y
 
         return pos_x, pos_y, theta
 
diff --git a/turtlebot3_automatic_parking_vision/package.xml b/turtlebot3_automatic_parking_vision/package.xml
@@ -17,5 +17,5 @@
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
-  <depend>ar_pose</depend>
+  <depend>ar_track_alvar_msgs</depend>
 </package>
diff --git a/turtlebot3_automatic_parking_vision/rviz/turtlebot3_automatic_parking_vision.rviz b/turtlebot3_automatic_parking_vision/rviz/turtlebot3_automatic_parking_vision.rviz
@@ -13,6 +13,7 @@ Panels:
         - /TF1
         - /TF1/Frames1
         - /Camera1
+        - /Marker1
       Splitter Ratio: 0.602941155
     Tree Height: 567
   - Class: rviz/Selection
@@ -74,7 +75,7 @@ Visualization Manager:
       Color: 255; 0; 0
       Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 5932
@@ -91,7 +92,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -163,7 +164,27 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        ar_marker:
+        ar_marker_0:
+          Value: true
+        ar_marker_1:
+          Value: true
+        ar_marker_11:
+          Value: true
+        ar_marker_14:
+          Value: true
+        ar_marker_16:
+          Value: true
+        ar_marker_17:
+          Value: true
+        ar_marker_243:
+          Value: true
+        ar_marker_255:
+          Value: true
+        ar_marker_3:
+          Value: true
+        ar_marker_32:
+          Value: true
+        ar_marker_4:
           Value: true
         base_footprint:
           Value: true
@@ -206,7 +227,27 @@ Visualization Manager:
                 camera_rgb_frame:
                   camera_rgb_optical_frame:
                     camera:
-                      ar_marker:
+                      ar_marker_0:
+                        {}
+                      ar_marker_1:
+                        {}
+                      ar_marker_11:
+                        {}
+                      ar_marker_14:
+                        {}
+                      ar_marker_16:
+                        {}
+                      ar_marker_17:
+                        {}
+                      ar_marker_243:
+                        {}
+                      ar_marker_255:
+                        {}
+                      ar_marker_3:
+                        {}
+                      ar_marker_32:
+                        {}
+                      ar_marker_4:
                         {}
               caster_back_left_link:
                 {}
@@ -234,10 +275,19 @@ Visualization Manager:
         Axes: true
         Grid: true
         LaserScan: true
+        Marker: true
         RobotModel: true
         TF: true
         Value: true
       Zoom Factor: 1
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /visualization_marker
+      Name: Marker
+      Namespaces:
+        basic_shapes: true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 108; 108; 108
@@ -261,21 +311,26 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: 0
-      Class: rviz/TopDownOrtho
+      Class: rviz/ThirdPersonFollower
+      Distance: 1.62624097
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Focal Point:
+        X: 0.472872198
+        Y: -0.0560777262
+        Z: -5.96046448e-08
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Scale: 1124.44324
+      Pitch: 1.34539759
       Target Frame: <Fixed Frame>
-      Value: TopDownOrtho (rviz)
-      X: 0.0795647353
-      Y: 0.0197073855
+      Value: ThirdPersonFollower (rviz)
+      Yaw: 5.08540154
     Saved: ~
 Window Geometry:
   Camera:
