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

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/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