diff --git a/aigo_nav/launch/aigo.launch b/aigo_nav/launch/aigo.launch index 9eee4f1..0ca73f2 100644 --- a/aigo_nav/launch/aigo.launch +++ b/aigo_nav/launch/aigo.launch @@ -11,7 +11,7 @@ - + diff --git a/aigo_nav/launch/configure.launch b/aigo_nav/launch/configure.launch index 5cf4d77..f2da0ef 100644 --- a/aigo_nav/launch/configure.launch +++ b/aigo_nav/launch/configure.launch @@ -4,8 +4,10 @@ - + + - + + diff --git a/aigo_nav/params/base_local_planner_params.yaml b/aigo_nav/params/base_local_planner_params.yaml index e1106dd..4b60f5a 100644 --- a/aigo_nav/params/base_local_planner_params.yaml +++ b/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 diff --git a/aigo_nav/params/costmap_common_params.yaml b/aigo_nav/params/costmap_common_params.yaml index 558a5e6..eedd5fc 100644 --- a/aigo_nav/params/costmap_common_params.yaml +++ b/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} diff --git a/aigo_nav/params/ekf_params.yaml b/aigo_nav/params/ekf_params.yaml index 50a2d16..799bf10 100644 --- a/aigo_nav/params/ekf_params.yaml +++ b/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, diff --git a/aigo_serial/src/aigo_serial.py b/aigo_serial/src/aigo_serial.py index b32dbdf..d418a73 100755 --- a/aigo_serial/src/aigo_serial.py +++ b/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 diff --git a/aigo_serial/src/aigo_serial_sub.py b/aigo_serial/src/aigo_serial_sub.py index bb6cafd..496e728 100755 --- a/aigo_serial/src/aigo_serial_sub.py +++ b/aigo_serial/src/aigo_serial_sub.py @@ -1,7 +1,9 @@ #!/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(\ @@ -9,6 +11,11 @@ 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 --git a/diff_drive/launch/aigo.launch b/diff_drive/launch/aigo.launch index 04df5b8..e81e7f7 100644 --- a/diff_drive/launch/aigo.launch +++ b/diff_drive/launch/aigo.launch @@ -1,6 +1,6 @@ - + diff --git a/diff_drive/nodes/diff_drive_odometry b/diff_drive/nodes/diff_drive_odometry index 71aa70e..6d21732 100755 --- a/diff_drive/nodes/diff_drive_odometry +++ b/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 diff --git a/gps_goal/.gitbak/HEAD b/gps_goal/.gitbak/HEAD new file mode 100644 index 0000000..cb089cd --- /dev/null +++ b/gps_goal/.gitbak/HEAD @@ -0,0 +1 @@ +ref: refs/heads/master diff --git a/gps_goal/.gitbak/config b/gps_goal/.gitbak/config new file mode 100644 index 0000000..a6502e8 --- /dev/null +++ b/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 diff --git a/gps_goal/.gitbak/description b/gps_goal/.gitbak/description new file mode 100644 index 0000000..498b267 --- /dev/null +++ b/gps_goal/.gitbak/description @@ -0,0 +1 @@ +Unnamed repository; edit this file 'description' to name the repository. diff --git a/gps_goal/.gitbak/hooks/applypatch-msg.sample b/gps_goal/.gitbak/hooks/applypatch-msg.sample new file mode 100755 index 0000000..a5d7b84 --- /dev/null +++ b/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+"$@"} +: diff --git a/gps_goal/.gitbak/hooks/commit-msg.sample b/gps_goal/.gitbak/hooks/commit-msg.sample new file mode 100755 index 0000000..b58d118 --- /dev/null +++ b/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 +} diff --git a/gps_goal/.gitbak/hooks/fsmonitor-watchman.sample b/gps_goal/.gitbak/hooks/fsmonitor-watchman.sample new file mode 100755 index 0000000..e673bb3 --- /dev/null +++ b/gps_goal/.gitbak/hooks/fsmonitor-watchman.sample @@ -0,0 +1,114 @@ +#!/usr/bin/perl + +use strict; +use warnings; +use IPC::Open2; + +# An example hook script to integrate Watchman +# (https://facebook.github.io/watchman/) with git to speed up detecting +# new and modified files. +# +# The hook is passed a version (currently 1) and a time in nanoseconds +# formatted as a string and outputs to stdout all files that have been +# modified since the given time. Paths must be relative to the root of +# the working tree and separated by a single NUL. +# +# To enable this hook, rename this file to "query-watchman" and set +# 'git config core.fsmonitor .git/hooks/query-watchman' +# +my ($version, $time) = @ARGV; + +# Check the hook interface version + +if ($version == 1) { + # convert nanoseconds to seconds + $time = int $time / 1000000000; +} else { + die "Unsupported query-fsmonitor hook version '$version'.\n" . + "Falling back to scanning...\n"; +} + +my $git_work_tree; +if ($^O =~ 'msys' || $^O =~ 'cygwin') { + $git_work_tree = Win32::GetCwd(); + $git_work_tree =~ tr/\\/\//; +} else { + require Cwd; + $git_work_tree = Cwd::cwd(); +} + +my $retry = 1; + +launch_watchman(); + +sub launch_watchman { + + my $pid = open2(\*CHLD_OUT, \*CHLD_IN, 'watchman -j --no-pretty') + or die "open2() failed: $!\n" . + "Falling back to scanning...\n"; + + # In the query expression below we're asking for names of files that + # changed since $time but were not transient (ie created after + # $time but no longer exist). + # + # To accomplish this, we're using the "since" generator to use the + # recency index to select candidate nodes and "fields" to limit the + # output to file names only. Then we're using the "expression" term to + # further constrain the results. + # + # The category of transient files that we want to ignore will have a + # creation clock (cclock) newer than $time_t value and will also not + # currently exist. + + my $query = <<" END"; + ["query", "$git_work_tree", { + "since": $time, + "fields": ["name"], + "expression": ["not", ["allof", ["since", $time, "cclock"], ["not", "exists"]]] + }] + END + + print CHLD_IN $query; + close CHLD_IN; + my $response = do {local $/; }; + + die "Watchman: command returned no output.\n" . + "Falling back to scanning...\n" if $response eq ""; + die "Watchman: command returned invalid output: $response\n" . + "Falling back to scanning...\n" unless $response =~ /^\{/; + + my $json_pkg; + eval { + require JSON::XS; + $json_pkg = "JSON::XS"; + 1; + } or do { + require JSON::PP; + $json_pkg = "JSON::PP"; + }; + + my $o = $json_pkg->new->utf8->decode($response); + + if ($retry > 0 and $o->{error} and $o->{error} =~ m/unable to resolve root .* directory (.*) is not watched/) { + print STDERR "Adding '$git_work_tree' to watchman's watch list.\n"; + $retry--; + qx/watchman watch "$git_work_tree"/; + die "Failed to make watchman watch '$git_work_tree'.\n" . + "Falling back to scanning...\n" if $? != 0; + + # Watchman will always return all files on the first query so + # return the fast "everything is dirty" flag to git and do the + # Watchman query just to get it over with now so we won't pay + # the cost in git to look up each individual file. + print "/\0"; + eval { launch_watchman() }; + exit 0; + } + + die "Watchman: $o->{error}.\n" . + "Falling back to scanning...\n" if $o->{error}; + + binmode STDOUT, ":utf8"; + local $, = "\0"; + print @{$o->{files}}; +} diff --git a/gps_goal/.gitbak/hooks/post-update.sample b/gps_goal/.gitbak/hooks/post-update.sample new file mode 100755 index 0000000..ec17ec1 --- /dev/null +++ b/gps_goal/.gitbak/hooks/post-update.sample @@ -0,0 +1,8 @@ +#!/bin/sh +# +# An example hook script to prepare a packed repository for use over +# dumb transports. +# +# To enable this hook, rename this file to "post-update". + +exec git update-server-info diff --git a/gps_goal/.gitbak/hooks/pre-applypatch.sample b/gps_goal/.gitbak/hooks/pre-applypatch.sample new file mode 100755 index 0000000..4142082 --- /dev/null +++ b/gps_goal/.gitbak/hooks/pre-applypatch.sample @@ -0,0 +1,14 @@ +#!/bin/sh +# +# An example hook script to verify what is about to be committed +# 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. +# +# To enable this hook, rename this file to "pre-applypatch". + +. git-sh-setup +precommit="$(git rev-parse --git-path hooks/pre-commit)" +test -x "$precommit" && exec "$precommit" ${1+"$@"} +: diff --git a/gps_goal/.gitbak/hooks/pre-commit.sample b/gps_goal/.gitbak/hooks/pre-commit.sample new file mode 100755 index 0000000..68d62d5 --- /dev/null +++ b/gps_goal/.gitbak/hooks/pre-commit.sample @@ -0,0 +1,49 @@ +#!/bin/sh +# +# An example hook script to verify what is about to be committed. +# Called by "git commit" with no arguments. The hook should +# exit with non-zero status after issuing an appropriate message if +# it wants to stop the commit. +# +# To enable this hook, rename this file to "pre-commit". + +if git rev-parse --verify HEAD >/dev/null 2>&1 +then + against=HEAD +else + # Initial commit: diff against an empty tree object + against=4b825dc642cb6eb9a060e54bf8d69288fbee4904 +fi + +# If you want to allow non-ASCII filenames set this variable to true. +allownonascii=$(git config --bool hooks.allownonascii) + +# Redirect output to stderr. +exec 1>&2 + +# Cross platform projects tend to avoid non-ASCII filenames; prevent +# them from being added to the repository. We exploit the fact that the +# printable range starts at the space character and ends with tilde. +if [ "$allownonascii" != "true" ] && + # Note that the use of brackets around a tr range is ok here, (it's + # even required, for portability to Solaris 10's /usr/bin/tr), since + # the square bracket bytes happen to fall in the designated range. + test $(git diff --cached --name-only --diff-filter=A -z $against | + LC_ALL=C tr -d '[ -~]\0' | wc -c) != 0 +then + cat <<\EOF +Error: Attempt to add a non-ASCII file name. + +This can cause problems if you want to work with people on other platforms. + +To be portable it is advisable to rename the file. + +If you know what you are doing you can disable this check using: + + git config hooks.allownonascii true +EOF + exit 1 +fi + +# If there are whitespace errors, print the offending file names and fail. +exec git diff-index --check --cached $against -- diff --git a/gps_goal/.gitbak/hooks/pre-push.sample b/gps_goal/.gitbak/hooks/pre-push.sample new file mode 100755 index 0000000..6187dbf --- /dev/null +++ b/gps_goal/.gitbak/hooks/pre-push.sample @@ -0,0 +1,53 @@ +#!/bin/sh + +# An example hook script to verify what is about to be pushed. Called by "git +# push" after it has checked the remote status, but before anything has been +# pushed. If this script exits with a non-zero status nothing will be pushed. +# +# This hook is called with the following parameters: +# +# $1 -- Name of the remote to which the push is being done +# $2 -- URL to which the push is being done +# +# If pushing without using a named remote those arguments will be equal. +# +# Information about the commits which are being pushed is supplied as lines to +# the standard input in the form: +# +# +# +# This sample shows how to prevent push of commits where the log message starts +# with "WIP" (work in progress). + +remote="$1" +url="$2" + +z40=0000000000000000000000000000000000000000 + +while read local_ref local_sha remote_ref remote_sha +do + if [ "$local_sha" = $z40 ] + then + # Handle delete + : + else + if [ "$remote_sha" = $z40 ] + then + # New branch, examine all commits + range="$local_sha" + else + # Update to existing branch, examine new commits + range="$remote_sha..$local_sha" + fi + + # Check for WIP commit + commit=`git rev-list -n 1 --grep '^WIP' "$range"` + if [ -n "$commit" ] + then + echo >&2 "Found WIP commit in $local_ref, not pushing" + exit 1 + fi + fi +done + +exit 0 diff --git a/gps_goal/.gitbak/hooks/pre-rebase.sample b/gps_goal/.gitbak/hooks/pre-rebase.sample new file mode 100755 index 0000000..6cbef5c --- /dev/null +++ b/gps_goal/.gitbak/hooks/pre-rebase.sample @@ -0,0 +1,169 @@ +#!/bin/sh +# +# Copyright (c) 2006, 2008 Junio C Hamano +# +# The "pre-rebase" hook is run just before "git rebase" starts doing +# its job, and can prevent the command from running by exiting with +# non-zero status. +# +# The hook is called with the following parameters: +# +# $1 -- the upstream the series was forked from. +# $2 -- the branch being rebased (or empty when rebasing the current branch). +# +# This sample shows how to prevent topic branches that are already +# merged to 'next' branch from getting rebased, because allowing it +# would result in rebasing already published history. + +publish=next +basebranch="$1" +if test "$#" = 2 +then + topic="refs/heads/$2" +else + topic=`git symbolic-ref HEAD` || + exit 0 ;# we do not interrupt rebasing detached HEAD +fi + +case "$topic" in +refs/heads/??/*) + ;; +*) + exit 0 ;# we do not interrupt others. + ;; +esac + +# Now we are dealing with a topic branch being rebased +# on top of master. Is it OK to rebase it? + +# Does the topic really exist? +git show-ref -q "$topic" || { + echo >&2 "No such branch $topic" + exit 1 +} + +# Is topic fully merged to master? +not_in_master=`git rev-list --pretty=oneline ^master "$topic"` +if test -z "$not_in_master" +then + echo >&2 "$topic is fully merged to master; better remove it." + exit 1 ;# we could allow it, but there is no point. +fi + +# Is topic ever merged to next? If so you should not be rebasing it. +only_next_1=`git rev-list ^master "^$topic" ${publish} | sort` +only_next_2=`git rev-list ^master ${publish} | sort` +if test "$only_next_1" = "$only_next_2" +then + not_in_topic=`git rev-list "^$topic" master` + if test -z "$not_in_topic" + then + echo >&2 "$topic is already up to date with master" + exit 1 ;# we could allow it, but there is no point. + else + exit 0 + fi +else + not_in_next=`git rev-list --pretty=oneline ^${publish} "$topic"` + /usr/bin/perl -e ' + my $topic = $ARGV[0]; + my $msg = "* $topic has commits already merged to public branch:\n"; + my (%not_in_next) = map { + /^([0-9a-f]+) /; + ($1 => 1); + } split(/\n/, $ARGV[1]); + for my $elem (map { + /^([0-9a-f]+) (.*)$/; + [$1 => $2]; + } split(/\n/, $ARGV[2])) { + if (!exists $not_in_next{$elem->[0]}) { + if ($msg) { + print STDERR $msg; + undef $msg; + } + print STDERR " $elem->[1]\n"; + } + } + ' "$topic" "$not_in_next" "$not_in_master" + exit 1 +fi + +<<\DOC_END + +This sample hook safeguards topic branches that have been +published from being rewound. + +The workflow assumed here is: + + * Once a topic branch forks from "master", "master" is never + merged into it again (either directly or indirectly). + + * Once a topic branch is fully cooked and merged into "master", + it is deleted. If you need to build on top of it to correct + earlier mistakes, a new topic branch is created by forking at + the tip of the "master". This is not strictly necessary, but + it makes it easier to keep your history simple. + + * Whenever you need to test or publish your changes to topic + branches, merge them into "next" branch. + +The script, being an example, hardcodes the publish branch name +to be "next", but it is trivial to make it configurable via +$GIT_DIR/config mechanism. + +With this workflow, you would want to know: + +(1) ... if a topic branch has ever been merged to "next". Young + topic branches can have stupid mistakes you would rather + clean up before publishing, and things that have not been + merged into other branches can be easily rebased without + affecting other people. But once it is published, you would + not want to rewind it. + +(2) ... if a topic branch has been fully merged to "master". + Then you can delete it. More importantly, you should not + build on top of it -- other people may already want to + change things related to the topic as patches against your + "master", so if you need further changes, it is better to + fork the topic (perhaps with the same name) afresh from the + tip of "master". + +Let's look at this example: + + o---o---o---o---o---o---o---o---o---o "next" + / / / / + / a---a---b A / / + / / / / + / / c---c---c---c B / + / / / \ / + / / / b---b C \ / + / / / / \ / + ---o---o---o---o---o---o---o---o---o---o---o "master" + + +A, B and C are topic branches. + + * A has one fix since it was merged up to "next". + + * B has finished. It has been fully merged up to "master" and "next", + and is ready to be deleted. + + * C has not merged to "next" at all. + +We would want to allow C to be rebased, refuse A, and encourage +B to be deleted. + +To compute (1): + + git rev-list ^master ^topic next + git rev-list ^master next + + if these match, topic has not merged in next at all. + +To compute (2): + + git rev-list master..topic + + if this is empty, it is fully merged to "master". + +DOC_END diff --git a/gps_goal/.gitbak/hooks/pre-receive.sample b/gps_goal/.gitbak/hooks/pre-receive.sample new file mode 100755 index 0000000..a1fd29e --- /dev/null +++ b/gps_goal/.gitbak/hooks/pre-receive.sample @@ -0,0 +1,24 @@ +#!/bin/sh +# +# An example hook script to make use of push options. +# The example simply echoes all push options that start with 'echoback=' +# and rejects all pushes when the "reject" push option is used. +# +# To enable this hook, rename this file to "pre-receive". + +if test -n "$GIT_PUSH_OPTION_COUNT" +then + i=0 + while test "$i" -lt "$GIT_PUSH_OPTION_COUNT" + do + eval "value=\$GIT_PUSH_OPTION_$i" + case "$value" in + echoback=*) + echo "echo from the pre-receive-hook: ${value#*=}" >&2 + ;; + reject) + exit 1 + esac + i=$((i + 1)) + done +fi diff --git a/gps_goal/.gitbak/hooks/prepare-commit-msg.sample b/gps_goal/.gitbak/hooks/prepare-commit-msg.sample new file mode 100755 index 0000000..10fa14c --- /dev/null +++ b/gps_goal/.gitbak/hooks/prepare-commit-msg.sample @@ -0,0 +1,42 @@ +#!/bin/sh +# +# An example hook script to prepare the commit log message. +# Called by "git commit" with the name of the file that has the +# commit message, followed by the description of the commit +# message's source. The hook's purpose is to edit the commit +# message file. If the hook fails with a non-zero status, +# the commit is aborted. +# +# To enable this hook, rename this file to "prepare-commit-msg". + +# This hook includes three examples. The first one removes the +# "# Please enter the commit message..." help message. +# +# The second includes the output of "git diff --name-status -r" +# into the message, just before the "git status" output. It is +# commented because it doesn't cope with --amend or with squashed +# commits. +# +# The third example adds a Signed-off-by line to the message, that can +# still be edited. This is rarely a good idea. + +COMMIT_MSG_FILE=$1 +COMMIT_SOURCE=$2 +SHA1=$3 + +/usr/bin/perl -i.bak -ne 'print unless(m/^. Please enter the commit message/..m/^#$/)' "$COMMIT_MSG_FILE" + +# case "$COMMIT_SOURCE,$SHA1" in +# ,|template,) +# /usr/bin/perl -i.bak -pe ' +# print "\n" . `git diff --cached --name-status -r` +# if /^#/ && $first++ == 0' "$COMMIT_MSG_FILE" ;; +# *) ;; +# esac + +# SOB=$(git var GIT_COMMITTER_IDENT | sed -n 's/^\(.*>\).*$/Signed-off-by: \1/p') +# git interpret-trailers --in-place --trailer "$SOB" "$COMMIT_MSG_FILE" +# if test -z "$COMMIT_SOURCE" +# then +# /usr/bin/perl -i.bak -pe 'print "\n" if !$first_line++' "$COMMIT_MSG_FILE" +# fi diff --git a/gps_goal/.gitbak/hooks/update.sample b/gps_goal/.gitbak/hooks/update.sample new file mode 100755 index 0000000..80ba941 --- /dev/null +++ b/gps_goal/.gitbak/hooks/update.sample @@ -0,0 +1,128 @@ +#!/bin/sh +# +# An example hook script to block unannotated tags from entering. +# Called by "git receive-pack" with arguments: refname sha1-old sha1-new +# +# To enable this hook, rename this file to "update". +# +# Config +# ------ +# hooks.allowunannotated +# This boolean sets whether unannotated tags will be allowed into the +# repository. By default they won't be. +# hooks.allowdeletetag +# This boolean sets whether deleting tags will be allowed in the +# repository. By default they won't be. +# hooks.allowmodifytag +# This boolean sets whether a tag may be modified after creation. By default +# it won't be. +# hooks.allowdeletebranch +# This boolean sets whether deleting branches will be allowed in the +# repository. By default they won't be. +# hooks.denycreatebranch +# This boolean sets whether remotely creating branches will be denied +# in the repository. By default this is allowed. +# + +# --- Command line +refname="$1" +oldrev="$2" +newrev="$3" + +# --- Safety check +if [ -z "$GIT_DIR" ]; then + echo "Don't run this script from the command line." >&2 + echo " (if you want, you could supply GIT_DIR then run" >&2 + echo " $0 )" >&2 + exit 1 +fi + +if [ -z "$refname" -o -z "$oldrev" -o -z "$newrev" ]; then + echo "usage: $0 " >&2 + exit 1 +fi + +# --- Config +allowunannotated=$(git config --bool hooks.allowunannotated) +allowdeletebranch=$(git config --bool hooks.allowdeletebranch) +denycreatebranch=$(git config --bool hooks.denycreatebranch) +allowdeletetag=$(git config --bool hooks.allowdeletetag) +allowmodifytag=$(git config --bool hooks.allowmodifytag) + +# check for no description +projectdesc=$(sed -e '1q' "$GIT_DIR/description") +case "$projectdesc" in +"Unnamed repository"* | "") + echo "*** Project description file hasn't been set" >&2 + exit 1 + ;; +esac + +# --- Check types +# if $newrev is 0000...0000, it's a commit to delete a ref. +zero="0000000000000000000000000000000000000000" +if [ "$newrev" = "$zero" ]; then + newrev_type=delete +else + newrev_type=$(git cat-file -t $newrev) +fi + +case "$refname","$newrev_type" in + refs/tags/*,commit) + # un-annotated tag + short_refname=${refname##refs/tags/} + if [ "$allowunannotated" != "true" ]; then + echo "*** The un-annotated tag, $short_refname, is not allowed in this repository" >&2 + echo "*** Use 'git tag [ -a | -s ]' for tags you want to propagate." >&2 + exit 1 + fi + ;; + refs/tags/*,delete) + # delete tag + if [ "$allowdeletetag" != "true" ]; then + echo "*** Deleting a tag is not allowed in this repository" >&2 + exit 1 + fi + ;; + refs/tags/*,tag) + # annotated tag + if [ "$allowmodifytag" != "true" ] && git rev-parse $refname > /dev/null 2>&1 + then + echo "*** Tag '$refname' already exists." >&2 + echo "*** Modifying a tag is not allowed in this repository." >&2 + exit 1 + fi + ;; + refs/heads/*,commit) + # branch + if [ "$oldrev" = "$zero" -a "$denycreatebranch" = "true" ]; then + echo "*** Creating a branch is not allowed in this repository" >&2 + exit 1 + fi + ;; + refs/heads/*,delete) + # delete branch + if [ "$allowdeletebranch" != "true" ]; then + echo "*** Deleting a branch is not allowed in this repository" >&2 + exit 1 + fi + ;; + refs/remotes/*,commit) + # tracking branch + ;; + refs/remotes/*,delete) + # delete tracking branch + if [ "$allowdeletebranch" != "true" ]; then + echo "*** Deleting a tracking branch is not allowed in this repository" >&2 + exit 1 + fi + ;; + *) + # Anything else (is there anything else?) + echo "*** Update hook: unknown type of update to ref $refname of type $newrev_type" >&2 + exit 1 + ;; +esac + +# --- Finished +exit 0 diff --git a/gps_goal/.gitbak/index b/gps_goal/.gitbak/index new file mode 100644 index 0000000..122d493 Binary files /dev/null and b/gps_goal/.gitbak/index differ diff --git a/gps_goal/.gitbak/info/exclude b/gps_goal/.gitbak/info/exclude new file mode 100644 index 0000000..a5196d1 --- /dev/null +++ b/gps_goal/.gitbak/info/exclude @@ -0,0 +1,6 @@ +# git ls-files --others --exclude-from=.git/info/exclude +# Lines that start with '#' are comments. +# For a project mostly in C, the following would be a good set of +# exclude patterns (uncomment them if you want to use them): +# *.[oa] +# *~ diff --git a/gps_goal/.gitbak/logs/HEAD b/gps_goal/.gitbak/logs/HEAD new file mode 100644 index 0000000..033b1b5 --- /dev/null +++ b/gps_goal/.gitbak/logs/HEAD @@ -0,0 +1 @@ +0000000000000000000000000000000000000000 6cfdb24513fd603e803a809faac404cd52979c7c easyblank 1630310531 +0000 clone: from https://github.com/danielsnider/gps_goal diff --git a/gps_goal/.gitbak/logs/refs/heads/master b/gps_goal/.gitbak/logs/refs/heads/master new file mode 100644 index 0000000..033b1b5 --- /dev/null +++ b/gps_goal/.gitbak/logs/refs/heads/master @@ -0,0 +1 @@ +0000000000000000000000000000000000000000 6cfdb24513fd603e803a809faac404cd52979c7c easyblank 1630310531 +0000 clone: from https://github.com/danielsnider/gps_goal diff --git a/gps_goal/.gitbak/logs/refs/remotes/origin/HEAD b/gps_goal/.gitbak/logs/refs/remotes/origin/HEAD new file mode 100644 index 0000000..033b1b5 --- /dev/null +++ b/gps_goal/.gitbak/logs/refs/remotes/origin/HEAD @@ -0,0 +1 @@ +0000000000000000000000000000000000000000 6cfdb24513fd603e803a809faac404cd52979c7c easyblank 1630310531 +0000 clone: from https://github.com/danielsnider/gps_goal diff --git a/gps_goal/.gitbak/objects/12/1aaf41dba19a0601cf5ddb3907f052768c86ca b/gps_goal/.gitbak/objects/12/1aaf41dba19a0601cf5ddb3907f052768c86ca new file mode 100644 index 0000000..7ec7f8c Binary files /dev/null and b/gps_goal/.gitbak/objects/12/1aaf41dba19a0601cf5ddb3907f052768c86ca differ diff --git a/gps_goal/.gitbak/objects/1c/f3cd0fef1415066997e9b34d1596f8bddaf759 b/gps_goal/.gitbak/objects/1c/f3cd0fef1415066997e9b34d1596f8bddaf759 new file mode 100644 index 0000000..d057c96 Binary files /dev/null and b/gps_goal/.gitbak/objects/1c/f3cd0fef1415066997e9b34d1596f8bddaf759 differ diff --git a/gps_goal/.gitbak/objects/1d/0fff5da3cb015fda2c32082bccf772725abe0a b/gps_goal/.gitbak/objects/1d/0fff5da3cb015fda2c32082bccf772725abe0a new file mode 100644 index 0000000..f482b19 Binary files /dev/null and b/gps_goal/.gitbak/objects/1d/0fff5da3cb015fda2c32082bccf772725abe0a differ diff --git a/gps_goal/.gitbak/objects/34/b0bc02fd8e56d2857baf004e2422eae5cf3c3a b/gps_goal/.gitbak/objects/34/b0bc02fd8e56d2857baf004e2422eae5cf3c3a new file mode 100644 index 0000000..c77bab4 Binary files /dev/null and b/gps_goal/.gitbak/objects/34/b0bc02fd8e56d2857baf004e2422eae5cf3c3a differ diff --git a/gps_goal/.gitbak/objects/35/cae16cb3827a0163cc38a132a9ae54c1f0f8f8 b/gps_goal/.gitbak/objects/35/cae16cb3827a0163cc38a132a9ae54c1f0f8f8 new file mode 100644 index 0000000..4d7f9ed Binary files /dev/null and b/gps_goal/.gitbak/objects/35/cae16cb3827a0163cc38a132a9ae54c1f0f8f8 differ diff --git a/gps_goal/.gitbak/objects/3a/e542c6da1e19b04fc77d672bb379e0d64b9f1a b/gps_goal/.gitbak/objects/3a/e542c6da1e19b04fc77d672bb379e0d64b9f1a new file mode 100644 index 0000000..9e8d743 Binary files /dev/null and b/gps_goal/.gitbak/objects/3a/e542c6da1e19b04fc77d672bb379e0d64b9f1a differ diff --git a/gps_goal/.gitbak/objects/3b/4981e4ac8d27606c572aa229dfbf3813fc42a8 b/gps_goal/.gitbak/objects/3b/4981e4ac8d27606c572aa229dfbf3813fc42a8 new file mode 100644 index 0000000..5a342ef Binary files /dev/null and b/gps_goal/.gitbak/objects/3b/4981e4ac8d27606c572aa229dfbf3813fc42a8 differ diff --git a/gps_goal/.gitbak/objects/4a/86b8cc62cf1dcfcf35bc4d75f66d4f7c921bf8 b/gps_goal/.gitbak/objects/4a/86b8cc62cf1dcfcf35bc4d75f66d4f7c921bf8 new file mode 100644 index 0000000..d5c8353 Binary files /dev/null and b/gps_goal/.gitbak/objects/4a/86b8cc62cf1dcfcf35bc4d75f66d4f7c921bf8 differ diff --git a/gps_goal/.gitbak/objects/4c/a38ca43f9b443921f27b88453d4750011122e6 b/gps_goal/.gitbak/objects/4c/a38ca43f9b443921f27b88453d4750011122e6 new file mode 100644 index 0000000..1edd1a5 Binary files /dev/null and b/gps_goal/.gitbak/objects/4c/a38ca43f9b443921f27b88453d4750011122e6 differ diff --git a/gps_goal/.gitbak/objects/4d/aab938c5b589be0f825d18eff5e1a1492d4bde b/gps_goal/.gitbak/objects/4d/aab938c5b589be0f825d18eff5e1a1492d4bde new file mode 100644 index 0000000..933333f Binary files /dev/null and b/gps_goal/.gitbak/objects/4d/aab938c5b589be0f825d18eff5e1a1492d4bde differ diff --git a/gps_goal/.gitbak/objects/59/d5cd5347455e109c4d28e658bf929ce53d2c30 b/gps_goal/.gitbak/objects/59/d5cd5347455e109c4d28e658bf929ce53d2c30 new file mode 100644 index 0000000..ae302c1 Binary files /dev/null and b/gps_goal/.gitbak/objects/59/d5cd5347455e109c4d28e658bf929ce53d2c30 differ diff --git a/gps_goal/.gitbak/objects/5c/3d63adfac9942b7886744bc622a38d3480c6e1 b/gps_goal/.gitbak/objects/5c/3d63adfac9942b7886744bc622a38d3480c6e1 new file mode 100644 index 0000000..9c582e0 --- /dev/null +++ b/gps_goal/.gitbak/objects/5c/3d63adfac9942b7886744bc622a38d3480c6e1 @@ -0,0 +1,2 @@ +xRAj0YPllh)[.d,}eىEj*x~*|gM^I=-i&UKƁ +oC=0VR`ݚ[0b_U|=YZ( 9WBj9eAlǔqMkLb[myC){5ntraTb܊MGn_[ Ztpn0hYz }.~M[FA5`=~+939I}eFꚒRtRo|NAʧY?EzXO/P"˲@fH!>~0 N( \ No newline at end of file diff --git a/gps_goal/.gitbak/objects/61/a84933bec75cfbca81b4d1b02b30b3897e1bad b/gps_goal/.gitbak/objects/61/a84933bec75cfbca81b4d1b02b30b3897e1bad new file mode 100644 index 0000000..f09d722 Binary files /dev/null and b/gps_goal/.gitbak/objects/61/a84933bec75cfbca81b4d1b02b30b3897e1bad differ diff --git a/gps_goal/.gitbak/objects/68/a49daad8ff7e35068f2b7a97d643aab440eaec b/gps_goal/.gitbak/objects/68/a49daad8ff7e35068f2b7a97d643aab440eaec new file mode 100644 index 0000000..acc13a9 Binary files /dev/null and b/gps_goal/.gitbak/objects/68/a49daad8ff7e35068f2b7a97d643aab440eaec differ diff --git a/gps_goal/.gitbak/objects/6b/1607ce8a4d3eee2cff9db15581d56f394e06d8 b/gps_goal/.gitbak/objects/6b/1607ce8a4d3eee2cff9db15581d56f394e06d8 new file mode 100644 index 0000000..d391f78 Binary files /dev/null and b/gps_goal/.gitbak/objects/6b/1607ce8a4d3eee2cff9db15581d56f394e06d8 differ diff --git a/gps_goal/.gitbak/objects/6c/fdb24513fd603e803a809faac404cd52979c7c b/gps_goal/.gitbak/objects/6c/fdb24513fd603e803a809faac404cd52979c7c new file mode 100644 index 0000000..dbf5599 Binary files /dev/null and b/gps_goal/.gitbak/objects/6c/fdb24513fd603e803a809faac404cd52979c7c differ diff --git a/gps_goal/.gitbak/objects/71/108766b884fac031ec7bfce16c470231f115ff b/gps_goal/.gitbak/objects/71/108766b884fac031ec7bfce16c470231f115ff new file mode 100644 index 0000000..b484601 --- /dev/null +++ b/gps_goal/.gitbak/objects/71/108766b884fac031ec7bfce16c470231f115ff @@ -0,0 +1 @@ +x+)JMU0`040031Q,+dx6M9{wk+qIOD2sSS7z4la7% \ No newline at end of file diff --git a/gps_goal/.gitbak/objects/74/d75d544affc53486b83e44f945e1150b7a059d b/gps_goal/.gitbak/objects/74/d75d544affc53486b83e44f945e1150b7a059d new file mode 100644 index 0000000..6185148 Binary files /dev/null and b/gps_goal/.gitbak/objects/74/d75d544affc53486b83e44f945e1150b7a059d differ diff --git a/gps_goal/.gitbak/objects/79/51869c3021a7f7627613f111d81c6ea6069598 b/gps_goal/.gitbak/objects/79/51869c3021a7f7627613f111d81c6ea6069598 new file mode 100644 index 0000000..3baf986 Binary files /dev/null and b/gps_goal/.gitbak/objects/79/51869c3021a7f7627613f111d81c6ea6069598 differ diff --git a/gps_goal/.gitbak/objects/80/121781f6ed174bd7bd32aba5b197ea80e7f38b b/gps_goal/.gitbak/objects/80/121781f6ed174bd7bd32aba5b197ea80e7f38b new file mode 100644 index 0000000..5ed6279 Binary files /dev/null and b/gps_goal/.gitbak/objects/80/121781f6ed174bd7bd32aba5b197ea80e7f38b differ diff --git a/gps_goal/.gitbak/objects/94/b88f81ca92f4347a520a412364c8084b65180f b/gps_goal/.gitbak/objects/94/b88f81ca92f4347a520a412364c8084b65180f new file mode 100644 index 0000000..4c1680b --- /dev/null +++ b/gps_goal/.gitbak/objects/94/b88f81ca92f4347a520a412364c8084b65180f @@ -0,0 +1 @@ +xmQ]k0ݳ" TTeVKF ^mXLB"ENXvϽ-/OOPE]_}FkQKy.~ol7' F! B/eR KLpX"ZF6R?U+eH%)vERfƲ16x,մ/}kĉh~e(KPe=% Nnu{*h[وFhD!?7S7C-j +K(Z[^7v+5G^i hSk8xnV.cV@ets ++a)`7۵CenbFضv/ʃ ~fӀ$hiDxIt'1 ׳9:/-/GTL]Ŝe^ 39-^|An`co͎? n (ǣ*ߠ,qfS4C?L)A~69!4 \ No newline at end of file diff --git a/gps_goal/.gitbak/objects/9c/5732753ef3fc9b3d2145b45d132ecfb074577c b/gps_goal/.gitbak/objects/9c/5732753ef3fc9b3d2145b45d132ecfb074577c new file mode 100644 index 0000000..8110062 Binary files /dev/null and b/gps_goal/.gitbak/objects/9c/5732753ef3fc9b3d2145b45d132ecfb074577c differ diff --git a/gps_goal/.gitbak/objects/9e/267008cdc781977367f6d4f9ae65b35896912f b/gps_goal/.gitbak/objects/9e/267008cdc781977367f6d4f9ae65b35896912f new file mode 100644 index 0000000..bf6e8ab Binary files /dev/null and b/gps_goal/.gitbak/objects/9e/267008cdc781977367f6d4f9ae65b35896912f differ diff --git a/gps_goal/.gitbak/objects/a6/48142c12fc09c53af08b11189ba0ca37d2c016 b/gps_goal/.gitbak/objects/a6/48142c12fc09c53af08b11189ba0ca37d2c016 new file mode 100644 index 0000000..51283d9 Binary files /dev/null and b/gps_goal/.gitbak/objects/a6/48142c12fc09c53af08b11189ba0ca37d2c016 differ diff --git a/gps_goal/.gitbak/objects/a6/af7617870a5ae8e9cfd1a45533b4d7a95d3c21 b/gps_goal/.gitbak/objects/a6/af7617870a5ae8e9cfd1a45533b4d7a95d3c21 new file mode 100644 index 0000000..59ea83e Binary files /dev/null and b/gps_goal/.gitbak/objects/a6/af7617870a5ae8e9cfd1a45533b4d7a95d3c21 differ diff --git a/gps_goal/.gitbak/objects/ab/2324151b67f0ab747591a9be54b4e05205e785 b/gps_goal/.gitbak/objects/ab/2324151b67f0ab747591a9be54b4e05205e785 new file mode 100644 index 0000000..16a0df3 Binary files /dev/null and b/gps_goal/.gitbak/objects/ab/2324151b67f0ab747591a9be54b4e05205e785 differ diff --git a/gps_goal/.gitbak/objects/b1/babca127e7d9efe488aac24af3af7dd31e9551 b/gps_goal/.gitbak/objects/b1/babca127e7d9efe488aac24af3af7dd31e9551 new file mode 100644 index 0000000..de1acd9 Binary files /dev/null and b/gps_goal/.gitbak/objects/b1/babca127e7d9efe488aac24af3af7dd31e9551 differ diff --git a/gps_goal/.gitbak/objects/bb/a73d4bf2aaf06f1df7f1cf4c28d49d631ce0e2 b/gps_goal/.gitbak/objects/bb/a73d4bf2aaf06f1df7f1cf4c28d49d631ce0e2 new file mode 100644 index 0000000..4a6804a Binary files /dev/null and b/gps_goal/.gitbak/objects/bb/a73d4bf2aaf06f1df7f1cf4c28d49d631ce0e2 differ diff --git a/gps_goal/.gitbak/objects/cd/4e665f9d5da5b74d24b1f6a073623867e74968 b/gps_goal/.gitbak/objects/cd/4e665f9d5da5b74d24b1f6a073623867e74968 new file mode 100644 index 0000000..cbe51a8 Binary files /dev/null and b/gps_goal/.gitbak/objects/cd/4e665f9d5da5b74d24b1f6a073623867e74968 differ diff --git a/gps_goal/.gitbak/objects/ce/42065ec766eb1dc974a0423cce585697dd1521 b/gps_goal/.gitbak/objects/ce/42065ec766eb1dc974a0423cce585697dd1521 new file mode 100644 index 0000000..f6238f5 Binary files /dev/null and b/gps_goal/.gitbak/objects/ce/42065ec766eb1dc974a0423cce585697dd1521 differ diff --git a/gps_goal/.gitbak/objects/cf/a0c081dc26d3b548b38fda1a4bc9d52980956e b/gps_goal/.gitbak/objects/cf/a0c081dc26d3b548b38fda1a4bc9d52980956e new file mode 100644 index 0000000..b3df684 --- /dev/null +++ b/gps_goal/.gitbak/objects/cf/a0c081dc26d3b548b38fda1a4bc9d52980956e @@ -0,0 +1,2 @@ +x=; +0+jc<#?Zt30nU]eEn-ŌrrEhJxf+5ax=76ojV# \ No newline at end of file diff --git a/gps_goal/.gitbak/objects/d0/371d45b8a97b9ecad47dc53b0454ec076eb9dc b/gps_goal/.gitbak/objects/d0/371d45b8a97b9ecad47dc53b0454ec076eb9dc new file mode 100644 index 0000000..c2ee9f5 --- /dev/null +++ b/gps_goal/.gitbak/objects/d0/371d45b8a97b9ecad47dc53b0454ec076eb9dc @@ -0,0 +1,4 @@ +xK +1]}%a# b7u +^4G΀YZǹ):/9e]I ٚ2- +YXWNwJyv\Bޞ-Pޮ ^Zm -粎_ viRk/;+?^ \ No newline at end of file diff --git a/gps_goal/.gitbak/objects/d3/257e31d642f7f912c38b7d6729ef7a4429daca b/gps_goal/.gitbak/objects/d3/257e31d642f7f912c38b7d6729ef7a4429daca new file mode 100644 index 0000000..751a1ff Binary files /dev/null and b/gps_goal/.gitbak/objects/d3/257e31d642f7f912c38b7d6729ef7a4429daca differ diff --git a/gps_goal/.gitbak/objects/d7/b40be7553b684ba211f53ed51b5bde917b59f1 b/gps_goal/.gitbak/objects/d7/b40be7553b684ba211f53ed51b5bde917b59f1 new file mode 100644 index 0000000..e149329 Binary files /dev/null and b/gps_goal/.gitbak/objects/d7/b40be7553b684ba211f53ed51b5bde917b59f1 differ diff --git a/gps_goal/.gitbak/objects/d8/0d3834f49a777b83c034a7f649f36be9a96f67 b/gps_goal/.gitbak/objects/d8/0d3834f49a777b83c034a7f649f36be9a96f67 new file mode 100644 index 0000000..e25ec7a Binary files /dev/null and b/gps_goal/.gitbak/objects/d8/0d3834f49a777b83c034a7f649f36be9a96f67 differ diff --git a/gps_goal/.gitbak/objects/e6/9de29bb2d1d6434b8b29ae775ad8c2e48c5391 b/gps_goal/.gitbak/objects/e6/9de29bb2d1d6434b8b29ae775ad8c2e48c5391 new file mode 100644 index 0000000..7112238 Binary files /dev/null and b/gps_goal/.gitbak/objects/e6/9de29bb2d1d6434b8b29ae775ad8c2e48c5391 differ diff --git a/gps_goal/.gitbak/objects/eb/c439705c0c7bfc2e8522dda58b30bd51c2fab5 b/gps_goal/.gitbak/objects/eb/c439705c0c7bfc2e8522dda58b30bd51c2fab5 new file mode 100644 index 0000000..13a72fc Binary files /dev/null and b/gps_goal/.gitbak/objects/eb/c439705c0c7bfc2e8522dda58b30bd51c2fab5 differ diff --git a/gps_goal/.gitbak/objects/ed/17e8094beae72e6e98c1bcc1e04a7a8ef5e080 b/gps_goal/.gitbak/objects/ed/17e8094beae72e6e98c1bcc1e04a7a8ef5e080 new file mode 100644 index 0000000..a549c68 Binary files /dev/null and b/gps_goal/.gitbak/objects/ed/17e8094beae72e6e98c1bcc1e04a7a8ef5e080 differ diff --git a/gps_goal/.gitbak/objects/ef/476929c7b2823083678e813b0ae389052e6aff b/gps_goal/.gitbak/objects/ef/476929c7b2823083678e813b0ae389052e6aff new file mode 100644 index 0000000..11e063e Binary files /dev/null and b/gps_goal/.gitbak/objects/ef/476929c7b2823083678e813b0ae389052e6aff differ diff --git a/gps_goal/.gitbak/objects/f2/2dc50f9cf18e45dc8aa9a4fe7afe55695a0a59 b/gps_goal/.gitbak/objects/f2/2dc50f9cf18e45dc8aa9a4fe7afe55695a0a59 new file mode 100644 index 0000000..9300f00 Binary files /dev/null and b/gps_goal/.gitbak/objects/f2/2dc50f9cf18e45dc8aa9a4fe7afe55695a0a59 differ diff --git a/gps_goal/.gitbak/objects/f3/6dbfca991209c81c7abe3630e97829a0600573 b/gps_goal/.gitbak/objects/f3/6dbfca991209c81c7abe3630e97829a0600573 new file mode 100644 index 0000000..90bf569 Binary files /dev/null and b/gps_goal/.gitbak/objects/f3/6dbfca991209c81c7abe3630e97829a0600573 differ diff --git a/gps_goal/.gitbak/objects/f3/80265f6b5c7b5a239151346056998a1f341fb1 b/gps_goal/.gitbak/objects/f3/80265f6b5c7b5a239151346056998a1f341fb1 new file mode 100644 index 0000000..09652ff Binary files /dev/null and b/gps_goal/.gitbak/objects/f3/80265f6b5c7b5a239151346056998a1f341fb1 differ diff --git a/gps_goal/.gitbak/objects/fa/b13542e123df24bc828252b23ee86cdeada3bb b/gps_goal/.gitbak/objects/fa/b13542e123df24bc828252b23ee86cdeada3bb new file mode 100644 index 0000000..e086afd Binary files /dev/null and b/gps_goal/.gitbak/objects/fa/b13542e123df24bc828252b23ee86cdeada3bb differ diff --git a/gps_goal/.gitbak/objects/fe/6571ba94b9fd7248b5e9eb79c57969aaf072ae b/gps_goal/.gitbak/objects/fe/6571ba94b9fd7248b5e9eb79c57969aaf072ae new file mode 100644 index 0000000..07f8ef7 Binary files /dev/null and b/gps_goal/.gitbak/objects/fe/6571ba94b9fd7248b5e9eb79c57969aaf072ae differ diff --git a/gps_goal/.gitbak/packed-refs b/gps_goal/.gitbak/packed-refs new file mode 100644 index 0000000..3e5f718 --- /dev/null +++ b/gps_goal/.gitbak/packed-refs @@ -0,0 +1,3 @@ +# pack-refs with: peeled fully-peeled sorted +6cfdb24513fd603e803a809faac404cd52979c7c refs/remotes/origin/master +4ca38ca43f9b443921f27b88453d4750011122e6 refs/tags/0.1.0 diff --git a/gps_goal/.gitbak/refs/heads/master b/gps_goal/.gitbak/refs/heads/master new file mode 100644 index 0000000..a3b1392 --- /dev/null +++ b/gps_goal/.gitbak/refs/heads/master @@ -0,0 +1 @@ +6cfdb24513fd603e803a809faac404cd52979c7c diff --git a/gps_goal/.gitbak/refs/remotes/origin/HEAD b/gps_goal/.gitbak/refs/remotes/origin/HEAD new file mode 100644 index 0000000..6efe28f --- /dev/null +++ b/gps_goal/.gitbak/refs/remotes/origin/HEAD @@ -0,0 +1 @@ +ref: refs/remotes/origin/master diff --git a/gps_goal/.gitignore b/gps_goal/.gitignore new file mode 100644 index 0000000..ebc4397 --- /dev/null +++ b/gps_goal/.gitignore @@ -0,0 +1,5 @@ +*.pyc +*.swp +*.swo +build_isolated/ +devel_isolated/ diff --git a/gps_goal/CHANGELOG.rst b/gps_goal/CHANGELOG.rst new file mode 100644 index 0000000..f380265 --- /dev/null +++ b/gps_goal/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gps_goal +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2017-07-20) +------------------ +* First release \ No newline at end of file diff --git a/gps_goal/CMakeLists.txt b/gps_goal/CMakeLists.txt new file mode 100644 index 0000000..9bd41f4 --- /dev/null +++ b/gps_goal/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(gps_goal) + +find_package(catkin REQUIRED COMPONENTS + rospy + tf + actionlib + actionlib_msgs + sensor_msgs + geometry_msgs + move_base_msgs +) + +catkin_python_setup() + +catkin_package( + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS rospy tf actionlib actionlib_msgs sensor_msgs geometry_msgs move_base_msgs +) + +catkin_install_python( + PROGRAMS nodes/gps_goal + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Install launch files +install( + DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/gps_goal/README.md b/gps_goal/README.md new file mode 100644 index 0000000..bba73d4 --- /dev/null +++ b/gps_goal/README.md @@ -0,0 +1,11 @@ +# gps_goal [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__gps_goal__ubuntu_xenial_amd64__binary)](http://build.ros.org/job/Kbin_uX64__gps_goal__ubuntu_xenial_amd64__binary) + +Set a ROS navigation goal using latitude and longitude. + +## Installation + +``` +$ sudo apt-get install ros-kinetic-gps-goal hugin-tools enblend +``` + +**Full documentation on wiki: [http://wiki.ros.org/gps_goal](http://wiki.ros.org/gps_goal)** diff --git a/gps_goal/UNLICENSE b/gps_goal/UNLICENSE new file mode 100644 index 0000000..68a49da --- /dev/null +++ b/gps_goal/UNLICENSE @@ -0,0 +1,24 @@ +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +OTHER DEALINGS IN THE SOFTWARE. + +For more information, please refer to diff --git a/gps_goal/launch/gps_goal.launch b/gps_goal/launch/gps_goal.launch new file mode 100644 index 0000000..94b88f8 --- /dev/null +++ b/gps_goal/launch/gps_goal.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/gps_goal/launch/initialize_origin.launch b/gps_goal/launch/initialize_origin.launch new file mode 100644 index 0000000..8012178 --- /dev/null +++ b/gps_goal/launch/initialize_origin.launch @@ -0,0 +1,17 @@ + + + + + + + + + + [{ name: MDRS, + latitude: 38.40630, + longitude: -110.79201, + altitude: 0.0, + heading: 0.0}] + + + diff --git a/gps_goal/nodes/gps_goal b/gps_goal/nodes/gps_goal new file mode 100755 index 0000000..35cae16 --- /dev/null +++ b/gps_goal/nodes/gps_goal @@ -0,0 +1,6 @@ +#!/usr/bin/env python + +from gps_goal import gps_goal + +if __name__ == '__main__': + gps_goal.ros_main() diff --git a/gps_goal/package.xml b/gps_goal/package.xml new file mode 100644 index 0000000..fe6571b --- /dev/null +++ b/gps_goal/package.xml @@ -0,0 +1,31 @@ + + gps_goal + 0.1.0 + + Set a ROS navigation goal using latitude and longitude. + + Daniel Snider + Daniel Snider + Unlicense + http://wiki.ros.org/gps_goal + https://github.com/danielsnider/gps_goal + + catkin + rospy + tf + actionlib + sensor_msgs + actionlib_msgs + geometry_msgs + move_base_msgs + rospy + tf + actionlib + sensor_msgs + actionlib_msgs + geometry_msgs + move_base_msgs + python-geographiclib + click + + diff --git a/gps_goal/setup.py b/gps_goal/setup.py new file mode 100644 index 0000000..1d0fff5 --- /dev/null +++ b/gps_goal/setup.py @@ -0,0 +1,11 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['gps_goal'], + package_dir={'': 'src'}) + +setup(**setup_args) diff --git a/gps_goal/src/gps_goal/__init__.py b/gps_goal/src/gps_goal/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gps_goal/src/gps_goal/gps_goal.py b/gps_goal/src/gps_goal/gps_goal.py new file mode 100755 index 0000000..11e58da --- /dev/null +++ b/gps_goal/src/gps_goal/gps_goal.py @@ -0,0 +1,183 @@ +#!/usr/bin/python +import rospy +import click +import math +import actionlib +import tf +import time +from geographiclib.geodesic import Geodesic +from actionlib_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped +from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal +from sensor_msgs.msg import NavSatFix +from wit_node.msg import ImuGpsRaw +import paho.mqtt.publish as publish +global rad_offset +def DMS_to_decimal_format(lat,long): + # Check for degrees, minutes, seconds format and convert to decimal + if ',' in lat: + degrees, minutes, seconds = lat.split(',') + degrees, minutes, seconds = float(degrees), float(minutes), float(seconds) + if lat[0] == '-': # check for negative sign + minutes = -minutes + seconds = -seconds + lat = degrees + minutes/60 + seconds/3600 + if ',' in long: + degrees, minutes, seconds = long.split(',') + degrees, minutes, seconds = float(degrees), float(minutes), float(seconds) + if long[0] == '-': # check for negative sign + minutes = -minutes + seconds = -seconds + long = degrees + minutes/60 + seconds/3600 + + lat = float(lat) + long = float(long) + rospy.loginfo('Given GPS goal: lat %s, long %s.' % (lat, long)) + return lat, long + +def get_origin_lat_long(): + + # Get the lat long coordinates of our map frame's origin which must be publshed on topic /local_xy_origin. We use this to calculate our goal within the map frame. + rospy.loginfo("Waiting for a message to initialize the origin GPS location...") + origin_pose = rospy.wait_for_message('local_xy_origin', PoseStamped) + origin_lat = origin_pose.pose.position.y + origin_long = origin_pose.pose.position.x + rospy.loginfo('Received origin: lat %s, long %s.' % (origin_lat, origin_long)) + return origin_lat, origin_long + +def calc_goal(origin_lat, origin_long, goal_lat, goal_long, origin): + # Calculate distance and azimuth between GPS points + rospy.loginfo('Given GPS goal: lat %s, long %s.' % (goal_lat, goal_long)) + geod = Geodesic.WGS84 # define the WGS84 ellipsoid + g = geod.Inverse(origin_lat, origin_long, goal_lat, goal_long) # Compute several geodesic calculations between two GPS points + hypotenuse = distance = g['s12'] # access distance + rospy.loginfo("The distance from the origin to the goal is {:.3f} m.".format(distance)) + azimuth = g['azi1'] + rospy.loginfo("The azimuth from the origin to the goal is {:.3f} degrees.".format(azimuth)) + + # Convert polar (distance and azimuth) to x,y translation in meters (needed for ROS) by finding side lenghs of a right-angle triangle + # Convert azimuth to radians + azimuth = (math.radians(azimuth) - origin)*(-1) + print(azimuth) + x = adjacent = math.cos(azimuth) * hypotenuse + y = opposite = math.sin(azimuth) * hypotenuse + rospy.loginfo("The translation from the origin to the goal is (x,y) {:.3f}, {:.3f} m.".format(x, y)) + + return x, y + +class GpsGoal(): + def __init__(self): + rospy.init_node('gps_goal') + + rospy.loginfo("Connecting to move_base...") + self.move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction) + self.move_base.wait_for_server() + rospy.loginfo("Connected.") + rospy.Subscriber('fix', NavSatFix, self.current_coord_callback) + rospy.Subscriber('gps_goal_pose', PoseStamped, self.gps_goal_pose_callback) + rospy.Subscriber('gps_goal_fix', NavSatFix, self.gps_goal_fix_callback) + rospy.Subscriber('/wit/raw_data', ImuGpsRaw, self.radian_callback) + # Get the lat long coordinates of our map frame's origin which must be publshed on topic /local_xy_origin. We use this to calculate our goal within the map frame. + time.sleep(3.0) + self.origin = rad_offset + self.origin_lat, self.origin_long = get_origin_lat_long() + + def current_coord_callback(self, data): + global current_latitude, current_longitude + current_latitude = data.latitude + current_longitude = data.longitude + + def do_gps_goal(self, goal_lat, goal_long, z=0, yaw=0, roll=0, pitch=0): + # Calculate goal x and y in the frame_id given the frame's origin GPS and a goal GPS location + x, y = calc_goal(self.origin_lat, self.origin_long, goal_lat, goal_long, self.origin) + # Create move_base goal + self.publish_goal(x=x, y=y, z=z, yaw=yaw, roll=roll, pitch=pitch) + + def gps_goal_pose_callback(self, data): + lat = data.pose.position.y + long = data.pose.position.x + z = data.pose.position.z + euler = tf.transformations.euler_from_quaternion(data.pose.orientation) + roll = euler[0] + pitch = euler[1] + yaw = euler[2] + self.do_gps_goal(lat, long, z=z, yaw=yaw, roll=roll, pitch=pitch) + + def radian_callback(self, data): + global rad_offset + rad_offset = data.rpy[2] + rad_offset = (0.5*math.pi+rad_offset)*(-1) + # if (rad_offset > 0): + # rad_offset = math.pi - rad_offset + # # # rad_offset = (-1)* rad_offset + # else: + # rad_offset = (-1) * math.pi - rad_offset + +# rospy.loginfo('radian,', x) + + def gps_goal_fix_callback(self, data): + print(data.latitude, data.longitude) + self.do_gps_goal(data.latitude, data.longitude) + + def publish_goal(self, x=0, y=0, z=0, yaw=0, roll=0, pitch=0): + # Create move_base goal + goal = MoveBaseGoal() + goal.target_pose.header.frame_id = rospy.get_param('~frame_id','map') + goal.target_pose.pose.position.x = x + goal.target_pose.pose.position.y = y + goal.target_pose.pose.position.z = z + quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + goal.target_pose.pose.orientation.x = quaternion[0] + goal.target_pose.pose.orientation.y = quaternion[1] + goal.target_pose.pose.orientation.z = quaternion[2] + goal.target_pose.pose.orientation.w = quaternion[3] + rospy.loginfo('Executing move_base goal to position (x,y) %s, %s, with %s degrees yaw.' % + (x, y, yaw)) + rospy.loginfo("To cancel the goal: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'") + + # Send goal + self.move_base.send_goal(goal) + rospy.loginfo('Inital goal status: %s' % GoalStatus.to_string(self.move_base.get_state())) + status = self.move_base.get_goal_status_text() + if status: + rospy.loginfo(status) + + # Wait for goal result + self.move_base.wait_for_result() + rospy.loginfo('Final goal status: %s' % GoalStatus.to_string(self.move_base.get_state())) + print(GoalStatus.to_string(self.move_base.get_state())) + if GoalStatus.to_string(self.move_base.get_state())=="SUCCEEDED": + print("MQTT published") + publish.single(topic="/goal", payload="1", hostname="163.180.117.43") + status = self.move_base.get_goal_status_text() + if status: + rospy.loginfo(status) + +@click.command() +@click.option('--lat', prompt='Latitude', help='Latitude') +@click.option('--long', prompt='Longitude', help='Longitude') +@click.option('--roll', '-r', help='Set target roll for goal', default=0.0) +@click.option('--pitch', '-p', help='Set target pitch for goal', default=0.0) +@click.option('--yaw', '-y', help='Set target yaw for goal', default=0.0) +def cli_main(lat, long, roll, pitch, yaw): + """Send goal to move_base given latitude and longitude + + \b + Two usage formats: + gps_goal.py --lat 43.658 --long -79.379 # decimal format + gps_goal.py --lat 43,39,31 --long -79,22,45 # DMS format + """ + gpsGoal = GpsGoal(); + + # Check for degrees, minutes, seconds format and convert to decimal + lat, long = DMS_to_decimal_format(lat, long) + gpsGoal.do_gps_goal(lat, long, roll=roll, pitch=pitch, yaw=yaw) + + +def ros_main(): + gpsGoal = GpsGoal(); + rospy.spin() + +if __name__ == '__main__': + cli_main() +