Fix/some bugs

igvc2023/config/ekf.yaml

@@ -4,9 +4,9 @@ frequency: 30
 sensor_timeout: 0.1
 silent_tf_failure: false
 two_d_mode: true
-transform_time_offset: 0.5
+transform_time_offset: 0.0 #0.5
 transform_timeout: 0.0
-print_diagnostics: ture
+print_diagnostics: true
 debug: false
 publish_tf: true
 publish_acceleration: false
@@ -20,15 +20,15 @@ base_link_frame: base_footprint
 odom0: odom0
 odom0_config: [true, true, false,
                false, false, false,
-               true, false, false,
+               true, true, false,
                false, false, true,
                false, false, false]
-odom0_queue_size: 10
+odom0_queue_size: 5
 odom0_nodelay: false
 odom0_differential: false
-odom0_relative: false
-odom0_pose_rejection_threshold: 5
-odom0_twist_rejection_threshold: 1
+odom0_relative: true
+odom0_pose_rejection_threshold: 6
+odom0_twist_rejection_threshold: 3.15
 
 #odom1: gps of wheel odometry
 odom1: odom1
@@ -55,28 +55,19 @@ imu0_nodelay: false
 imu0_differential: false
 imu0_relative: true
 imu0_queue_size: 10
-imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
-imu0_twist_rejection_threshold: 0.8                #
+imu0_pose_rejection_threshold: 1.5                 # Note the difference in parameter names
+imu0_twist_rejection_threshold: 0.4                #
 imu0_linear_acceleration_rejection_threshold: 0.8  #
 imu0_remove_gravitational_acceleration: true
