Merge pull request #10 from icns-distributed-cloud/v0.4.0

add gps goal

aigo_nav/launch/aigo.launch

@@ -11,7 +11,7 @@
     <!-- launch imu, topic will be published to /imu and gps topic will be published to /gps -->
 
     <!-- <include file="$(find wit_node)/launch/wit.launch"/> -->
-    <include file="$(find ublox_gps)/launch/ublox_zed-f9p.launch"/>
+
 
     <!--include file="$(find ntrip_ros)/launch/ntrip_ros.launch"/-->
     <!-- launch aigo_serial to communicate with STM32 board -->

aigo_nav/launch/configure.launch

@@ -4,8 +4,10 @@
     <!-- launch lidar, topic will be published to /scan-->
     <include file="$(find ydlidar_ros_driver)/launch/X4.launch"/>
     <!-- launch imu, topic will be published to /imu and filtered to /imu/data -->
-    <include file="$(find aigo_nav)/launch/imu.launch"/>
+    <include file="$(find wit_node)/launch/wit.launch"/>
+    <include file="$(find ublox_gps)/launch/ublox_zed-f9p.launch"/>
     <!-- launch gps, topic will be published to /fix -->
-    <include file="$(find aigo_nav)/launch/gps.launch"/>
+    <!-- <include file="$(find aigo_nav)/launch/gps.launch"/> -->
     <!--include file="$(find ublox_gps)/launch/ublox_zed-f9p.launch"/-->
+    <include file="$(find ntrip_ros)/launch/ntrip_ros.launch"/>
 </launch>

aigo_nav/params/base_local_planner_params.yaml

@@ -18,7 +18,7 @@ TrajectoryPlannerROS:
 
   # Goal Tolerance Parameters
   yaw_goal_tolerance: 3.14  # in rads
-  xy_goal_tolerance: 1  # in meters
+  xy_goal_tolerance: 0.5  # in meters
   latch_xy_goal_tolerance: false
 
   # Forward Simulation Parameters

aigo_nav/params/costmap_common_params.yaml

@@ -5,8 +5,8 @@ z_resolution: 1 # The z resolution of the map in meters/cell.
 z_voxels: 2  # The number of voxels to in each vertical column, the height of the grid is z resolution * z voxels.
 #
 
-obstacle_range: 1.5 # The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.
-raytrace_range: 1.5 # The default range in meters at which to raytrace out obstacles from the map using sensor data
+obstacle_range: 1.0 # The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.
+raytrace_range: 1.0 # The default range in meters at which to raytrace out obstacles from the map using sensor data
 
 #
 publish_voxel_map: false
@@ -20,4 +20,4 @@ cost_scaling_factor: 10 # slope of the cost decay curve with respect to distance
 
 observation_sources: laser_scan_sensor
 
-laser_scan_sensor: {sensor_frame: base_link, data_type: LaserScan, topic: scan, marking: true, clearing: true}
+laser_scan_sensor: {sensor_frame: base_link, data_type: LaserScan, topic: scan, expected_update_rate: 0.5, marking: true, clearing: true}

aigo_nav/params/ekf_params.yaml

@@ -50,7 +50,7 @@ ekf_se_odom: # Used only for broadcasting odom to base_link transforms
                 false,  false, false,
                 false,  false, false,
                 false,  false, true,
-                false,  false, false]
+                true,  true, false]
   imu0_nodelay: false
   imu0_differential: false
   imu0_relative: false
@@ -59,14 +59,14 @@ ekf_se_odom: # Used only for broadcasting odom to base_link transforms
 
   use_control: false
 