-
-
-use_control: true
-stamped_control: false
-control_timeout: 0.2
-control_config: [true, false, false, false, false, true]
-acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
-deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
-acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
-deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+imu0_orientation_outlier_rejection: 4
 
 
 process_noise_covariance: [0.05, 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.05, 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.06, 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.86, 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.03, 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.03, 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.06, 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.7, 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.025, 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.025, 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.04, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
@@ -89,10 +80,10 @@ process_noise_covariance: [0.05, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
 
 initial_estimate_covariance: [1e-9, 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,    1e-9, 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,    1e-9, 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,    1e-2, 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,    1e-9, 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,    1e-9, 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,    1e-9, 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,    1e-3, 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,    1e-9, 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,    1e-9, 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,    1e-9, 0.0,     0.0,     0.0,     0.0,    0.0,    0.0,

igvc2023/config/gps_parameters.yaml

@@ -1,7 +1,7 @@
 # maintainer:kbkn/mori
 #
 # name of the port that connects to gps device
-dev_name: /dev/ttyACM0
+dev_name: /dev/sensors/GNSSbase
 # directory of the file where waypoints will be saved
 output_file: /home/ubuntu/catkin_ws/src/igvc2023/config/gps_waypoints/gps_waypoints.yaml
 #output_file: /home/ubuntu/catkin_ws/src/igvc2023/config/gps_waypoints/qualification_waypoints.yaml

igvc2023/config/navsatekf.yaml

@@ -1,10 +1,10 @@
 frequency: 30 # 30
 delay: 0
 magnetic_declination_radians: 0 # 0
-yaw_offset: 0.0
+yaw_offset: 1.5707963
 zero_altitude: false # false
-broadcast_utm_transform: ture # false
-publish_filtered_gps: ture
+broadcast_cartesian_transform: true # false
+publish_filtered_gps: true
 use_odometry_yaw: false #false
-wait_for_datum: ture #true
+wait_for_datum: true
 datum: [0, 0, 0.0] #[55.944904, -3.186693, 0.0]

igvc2023/launch/segmentation.launch

@@ -11,8 +11,8 @@
 
     <!-- Convert 3d pointcloud to 2d laserscan -->
     <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
-        <remap from="cloud_in" to="obstacle_cloud"/>
-        <!--<remap from="cloud_in" to="ground_segmentation/obstacle_cloud"/>-->
+        <remap from="cloud_in" to="ground_segmentation/obstacle_cloud"/>
+        <!-- <remap from="cloud_in" to="velodyne_points"/> -->
         <remap from="scan"     to="velodyne_scan"/>
         <rosparam command="load" file="$(find igvc2023)/params/pointcloud_to_laserscan_params.yaml"/>
     </node>

igvc2023/launch/sensors.launch

@@ -7,9 +7,9 @@
     <arg name="imu_dev"          default="/dev/sensors/imu"/>
     <arg name="camera_dev"       default="/dev/sensors/insta360_air"/>
     <arg name="LED_dev"          default="/dev/sensors/LED"/>
-    <arg name="gps_dev"          default="/dev/ttyACM0"/>
-    <arg name="gps1_dev"         default="/dev/ttyACM1"/>
-    <arg name="gps2_dev"         default="/dev/ttyACM2"/>
+    <arg name="gps_dev"          default="/dev/sensors/CLAS"/>
+    <arg name="gps1_dev"         default="/dev/sensors/GNSSbase"/>
+    <arg name="gps2_dev"         default="/dev/sensors/GNSSrover"/>
     <arg name="use_ekf"          default="true"/>
     <arg name="urg_ip"           default="192.168.3.11"/>
 
@@ -32,6 +32,11 @@
         <param name="baud" value="115200"/>
     </node>
 
+    <!-- Node to setting up Arduino serial communication for LEDs -->
+    <node pkg="rosserial_python" type="serial_node.py" name="serial_node_LED">
+        <param name="port" value="$(arg LED_dev)"/>
+        <param name="baud" value="115200"/>
+    </node>
 
     <!-- Node for setting up 2D-LiDAR via Ethernet connection -->
     <node pkg="urg_node" type="urg_node" name="urg_node">

igvc2023/launch/start.launch

@@ -7,9 +7,9 @@
     <arg name="imu_dev"          default="/dev/sensors/imu"/>
     <arg name="camera_dev"       default="/dev/sensors/insta360_air"/>
     <arg name="LED_dev"          default="/dev/sensors/LED"/>
-    <arg name="gps_dev"          default="/dev/ttyACM0"/>
-    <arg name="gps1_dev"         default="/dev/ttyACM1"/>
-    <arg name="gps2_dev"         default="/dev/ttyACM2"/>
+    <arg name="gps_dev"          default="/dev/sensors/CLAS"/>
+    <arg name="gps1_dev"         default="/dev/sensors/GNSSbase"/>
+    <arg name="gps2_dev"         default="/dev/sensors/GNSSrover"/>
     <arg name="cmd_vel"          default="/cmd_vel"/>
     <arg name="motor_driver_dev" default="/dev/ZLAC8015D"/>
     <arg name="estop_dev"        default="/dev/E-Stop"/>

igvc2023/scripts/gps_goal_setter2.py

@@ -49,7 +49,7 @@ def get_coordinate_from_tf(self, stamp):
     # if failed, retry up to 3 times
     for i in range(3):
       try:
-        tf = self.tfBuffer.lookup_transform("map", "base_link", stamp, rospy.Duration(1.0))
+        tf = self.tfBuffer.lookup_transform("map", "base_footprint", stamp, rospy.Duration(1.0))
       except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as err:
         if not i == 2:
           rospy.logwarn(err)

igvc2023/scripts/local_goal_setter2.py

@@ -126,7 +126,7 @@ def callback(self, msg):
 		#-----Listen tf map->base_link-----
 		try:
 			stamp = msg.header.stamp
-			tf = self.tfBuffer.lookup_transform("map", "base_link", stamp, rospy.Duration(1.0))
+			tf = self.tfBuffer.lookup_transform("map", "base_footprint", stamp, rospy.Duration(1.0))
 		except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
 			print(e)
 			return

igvc2023/scripts/marker_setter.py

@@ -7,7 +7,7 @@
 from visualization_msgs.msg import MarkerArray
 
 topic = "visualization_marker_array"
-publisher = rospy.Publisher(topic, MarkerArray)
+publisher = rospy.Publisher(topic, MarkerArray, queue_size=1)
 
 rospy.init_node("marker_setter")
 markerArray = MarkerArray()

igvc2023/urdf/orange.gazebo

@@ -5,7 +5,7 @@
     <plugin name="diff_drive_controller" filename="libgazebo_ros_diff_drive.so">
       <commandTopic>cmd_vel</commandTopic>
       <odometryTopic>odom</odometryTopic>
-      <robotBaseFrame>base_link</robotBaseFrame>
+      <robotBaseFrame>base_footprint</robotBaseFrame>
       <odometryFrame>odom</odometryFrame>
       <odometrySource>world</odometrySource>
 

igvc2023/urdf/orange.xacro

@@ -41,7 +41,7 @@
   <!-- Import fisheye camera model -->
   <xacro:include filename="$(find igvc2023)/urdf/sensors/camera.urdf.xacro"/>
   <xacro:sensor_camera name="camera" parent="base_link">
-    <origin xyz="-0.25 0 1.6" rpy="0 ${PI/2} 0"/>
+    <origin xyz="-0.25 0 1.6" rpy="0 0 0"/>
   </xacro:sensor_camera>
 
   <!-- Import gps model -->

igvc2023/urdf/sensors/camera.urdf.xacro

@@ -33,7 +33,7 @@
       <sensor type="wideanglecamera" name="cv_camera">
         <update_rate>10.0</update_rate>
         <camera>
-          <pose>0 0 0 0 0 0</pose>
+          <pose>0 0 0 0 ${PI/2} 0</pose>
           <horizontal_fov>3.1415</horizontal_fov>
           <image>
             <format>R8G8B8</format>

igvc2023/world/course_v2.world

@@ -42,7 +42,7 @@
     </include>
     <include>
     	<uri>model://barricade</uri>
-    	<pose>9.57 28.8819 0 0 0 0.30353</pose>
+    	<pose>9.42 29.0819 0 0 0 -0.30353</pose>
     	<name>Barricade 3</name>
     </include>
     <include>

line_detection/src/imRtheta_igvc2022.cpp

@@ -97,9 +97,9 @@ class ImageConverter
 		}
 		*/
 
-		cv::resize(rtheta_img, rtheta_img, cv::Size(), 2, 2);
-		cv::imshow("r-theta image", rtheta_img*255);
-		cv::waitKey(1);
+		// cv::resize(rtheta_img, rtheta_img, cv::Size(), 2, 2);
+		// cv::imshow("r-theta image", rtheta_img*255);
+		// cv::waitKey(1);
 	}
 
 

line_detection/src/rtheta2scan_igvc2022.cpp

@@ -74,8 +74,8 @@ class RthetaToLaserScan
 		camera_scan.intensities = {};
 
 		vector<float> ranges(num_readings, numeric_limits<float>::infinity());
-		//float r2d = 0.063;  // multiplication value for convert to reasl destance from pixel distance
-		float r2d = 0.09;
+		float r2d = 0.063;  // multiplication value for convert to real distance from pixel distance
+		// float r2d = 0.09;
 		unsigned char *ptr;
 		unsigned char pixval;
 		for(int i=0; i<max_distance; i++) {

openslam_gmapping/gridfastslam/gridslamprocessor.cpp

@@ -397,8 +397,8 @@ void GridSlamProcessor::setMotionModelParameters
 		     << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
 
 
-      cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y 
-	   << " " << reading.getPose().theta << endl;
+    //   cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y 
+	  //  << " " << reading.getPose().theta << endl;
 
 
       //this is for converting the reading in a scan-matcher feedable form

openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx

@@ -148,7 +148,7 @@ inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSiz
     hasResampled = true;
   } else {
     int index=0;
-    std::cerr << "Registering Scans:";
+    // std::cerr << "Registering Scans:";
     TNodeVector::iterator node_it=oldGeneration.begin();
     for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
       //create a new node in the particle tree and add it to the old tree
@@ -168,7 +168,7 @@ inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSiz
       node_it++;
 
     }
-    std::cerr  << "Done" <<std::endl;
+    // std::cerr  << "Done" <<std::endl;
 
   }
   //END: BUILDING TREE