-process_noise_covariance: [0.05, 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,
+process_noise_covariance: [5.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    5.05, 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.03, 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.06, 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.025, 0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    10,    0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     10,    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,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
@@ -138,16 +138,16 @@ ekf_se_map:
 # -------------------------------------
 # GPS odometry:
 
-  #  odom1: /odometry/gps
-  #odom1_config: [true,  true,  false,
-  #               false, false, false,
-  #               false, false, false,
-  #               false, false, false,
-  #               false, false, false]
-  #odom1_queue_size: 10
-  #odom1_nodelay: true
-  #odom1_differential: false
-  #odom1_relative: false
+  odom1: /odometry/gps
+  odom1_config: [true,  true,  false,
+                 false, false, false,
+                 false, false, false,
+                 false, false, false,
+                 false, false, false]
+  odom1_queue_size: 10
+  odom1_nodelay: true
+  odom1_differential: false
+  odom1_relative: false
 
 
 # --------------------------------------
@@ -158,7 +158,7 @@ ekf_se_map:
                 false, false, false,
                 false, false, false,
                 false, false, true,
-                false, false, false]
+                true, true, false]
   imu0_nodelay: true
   imu0_differential: false
   imu0_relative: false
@@ -173,8 +173,8 @@ process_noise_covariance: [0.05, 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.03, 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.025, 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,    10,    0,     0,    0,    0,    0,    0,    0,    0,
+                           0,    0,    0,    0,    0,    0,    0,     10,    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,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,

aigo_serial/src/aigo_serial.py

@@ -20,13 +20,13 @@ def calculate_pose():
     global right_vel
     #in STM32, MOTOR_SPEED = OUTPUT * 15 (MOTOR_SPEED < 9000)
     #turn right
-    if(left > right+100):
-        left_vel = 600
-        right_vel = 600
-    #turn left
-    elif(left+100 < right):
+    if(left > right+50):
         left_vel = 600
         right_vel = -600
+    #turn left
+    elif(left+50 < right):
+        left_vel = -600
+        right_vel = 600
     elif(left > 0 and right > 0):
         left_vel = 400
         right_vel = 400

aigo_serial/src/aigo_serial_sub.py

@@ -1,14 +1,21 @@
 #!/usr/bin/env python
 
 import rospy
+import 
 from std_msgs.msg import Float32, Int32
+from sensor_msgs.msg import NavSatFix
 import serial, time, re
 
 ser_front = serial.Serial(\
     port = '/dev/ttyAMA2', \
     baudrate = 115200,
 )
 
+def gps_callback(msg):
+    global lat
+    global long
+
+
 def left_callback(msg):
     lwheel_desired_rate = msg.data
 
@@ -17,16 +24,18 @@ def right_callback(msg):
 
 if __name__ == '__main__':
     rospy.init_node('aigo_serial_sub')
-    lwheel_desired_rate = 0
-    rwheel_desired_rate = 0
-    lwheel_desired_rate_sub = rospy.Subscriber('lwheel_desired_rate', Int32, left_callback)
-    rwheel_desired_rate_sub = rospy.Subscriber('rwheel_desired_rate', Int32, right_callback)
-    stm32_msg = str(lwheel_desired_rate)+','+str(rwheel_desired_rate)
+   # lwheel_desired_rate = 0
+  #  rwheel_desired_rate = 0
+    gps_sub = rospy.Subscriber('/fix', NavSatFix, gps_callback)
+    with 
+    #lwheel_desired_rate_sub = rospy.Subscriber('lwheel_desired_rate', Int32, left_callback)
+    #rwheel_desired_rate_sub = rospy.Subscriber('rwheel_desired_rate', Int32, right_callback)
+ #   stm32_msg = str(lwheel_desired_rate)+','+str(rwheel_desired_rate)
 
-    while not rospy.is_shutdown():
-        time.sleep(0.1)
-        ser_front.write(stm32_msg)
-	ser_front.flushInput()
-    	ser_front.flushOutput()
-ser_front.close()
+    #while not rospy.is_shutdown():
+    #    time.sleep(0.1)
+    #    ser_front.write(stm32_msg)
+#	ser_front.flushInput()
+#    	ser_front.flushOutput()
+#ser_front.close()
 

diff_drive/launch/aigo.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <arg name="ticks_per_meter" value="10000" />
+  <arg name="ticks_per_meter" value="5363" />
   <arg name="wheel_separation" value="0.2" />
   <arg name="rate" value="2.0" />
   <arg name="urdf_file" default="$(find aigo_urdf)/urdf/aigo.urdf" />

diff_drive/nodes/diff_drive_odometry

@@ -20,7 +20,7 @@ class OdometryNode:
 
     def main(self):
         self.odomPub = rospy.Publisher('odom', Odometry, queue_size=10)
-        self.tfPub = TransformBroadcaster()
+#        self.tfPub = TransformBroadcaster()
 
         rospy.init_node('diff_drive_odometry')
         self.nodeName = rospy.get_name()
@@ -55,13 +55,13 @@ class OdometryNode:
         pose = self.odometry.getPose()
 
         q = quaternion_from_euler(0, 0, pose.theta)
-        self.tfPub.sendTransform(
-            (pose.x, pose.y, 0),
-            (q[0], q[1], q[2], q[3]),
-            now,
-            self.baseFrameID,
-            self.odomFrameID
-        )
+#        self.tfPub.sendTransform(
+#            (pose.x, pose.y, 0),
+#            (q[0], q[1], q[2], q[3]),
+#            now,
+#            self.baseFrameID,
+#            self.odomFrameID
+#        )
 
         odom = Odometry()
         odom.header.stamp = now

gps_goal/.gitbak/HEAD

@@ -0,0 +1 @@
+ref: refs/heads/master

gps_goal/.gitbak/config

@@ -0,0 +1,11 @@
+[core]
+	repositoryformatversion = 0
+	filemode = true
+	bare = false
+	logallrefupdates = true
+[remote "origin"]
+	url = https://github.com/danielsnider/gps_goal
+	fetch = +refs/heads/*:refs/remotes/origin/*
+[branch "master"]
+	remote = origin
+	merge = refs/heads/master

gps_goal/.gitbak/description

@@ -0,0 +1 @@
+Unnamed repository; edit this file 'description' to name the repository.

gps_goal/.gitbak/hooks/applypatch-msg.sample

@@ -0,0 +1,15 @@
+#!/bin/sh
+#
+# An example hook script to check the commit log message taken by
+# applypatch from an e-mail message.
+#
+# The hook should exit with non-zero status after issuing an
+# appropriate message if it wants to stop the commit.  The hook is
+# allowed to edit the commit message file.
+#
+# To enable this hook, rename this file to "applypatch-msg".
+
+. git-sh-setup
+commitmsg="$(git rev-parse --git-path hooks/commit-msg)"
+test -x "$commitmsg" && exec "$commitmsg" ${1+"$@"}
+:

gps_goal/.gitbak/hooks/commit-msg.sample

@@ -0,0 +1,24 @@
+#!/bin/sh
+#
+# An example hook script to check the commit log message.
+# Called by "git commit" with one argument, the name of the file
+# that has the commit message.  The hook should exit with non-zero
+# status after issuing an appropriate message if it wants to stop the
+# commit.  The hook is allowed to edit the commit message file.
+#
+# To enable this hook, rename this file to "commit-msg".
+
+# Uncomment the below to add a Signed-off-by line to the message.
+# Doing this in a hook is a bad idea in general, but the prepare-commit-msg
+# hook is more suited to it.
+#
+# SOB=$(git var GIT_AUTHOR_IDENT | sed -n 's/^\(.*>\).*$/Signed-off-by: \1/p')
+# grep -qs "^$SOB" "$1" || echo "$SOB" >> "$1"
+
+# This example catches duplicate Signed-off-by lines.
+
+test "" = "$(grep '^Signed-off-by: ' "$1" |
+	 sort | uniq -c | sed -e '/^[ 	]*1[ 	]/d')" || {
+	echo >&2 Duplicate Signed-off-by lines.
+	exit 1
+}