From c259a1a6f72caa302f91096b0bc2d79c7abd71a4 Mon Sep 17 00:00:00 2001 From: Maximilian Ehrhardt Date: Mon, 10 Aug 2020 17:05:47 +0200 Subject: [PATCH 01/20] Add shell scripts for different docker run commands --- docker/run_autostart.sh | 15 +++++++++++++++ docker/run_config.sh | 15 +++++++++++++++ docker/run_ros.sh | 15 +++++++++++++++ 3 files changed, 45 insertions(+) create mode 100644 docker/run_autostart.sh create mode 100644 docker/run_config.sh create mode 100644 docker/run_ros.sh diff --git a/docker/run_autostart.sh b/docker/run_autostart.sh new file mode 100644 index 0000000..c081c32 --- /dev/null +++ b/docker/run_autostart.sh @@ -0,0 +1,15 @@ +#!/bin/bash +# Runs a docker container based on the exomy image that +# autostarts every time ExoMy is started + +docker run \ + -it \ + -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ + -p 8000:8000 \ + -p 8080:8080 \ + -p 9090:9090 \ + --restart always \ + --privileged \ + --name exomy \ + exomy \ + autostart \ No newline at end of file diff --git a/docker/run_config.sh b/docker/run_config.sh new file mode 100644 index 0000000..968085e --- /dev/null +++ b/docker/run_config.sh @@ -0,0 +1,15 @@ +#!/bin/bash +# Runs a docker container based on the exomy image that +# starts the motor configuration script + +docker run \ + -it \ + -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ + --rm \ + -p 8000:8000 \ + -p 8080:8080 \ + -p 9090:9090 \ + --privileged \ + --name exomy_motor_config\ + exomy \ + config \ No newline at end of file diff --git a/docker/run_ros.sh b/docker/run_ros.sh new file mode 100644 index 0000000..6f6c1a1 --- /dev/null +++ b/docker/run_ros.sh @@ -0,0 +1,15 @@ +#!/bin/bash +# Runs a docker container based on the exomy image that +# allows to do some changes in the code + +docker run \ + -it \ + -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ + -p 8000:8000 \ + -p 8080:8080 \ + -p 9090:9090 \ + --restart always \ + --privileged \ + --name exomy_ros \ + exomy \ + ros \ No newline at end of file From 8ec37f67f952be239474e8ce5425cdad458ba2bb Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Thu, 13 Aug 2020 11:26:37 +0200 Subject: [PATCH 02/20] Fix for entrypoint if started w/ docker run. --- docker/entrypoint.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh index 5bc84e3..f283289 100755 --- a/docker/entrypoint.sh +++ b/docker/entrypoint.sh @@ -1,9 +1,9 @@ #!/bin/bash -if [ $1 == "config" ] +if [[ $1 == "config" ]] then cd /root/exomy_ws/src/exomy/scripts bash -elif [ $1 == "autostart" ] +elif [[ $1 == "autostart" ]] then source /opt/ros/melodic/setup.bash cd /root/exomy_ws @@ -14,7 +14,7 @@ then roslaunch exomy exomy.launch bash -elif [ $1 == "ros" ] +elif [[ $1 == "ros" ]] then cd /root/exomy_ws source /opt/ros/melodic/setup.bash From ec5101bbea5b02bf9622eaab3371b9da6ca6c0de Mon Sep 17 00:00:00 2001 From: Maximilian Ehrhardt Date: Thu, 13 Aug 2020 14:07:17 +0200 Subject: [PATCH 03/20] Change run command from ros to devel --- docker/entrypoint.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh index 5bc84e3..36aeb98 100755 --- a/docker/entrypoint.sh +++ b/docker/entrypoint.sh @@ -14,7 +14,7 @@ then roslaunch exomy exomy.launch bash -elif [ $1 == "ros" ] +elif [ $1 == "devel" ] then cd /root/exomy_ws source /opt/ros/melodic/setup.bash From a708e6bf39bdda1dc9036006fe3f22a3a76b1031 Mon Sep 17 00:00:00 2001 From: Maximilian Ehrhardt Date: Thu, 13 Aug 2020 14:08:00 +0200 Subject: [PATCH 04/20] Combine run scripts to one --- docker/run_autostart.sh | 15 --------- docker/run_config.sh | 15 --------- docker/run_exomy.sh | 74 +++++++++++++++++++++++++++++++++++++++++ docker/run_ros.sh | 15 --------- 4 files changed, 74 insertions(+), 45 deletions(-) delete mode 100644 docker/run_autostart.sh delete mode 100644 docker/run_config.sh create mode 100644 docker/run_exomy.sh delete mode 100644 docker/run_ros.sh diff --git a/docker/run_autostart.sh b/docker/run_autostart.sh deleted file mode 100644 index c081c32..0000000 --- a/docker/run_autostart.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/bin/bash -# Runs a docker container based on the exomy image that -# autostarts every time ExoMy is started - -docker run \ - -it \ - -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ - -p 8000:8000 \ - -p 8080:8080 \ - -p 9090:9090 \ - --restart always \ - --privileged \ - --name exomy \ - exomy \ - autostart \ No newline at end of file diff --git a/docker/run_config.sh b/docker/run_config.sh deleted file mode 100644 index 968085e..0000000 --- a/docker/run_config.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/bin/bash -# Runs a docker container based on the exomy image that -# starts the motor configuration script - -docker run \ - -it \ - -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ - --rm \ - -p 8000:8000 \ - -p 8080:8080 \ - -p 9090:9090 \ - --privileged \ - --name exomy_motor_config\ - exomy \ - config \ No newline at end of file diff --git a/docker/run_exomy.sh b/docker/run_exomy.sh new file mode 100644 index 0000000..65cae55 --- /dev/null +++ b/docker/run_exomy.sh @@ -0,0 +1,74 @@ +#!/bin/bash +# run_exomy- A script to run containers for dedicated functions of exomy + +help_text="Usage: "$0" [MODE] [OPTIONS] + A script to run ExoMy in different configurations + Options: + -a, --autostart Toggles autostart mode on or off + -c, --config Runs the motor configuration of ExoMy + -d, --devel Runs the development mode to change some code of ExoMy + -h, --help Shows this text +" + +### Main +# Initialize parameters +container_name="exomy" +image_name="exomy" + +# Process parameters +if [ "$1" != "" ]; then + case $1 in + -a | --autostart) + container_name="${container_name}_autostart" + start_command="autostart" + options="--restart always" + + ;; + -c | --config) + container_name="${container_name}_config" + start_command="config" + options="--rm" + ;; + -d | --devel) + container_name="${container_name}_devel" + start_command="devel" + options="--restart always" + ;; + -h | --help ) echo "$help_text" + exit + ;; + * ) echo "ERROR: Not a valid mode!" + echo "$help_text" + exit 1 + esac +else + echo "ERROR: You need to specify a mode!" + echo "$help_text" + exit +fi + +# Check for image and build from Dockerfile if not accessible +result=$( docker images -q $image_name ) + +if [ ! -n "$result" ]; then + directory=$( dirname "$0" ) + + echo "Docker image not built yet. Build from Dockerfile. " + echo ${directory} + docker build -t $image_name $directory +else + echo "Use existing docker image: "$image_name"" +fi + +# Run docker container +docker run \ + -it \ + -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ + -p 8000:8000 \ + -p 8080:8080 \ + -p 9090:9090 \ + --privileged \ + ${options} \ + --name "${container_name}" \ + "${image_name}" \ + "${start_command}" \ No newline at end of file diff --git a/docker/run_ros.sh b/docker/run_ros.sh deleted file mode 100644 index 6f6c1a1..0000000 --- a/docker/run_ros.sh +++ /dev/null @@ -1,15 +0,0 @@ -#!/bin/bash -# Runs a docker container based on the exomy image that -# allows to do some changes in the code - -docker run \ - -it \ - -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ - -p 8000:8000 \ - -p 8080:8080 \ - -p 9090:9090 \ - --restart always \ - --privileged \ - --name exomy_ros \ - exomy \ - ros \ No newline at end of file From 16ef6bcb9d2bdf441ea27a9da95217bae7874173 Mon Sep 17 00:00:00 2001 From: Maximilian Ehrhardt Date: Thu, 13 Aug 2020 14:35:07 +0200 Subject: [PATCH 05/20] Stop exomy containers if they are started already --- docker/run_exomy.sh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docker/run_exomy.sh b/docker/run_exomy.sh index 65cae55..4920a00 100644 --- a/docker/run_exomy.sh +++ b/docker/run_exomy.sh @@ -60,6 +60,9 @@ else echo "Use existing docker image: "$image_name"" fi +# Stop any of the 3 containers if running +docker stop $(docker ps -q --filter ancestor=exomy) + # Run docker container docker run \ -it \ From 4bc4b1cc94c2fd65f4bacfabdbd03dbd5b1ffe43 Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Thu, 13 Aug 2020 16:03:40 +0200 Subject: [PATCH 06/20] Refactor motor actuation code and fix stop latency The motors would only stop with a 1 s delay as pwm was set to 0 instead of its neutral pwm value. The code refactor simplifies the files drastically and reduces copy pasta code Increase watchdog duration to 5s for more responsive behaviour. --- src/motor_node.py | 11 +-- src/motors.py | 174 ++++++++++++++++------------------------------ 2 files changed, 65 insertions(+), 120 deletions(-) diff --git a/src/motor_node.py b/src/motor_node.py index 90fc512..c91eb0e 100755 --- a/src/motor_node.py +++ b/src/motor_node.py @@ -11,15 +11,18 @@ def callback(cmds): motors.setSteering(cmds.motor_angles) motors.setDriving(cmds.motor_speeds) + global watchdog_timer watchdog_timer.shutdown() - watchdog_timer = rospy.Timer(rospy.Duration(1), watchdog, oneshot=True) + # If this timer runs longer than the duration specified, + # then watchdog() is called stopping the driving motors. + watchdog_timer = rospy.Timer(rospy.Duration(5.0), watchdog, oneshot=True) def shutdown(): motors.stopMotors() def watchdog(event): - rospy.loginfo("Watchdog fired") + rospy.loginfo("Watchdog fired. Stopping driving motors.") motors.stopMotors() if __name__ == "__main__": @@ -27,9 +30,9 @@ def watchdog(event): rospy.init_node("motors") rospy.loginfo("Starting the motors node") rospy.on_shutdown(shutdown) - global watchdog_timer - watchdog_timer = rospy.Timer(rospy.Duration(1), watchdog, oneshot=True) + global watchdog_timer + watchdog_timer = rospy.Timer(rospy.Duration(1.0), watchdog, oneshot=True) sub = rospy.Subscriber("/motor_commands",Commands, callback, queue_size=1) diff --git a/src/motors.py b/src/motors.py index c4dd607..55cc6c6 100755 --- a/src/motors.py +++ b/src/motors.py @@ -10,36 +10,49 @@ class Motors(): ''' - Motors class contains all functions to control the steering and driving motors. + Motors class contains all functions to control the steering and driving ''' + # Define wheel names - # rr- -rl - # cr- -cl - # | - # fr- -fl FL, FR, CL, CR, RL, RR = range(0, 6) + + # Motor commands are assuming positiv=driving_forward, negative=driving_backwards. + # The driving direction of the left side has to be inverted for this to apply to all wheels. + wheel_directions = [-1, 1, -1, 1, -1, 1] + + # 1 fl-||-fr 2 + # || + # 3 cl-||-cr 4 + # 5 rl====rr 6 - def __init__(self): + def __init__(self): + + # Dictionary containing the pins of all motors + self.pins = { + 'drive': {}, + 'steer': {} + } + # Set variables for the GPIO motor pins - self.pin_drive_fl = rospy.get_param("pin_drive_fl") - self.pin_steer_fl = rospy.get_param("pin_steer_fl") + self.pins['drive'][self.FL] = rospy.get_param("pin_drive_fl") + self.pins['steer'][self.FL] = rospy.get_param("pin_steer_fl") - self.pin_drive_fr = rospy.get_param("pin_drive_fr") - self.pin_steer_fr = rospy.get_param("pin_steer_fr") + self.pins['drive'][self.FR] = rospy.get_param("pin_drive_fr") + self.pins['steer'][self.FR] = rospy.get_param("pin_steer_fr") - self.pin_drive_cl = rospy.get_param("pin_drive_cl") - self.pin_steer_cl = rospy.get_param("pin_steer_cl") + self.pins['drive'][self.CL] = rospy.get_param("pin_drive_cl") + self.pins['steer'][self.CL] = rospy.get_param("pin_steer_cl") - self.pin_drive_cr = rospy.get_param("pin_drive_cr") - self.pin_steer_cr = rospy.get_param("pin_steer_cr") + self.pins['drive'][self.CR] = rospy.get_param("pin_drive_cr") + self.pins['steer'][self.CR] = rospy.get_param("pin_steer_cr") - self.pin_drive_rl = rospy.get_param("pin_drive_rl") - self.pin_steer_rl = rospy.get_param("pin_steer_rl") + self.pins['drive'][self.RL] = rospy.get_param("pin_drive_rl") + self.pins['steer'][self.RL] = rospy.get_param("pin_steer_rl") - self.pin_drive_rr = rospy.get_param("pin_drive_rr") - self.pin_steer_rr = rospy.get_param("pin_steer_rr") + self.pins['drive'][self.RR] = rospy.get_param("pin_drive_rr") + self.pins['steer'][self.RR] = rospy.get_param("pin_steer_rr") # PWM characteristics self.pwm = Adafruit_PCA9685.PCA9685() @@ -60,126 +73,55 @@ def __init__(self): self.driving_pwm_upper_limit = 500 self.driving_pwm_range = rospy.get_param("drive_range") - # Set the GPIO to software PWM at 'Frequency' Hertz - self.driving_motors = [None] * 6 - self.steering_motors = [None] * 6 - - # Set steering motors to neutral values - self.pwm.set_pwm(self.pin_steer_fl, 0, - self.steering_pwm_neutral[self.FL]) - time.sleep(0.1) - self.pwm.set_pwm(self.pin_steer_fr, 0, - self.steering_pwm_neutral[self.FR]) - time.sleep(0.1) - self.pwm.set_pwm(self.pin_steer_cl, 0, - self.steering_pwm_neutral[self.CL]) - time.sleep(0.1) - self.pwm.set_pwm(self.pin_steer_cr, 0, - self.steering_pwm_neutral[self.CR]) - time.sleep(0.1) - self.pwm.set_pwm(self.pin_steer_rl, 0, - self.steering_pwm_neutral[self.RL]) - time.sleep(0.1) - self.pwm.set_pwm(self.pin_steer_rr, 0, - self.steering_pwm_neutral[self.RR]) - time.sleep(0.1) + # Set steering motors to neutral values (straight) + for wheel_name, motor_pin in self.pins['steer'].items(): + self.pwm.set_pwm(motor_pin, 0, + self.steering_pwm_neutral[wheel_name]) + time.sleep(0.1) self.wiggle() def wiggle(self): time.sleep(0.1) - self.pwm.set_pwm(self.pin_steer_fl, 0, + self.pwm.set_pwm(self.pins['steer'][self.FL], 0, int(self.steering_pwm_neutral[self.FL] + self.steering_pwm_range * 0.3)) time.sleep(0.1) - self.pwm.set_pwm(self.pin_steer_fr, 0, + self.pwm.set_pwm(self.pins['steer'][self.FR], 0, int(self.steering_pwm_neutral[self.FR] + self.steering_pwm_range * 0.3)) time.sleep(0.3) - self.pwm.set_pwm(self.pin_steer_fl, 0, + self.pwm.set_pwm(self.pins['steer'][self.FL], 0, int(self.steering_pwm_neutral[self.FL] - self.steering_pwm_range * 0.3)) time.sleep(0.1) - self.pwm.set_pwm(self.pin_steer_fr, 0, + self.pwm.set_pwm(self.pins['steer'][self.FR], 0, int(self.steering_pwm_neutral[self.FR] - self.steering_pwm_range * 0.3)) time.sleep(0.3) - self.pwm.set_pwm(self.pin_steer_fl, 0, + self.pwm.set_pwm(self.pins['steer'][self.FL], 0, int(self.steering_pwm_neutral[self.FL])) time.sleep(0.1) - self.pwm.set_pwm(self.pin_steer_fr, 0, + self.pwm.set_pwm(self.pins['steer'][self.FR], 0, int(self.steering_pwm_neutral[self.FR])) time.sleep(0.3) def setSteering(self, steering_command): - duty_cycle = int( - self.steering_pwm_neutral[self.FL] + steering_command[self.FL]/90.0 * self.steering_pwm_range) - self.pwm.set_pwm(self.pin_steer_fl, 0, duty_cycle) - - duty_cycle = int( - self.steering_pwm_neutral[self.FR] + steering_command[self.FR]/90.0 * self.steering_pwm_range) - self.pwm.set_pwm(self.pin_steer_fr, 0, duty_cycle) - - duty_cycle = int( - self.steering_pwm_neutral[self.CL] + steering_command[self.CL]/90.0 * self.steering_pwm_range) - self.pwm.set_pwm(self.pin_steer_cl, 0, duty_cycle) - - duty_cycle = int( - self.steering_pwm_neutral[self.CR] + steering_command[self.CR]/90.0 * self.steering_pwm_range) - self.pwm.set_pwm(self.pin_steer_cr, 0, duty_cycle) - - duty_cycle = int( - self.steering_pwm_neutral[self.RL] + steering_command[self.RL]/90.0 * self.steering_pwm_range) - self.pwm.set_pwm(self.pin_steer_rl, 0, duty_cycle) - - duty_cycle = int( - self.steering_pwm_neutral[self.RR] + steering_command[self.RR]/90.0 * self.steering_pwm_range) - self.pwm.set_pwm(self.pin_steer_rr, 0, duty_cycle) + # Loop through pin dictionary. The items key is the wheel_name and the value the pin. + for wheel_name, motor_pin in self.pins['steer'].items(): + duty_cycle = int( + self.steering_pwm_neutral[wheel_name] + steering_command[wheel_name]/90.0 * self.steering_pwm_range) + + self.pwm.set_pwm(motor_pin, 0, duty_cycle) def setDriving(self, driving_command): - if(driving_command[self.FL] == 0.0): - duty_cycle = 0 - else: - duty_cycle = int(self.driving_pwm_neutral - - driving_command[self.FL]/100.0 * self.driving_pwm_range) - self.pwm.set_pwm(self.pin_drive_fl, 0, duty_cycle) - - if(driving_command[self.FR] == 0.0): - duty_cycle = 0 - else: - duty_cycle = int(self.driving_pwm_neutral + - driving_command[self.FR]/100.0 * self.driving_pwm_range) - self.pwm.set_pwm(self.pin_drive_fr, 0, duty_cycle) - - if(driving_command[self.CL] == 0.0): - duty_cycle = 0 - else: - duty_cycle = int(self.driving_pwm_neutral - - driving_command[self.CL]/100.0 * self.driving_pwm_range) - self.pwm.set_pwm(self.pin_drive_cl, 0, duty_cycle) - - if(driving_command[self.CR] == 0.0): - duty_cycle = 0 - else: + # Loop through pin dictionary. The items key is the wheel_name and the value the pin. + for wheel_name, motor_pin in self.pins['drive'].items(): duty_cycle = int(self.driving_pwm_neutral + - driving_command[self.CR]/100.0 * self.driving_pwm_range) - self.pwm.set_pwm(self.pin_drive_cr, 0, duty_cycle) - - if(driving_command[self.RL] == 0.0): - duty_cycle = 0 - else: - duty_cycle = int(self.driving_pwm_neutral - - driving_command[self.RL]/100.0 * self.driving_pwm_range) - self.pwm.set_pwm(self.pin_drive_rl, 0, duty_cycle) - - if(driving_command[self.RR] == 0.0): - duty_cycle = 0 - else: - duty_cycle = int(self.driving_pwm_neutral + - driving_command[self.RR]/100.0 * self.driving_pwm_range) - self.pwm.set_pwm(self.pin_drive_rr, 0, duty_cycle) + driving_command[wheel_name]/100.0 * self.driving_pwm_range * self.wheel_directions[wheel_name]) + + self.pwm.set_pwm(motor_pin, 0, duty_cycle) def stopMotors(self): - self.pwm.set_pwm(self.pin_drive_fl, 0, 0) - self.pwm.set_pwm(self.pin_drive_fr, 0, 0) - self.pwm.set_pwm(self.pin_drive_cl, 0, 0) - self.pwm.set_pwm(self.pin_drive_cr, 0, 0) - self.pwm.set_pwm(self.pin_drive_rl, 0, 0) - self.pwm.set_pwm(self.pin_drive_rr, 0, 0) + # Set driving wheels to neutral position to stop them + duty_cycle = int(self.driving_pwm_neutral) + + for wheel_name, motor_pin in self.pins['drive'].items(): + self.pwm.set_pwm(motor_pin, 0, duty_cycle) From 749bff81a74f4fee02a88082fb38fe908ef4cbcd Mon Sep 17 00:00:00 2001 From: Maximilian Ehrhardt Date: Thu, 13 Aug 2020 16:32:03 +0200 Subject: [PATCH 07/20] Delete containers if already existing --- docker/run_exomy.sh | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/docker/run_exomy.sh b/docker/run_exomy.sh index 4920a00..2483319 100644 --- a/docker/run_exomy.sh +++ b/docker/run_exomy.sh @@ -23,6 +23,10 @@ if [ "$1" != "" ]; then start_command="autostart" options="--restart always" + ;; + -s | --stop_autostart) + docker container stop "${container_name}_autostart" + exit ;; -c | --config) container_name="${container_name}_config" @@ -61,12 +65,15 @@ else fi # Stop any of the 3 containers if running -docker stop $(docker ps -q --filter ancestor=exomy) +RUNNING_CONTAINERS=$( docker container ls -a -q --filter ancestor=exomy ) +if [ -n "$RUNNING_CONTAINERS" ]; then + docker rm -f "$RUNNING_CONTAINERS" +fi +# -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ # Run docker container docker run \ -it \ - -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ -p 8000:8000 \ -p 8080:8080 \ -p 9090:9090 \ From fdb5b615d5b51816deceb1b06e56fb9b4de81bb5 Mon Sep 17 00:00:00 2001 From: Maximilian Ehrhardt Date: Thu, 13 Aug 2020 16:37:26 +0200 Subject: [PATCH 08/20] Fixed comment bug --- docker/run_exomy.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/run_exomy.sh b/docker/run_exomy.sh index 2483319..661bf45 100644 --- a/docker/run_exomy.sh +++ b/docker/run_exomy.sh @@ -70,10 +70,10 @@ if [ -n "$RUNNING_CONTAINERS" ]; then docker rm -f "$RUNNING_CONTAINERS" fi -# -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ # Run docker container docker run \ -it \ + -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ -p 8000:8000 \ -p 8080:8080 \ -p 9090:9090 \ From 96ff47420d5284b6ebefa59b28c438d5cc545818 Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Thu, 13 Aug 2020 17:11:30 +0200 Subject: [PATCH 09/20] Make path absolute instead of current directory dependent --- docker/run_exomy.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docker/run_exomy.sh b/docker/run_exomy.sh index 661bf45..90fe791 100644 --- a/docker/run_exomy.sh +++ b/docker/run_exomy.sh @@ -73,7 +73,7 @@ fi # Run docker container docker run \ -it \ - -v $(pwd)/ExoMy_Software:/root/exomy_ws/src/exomy \ + -v ~/ExoMy_Software:/root/exomy_ws/src/exomy \ -p 8000:8000 \ -p 8080:8080 \ -p 9090:9090 \ @@ -81,4 +81,4 @@ docker run \ ${options} \ --name "${container_name}" \ "${image_name}" \ - "${start_command}" \ No newline at end of file + "${start_command}" From 5aebc39e2315ac8c01b6e2a3480347284b5c24b5 Mon Sep 17 00:00:00 2001 From: maxehrhardt Date: Fri, 14 Aug 2020 14:59:43 +0200 Subject: [PATCH 10/20] Update entrypoint.sh --- docker/entrypoint.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/entrypoint.sh b/docker/entrypoint.sh index f283289..90ddb19 100755 --- a/docker/entrypoint.sh +++ b/docker/entrypoint.sh @@ -14,7 +14,7 @@ then roslaunch exomy exomy.launch bash -elif [[ $1 == "ros" ]] +elif [[ $1 == "devel" ]] then cd /root/exomy_ws source /opt/ros/melodic/setup.bash From f1d39380da0e2b19292e228bf1acfdff4445388f Mon Sep 17 00:00:00 2001 From: Maximilian Ehrhardt Date: Wed, 19 Aug 2020 16:47:03 +0200 Subject: [PATCH 11/20] Remove if conditions for docker image build --- docker/run_exomy.sh | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/docker/run_exomy.sh b/docker/run_exomy.sh index 90fe791..bb97e54 100644 --- a/docker/run_exomy.sh +++ b/docker/run_exomy.sh @@ -51,18 +51,9 @@ else exit fi -# Check for image and build from Dockerfile if not accessible -result=$( docker images -q $image_name ) - -if [ ! -n "$result" ]; then - directory=$( dirname "$0" ) - - echo "Docker image not built yet. Build from Dockerfile. " - echo ${directory} - docker build -t $image_name $directory -else - echo "Use existing docker image: "$image_name"" -fi +# Build docker image from Dockerfile in directory +directory=$( dirname "$0" ) +docker build -t $image_name $directory # Stop any of the 3 containers if running RUNNING_CONTAINERS=$( docker container ls -a -q --filter ancestor=exomy ) From c43a60d8fa3d44844e6cdafe3e10423a6d61aca4 Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Fri, 21 Aug 2020 11:02:32 +0200 Subject: [PATCH 12/20] Rename messagenames to be more descriptive. --- CMakeLists.txt | 4 ++-- msg/{Commands.msg => MotorCommands.msg} | 0 msg/{Joystick.msg => RoverCommand.msg} | 1 + package.xml | 2 +- src/joystick_node.py | 25 +++++++++++++------------ src/motor_node.py | 4 ++-- src/robot_node.py | 8 ++++---- 7 files changed, 23 insertions(+), 21 deletions(-) rename msg/{Commands.msg => MotorCommands.msg} (100%) rename msg/{Joystick.msg => RoverCommand.msg} (75%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e69995..abfa88c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,8 +51,8 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES - Joystick.msg - Commands.msg + RoverCommand.msg + MotorCommands.msg Screen.msg ) ## Generate services in the 'srv' folder diff --git a/msg/Commands.msg b/msg/MotorCommands.msg similarity index 100% rename from msg/Commands.msg rename to msg/MotorCommands.msg diff --git a/msg/Joystick.msg b/msg/RoverCommand.msg similarity index 75% rename from msg/Joystick.msg rename to msg/RoverCommand.msg index 49b0f42..cf67f6b 100644 --- a/msg/Joystick.msg +++ b/msg/RoverCommand.msg @@ -1,4 +1,5 @@ bool connected +bool motors_enabled int64 locomotion_mode int64 vel int64 steering diff --git a/package.xml b/package.xml index 607f8c9..55240eb 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ exomy - 1.0.0 + 1.0.1 The ExoMy ROS package Maximilian Ehrhardt diff --git a/src/joystick_node.py b/src/joystick_node.py index c1bd8c1..d5aad8f 100755 --- a/src/joystick_node.py +++ b/src/joystick_node.py @@ -2,7 +2,7 @@ import rospy import time from sensor_msgs.msg import Joy -from exomy.msg import Joystick +from exomy.msg import RoverCommand from locomotion_modes import LocomotionMode import math import enum @@ -15,15 +15,16 @@ def callback(data): global locomotion_mode - joy_out = Joystick() + rover_cmd = RoverCommand() # Function map for the Logitech F710 joystick # Button on pad | function # --------------|---------------------- - # A | Ackermann mode - # X | Point turn mode - # Y | Crabbing mode - # left stick | control speed and direction + # A | Ackermann mode + # X | Point turn mode + # Y | Crabbing mode + # Left Stick | Control speed and direction + # START Button | Enable and disable motors # Reading out joystick data y = data.axes[1] @@ -39,10 +40,10 @@ def callback(data): if (data.buttons[1] == 1): locomotion_mode = LocomotionMode.ACKERMANN.value - joy_out.locomotion_mode=locomotion_mode + rover_cmd.locomotion_mode=locomotion_mode # The velocity is decoded as value between 0...100 - joy_out.vel = 100 * min(math.sqrt(x*x + y*y),1.0) + rover_cmd.vel = 100 * min(math.sqrt(x*x + y*y),1.0) # The steering is described as an angle between -180...180 # Which describe the joystick position as follows: @@ -50,11 +51,11 @@ def callback(data): # 0 +-180 # -90 # - joy_out.steering = math.atan2(y, x)*180.0/math.pi + rover_cmd.steering = math.atan2(y, x)*180.0/math.pi - joy_out.connected = True + rover_cmd.connected = True - pub.publish(joy_out) + pub.publish(rover_cmd) @@ -65,6 +66,6 @@ def callback(data): rospy.loginfo('joystick started') sub = rospy.Subscriber("/joy", Joy, callback, queue_size=1) - pub = rospy.Publisher('/rover_commands', Joystick, queue_size=1) + pub = rospy.Publisher('/rover_commands', RoverCommand, queue_size=1) rospy.spin() diff --git a/src/motor_node.py b/src/motor_node.py index c91eb0e..9f612df 100755 --- a/src/motor_node.py +++ b/src/motor_node.py @@ -2,7 +2,7 @@ import time import rospy -from exomy.msg import Commands +from exomy.msg import MotorCommands from motors import Motors motors = Motors() @@ -34,7 +34,7 @@ def watchdog(event): global watchdog_timer watchdog_timer = rospy.Timer(rospy.Duration(1.0), watchdog, oneshot=True) - sub = rospy.Subscriber("/motor_commands",Commands, callback, queue_size=1) + sub = rospy.Subscriber("/motor_commands", MotorCommands, callback, queue_size=1) rate = rospy.Rate(10) diff --git a/src/robot_node.py b/src/robot_node.py index 282077e..bb9a1a5 100755 --- a/src/robot_node.py +++ b/src/robot_node.py @@ -1,6 +1,6 @@ #!/usr/bin/env python import time -from exomy.msg import Joystick, Commands, Screen +from exomy.msg import RoverCommand, MotorCommands, Screen import rospy from rover import Rover import message_filters @@ -11,7 +11,7 @@ def joy_callback(message): - cmds = Commands() + cmds = MotorCommands() exomy.setLocomotionMode(message.locomotion_mode) cmds.motor_angles = exomy.joystickToSteeringAngle( @@ -25,10 +25,10 @@ def joy_callback(message): rospy.init_node('robot') rospy.loginfo("Starting the robot node") global robot_pub - joy_sub = rospy.Subscriber("/rover_commands", Joystick, joy_callback, queue_size=1) + joy_sub = rospy.Subscriber("/rover_commands", RoverCommand, joy_callback, queue_size=1) rate = rospy.Rate(10) - robot_pub = rospy.Publisher("/motor_commands", Commands, queue_size=1) + robot_pub = rospy.Publisher("/motor_commands", MotorCommands, queue_size=1) rospy.spin() From c339e34d7f44410fb1c4d7a0b9abc2ddc02faf33 Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Fri, 21 Aug 2020 12:34:51 +0200 Subject: [PATCH 13/20] Added e-stop functionality. - Using the start button, the motors can be en- and disabled. - Joystick node was renamed for clarity. --- launch/exomy.launch | 2 +- ...ystick_node.py => joystick_parser_node.py} | 41 +++++++++++++++---- src/robot_node.py | 18 +++++--- src/rover.py | 3 ++ 4 files changed, 48 insertions(+), 16 deletions(-) rename src/{joystick_node.py => joystick_parser_node.py} (65%) diff --git a/launch/exomy.launch b/launch/exomy.launch index b912820..3dca58d 100644 --- a/launch/exomy.launch +++ b/launch/exomy.launch @@ -1,7 +1,7 @@ - + diff --git a/src/joystick_node.py b/src/joystick_parser_node.py similarity index 65% rename from src/joystick_node.py rename to src/joystick_parser_node.py index d5aad8f..efefc0e 100755 --- a/src/joystick_node.py +++ b/src/joystick_parser_node.py @@ -8,13 +8,17 @@ import enum # Define locomotion modes - global locomotion_mode +global motors_enabled + locomotion_mode = LocomotionMode.ACKERMANN.value +motors_enabled = True def callback(data): global locomotion_mode + global motors_enabled + rover_cmd = RoverCommand() # Function map for the Logitech F710 joystick @@ -31,16 +35,35 @@ def callback(data): x = data.axes[0] # Reading out button data to set locomotion mode + # X Button if (data.buttons[0] == 1): locomotion_mode = LocomotionMode.POINT_TURN.value - if (data.buttons[3] == 1): - locomotion_mode = LocomotionMode.CRABBING.value - if (data.buttons[2] == 1): - pass + # A Button if (data.buttons[1] == 1): locomotion_mode = LocomotionMode.ACKERMANN.value + # B Button + if (data.buttons[2] == 1): + pass + # Y Button + if (data.buttons[3] == 1): + locomotion_mode = LocomotionMode.CRABBING.value + + rover_cmd.locomotion_mode = locomotion_mode + + # Enable and disable motors + # START Button + if (data.buttons[9] == 1): + if motors_enabled is True: + motors_enabled = False + rospy.loginfo("Motors disabled!") + elif motors_enabled is False: + motors_enabled = True + rospy.loginfo("Motors enabled!") + else: + rospy.logerr("Exceptional value for [motors_enabled] = {}".format(motors_enabled)) + motors_enabled = False - rover_cmd.locomotion_mode=locomotion_mode + rover_cmd.motors_enabled = motors_enabled # The velocity is decoded as value between 0...100 rover_cmd.vel = 100 * min(math.sqrt(x*x + y*y),1.0) @@ -62,10 +85,10 @@ def callback(data): if __name__ == '__main__': global pub - rospy.init_node('joystick') - rospy.loginfo('joystick started') + rospy.init_node('joystick_parser_node') + rospy.loginfo('joystick_parser_node started') sub = rospy.Subscriber("/joy", Joy, callback, queue_size=1) - pub = rospy.Publisher('/rover_commands', RoverCommand, queue_size=1) + pub = rospy.Publisher('/rover_command', RoverCommand, queue_size=1) rospy.spin() diff --git a/src/robot_node.py b/src/robot_node.py index bb9a1a5..babb9cd 100755 --- a/src/robot_node.py +++ b/src/robot_node.py @@ -13,19 +13,25 @@ def joy_callback(message): cmds = MotorCommands() - exomy.setLocomotionMode(message.locomotion_mode) - cmds.motor_angles = exomy.joystickToSteeringAngle( - message.vel, message.steering) - cmds.motor_speeds = exomy.joystickToVelocity(message.vel, message.steering) + + if message.motors_enabled is True: + exomy.setLocomotionMode(message.locomotion_mode) + + cmds.motor_angles = exomy.joystickToSteeringAngle(message.vel, message.steering) + cmds.motor_speeds = exomy.joystickToVelocity(message.vel, message.steering) + else: + cmds.motor_angles = exomy.joystickToSteeringAngle(0, 0) + cmds.motor_speeds = exomy.joystickToVelocity(0, 0) + robot_pub.publish(cmds) if __name__ == '__main__': - rospy.init_node('robot') + rospy.init_node('robot_node') rospy.loginfo("Starting the robot node") global robot_pub - joy_sub = rospy.Subscriber("/rover_commands", RoverCommand, joy_callback, queue_size=1) + joy_sub = rospy.Subscriber("/rover_command", RoverCommand, joy_callback, queue_size=1) rate = rospy.Rate(10) diff --git a/src/rover.py b/src/rover.py index 75a07ce..71e64b7 100755 --- a/src/rover.py +++ b/src/rover.py @@ -42,6 +42,9 @@ def setLocomotionMode(self, locomotion_mode_command): def joystickToSteeringAngle(self, driving_command, steering_command): ''' Converts the steering command [angle of joystick] to angles for the different motors + + :param int driving_command: Drive speed command range from -100 to 100 + :param int stering_command: Turning radius command with the values 0(left) +90(forward) -90(backward) +-180(right) ''' steering_angles = [0]*6 From 7fba0890e1d35c02096c3940ea478e100d2f7ce2 Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Fri, 21 Aug 2020 14:26:44 +0200 Subject: [PATCH 14/20] Add basic linux tools to docker file. --- docker/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 7658b5c..1a01fef 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -6,7 +6,7 @@ RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone # Basic tools RUN apt-get update && \ - apt-get install vim git tmux wget curl python-pip -y + apt-get install vim nano git tmux wget curl python-pip net-tools iputils-ping -y # Install additional ros packages RUN apt-get update && apt-get install ros-melodic-rosbridge-server ros-melodic-joy -y From 32cc9fa8422d062e610adba3148de33a33d4954e Mon Sep 17 00:00:00 2001 From: Maximilian Ehrhardt Date: Tue, 25 Aug 2020 11:13:55 +0200 Subject: [PATCH 15/20] Code formatting --- src/joystick_parser_node.py | 17 ++++---- src/motor_node.py | 15 ++++--- src/motors.py | 25 +++++------ src/robot_node.py | 11 ++--- src/rover.py | 87 +++++++++++++++++-------------------- 5 files changed, 76 insertions(+), 79 deletions(-) diff --git a/src/joystick_parser_node.py b/src/joystick_parser_node.py index efefc0e..971b616 100755 --- a/src/joystick_parser_node.py +++ b/src/joystick_parser_node.py @@ -1,24 +1,23 @@ #!/usr/bin/env python import rospy -import time from sensor_msgs.msg import Joy from exomy.msg import RoverCommand from locomotion_modes import LocomotionMode import math -import enum # Define locomotion modes global locomotion_mode global motors_enabled locomotion_mode = LocomotionMode.ACKERMANN.value -motors_enabled = True +motors_enabled = True + def callback(data): global locomotion_mode global motors_enabled - + rover_cmd = RoverCommand() # Function map for the Logitech F710 joystick @@ -47,7 +46,7 @@ def callback(data): # Y Button if (data.buttons[3] == 1): locomotion_mode = LocomotionMode.CRABBING.value - + rover_cmd.locomotion_mode = locomotion_mode # Enable and disable motors @@ -60,13 +59,14 @@ def callback(data): motors_enabled = True rospy.loginfo("Motors enabled!") else: - rospy.logerr("Exceptional value for [motors_enabled] = {}".format(motors_enabled)) + rospy.logerr( + "Exceptional value for [motors_enabled] = {}".format(motors_enabled)) motors_enabled = False - + rover_cmd.motors_enabled = motors_enabled # The velocity is decoded as value between 0...100 - rover_cmd.vel = 100 * min(math.sqrt(x*x + y*y),1.0) + rover_cmd.vel = 100 * min(math.sqrt(x*x + y*y), 1.0) # The steering is described as an angle between -180...180 # Which describe the joystick position as follows: @@ -81,7 +81,6 @@ def callback(data): pub.publish(rover_cmd) - if __name__ == '__main__': global pub diff --git a/src/motor_node.py b/src/motor_node.py index 9f612df..fdaac6a 100755 --- a/src/motor_node.py +++ b/src/motor_node.py @@ -6,35 +6,40 @@ from motors import Motors motors = Motors() -global watchdog_timer +global watchdog_timer + def callback(cmds): motors.setSteering(cmds.motor_angles) motors.setDriving(cmds.motor_speeds) - + global watchdog_timer watchdog_timer.shutdown() # If this timer runs longer than the duration specified, # then watchdog() is called stopping the driving motors. watchdog_timer = rospy.Timer(rospy.Duration(5.0), watchdog, oneshot=True) + def shutdown(): motors.stopMotors() + def watchdog(event): rospy.loginfo("Watchdog fired. Stopping driving motors.") motors.stopMotors() + if __name__ == "__main__": # This node waits for commands from the robot and sets the motors accordingly rospy.init_node("motors") rospy.loginfo("Starting the motors node") rospy.on_shutdown(shutdown) - + global watchdog_timer - watchdog_timer = rospy.Timer(rospy.Duration(1.0), watchdog, oneshot=True) + watchdog_timer = rospy.Timer(rospy.Duration(1.0), watchdog, oneshot=True) - sub = rospy.Subscriber("/motor_commands", MotorCommands, callback, queue_size=1) + sub = rospy.Subscriber( + "/motor_commands", MotorCommands, callback, queue_size=1) rate = rospy.Rate(10) diff --git a/src/motors.py b/src/motors.py index 55cc6c6..ea27d18 100755 --- a/src/motors.py +++ b/src/motors.py @@ -13,28 +13,26 @@ class Motors(): Motors class contains all functions to control the steering and driving ''' - # Define wheel names FL, FR, CL, CR, RL, RR = range(0, 6) - + # Motor commands are assuming positiv=driving_forward, negative=driving_backwards. # The driving direction of the left side has to be inverted for this to apply to all wheels. wheel_directions = [-1, 1, -1, 1, -1, 1] - + # 1 fl-||-fr 2 # || # 3 cl-||-cr 4 # 5 rl====rr 6 - def __init__(self): - + # Dictionary containing the pins of all motors self.pins = { 'drive': {}, 'steer': {} - } - + } + # Set variables for the GPIO motor pins self.pins['drive'][self.FL] = rospy.get_param("pin_drive_fl") self.pins['steer'][self.FL] = rospy.get_param("pin_steer_fl") @@ -74,13 +72,12 @@ def __init__(self): self.driving_pwm_range = rospy.get_param("drive_range") # Set steering motors to neutral values (straight) - for wheel_name, motor_pin in self.pins['steer'].items(): + for wheel_name, motor_pin in self.pins['steer'].items(): self.pwm.set_pwm(motor_pin, 0, self.steering_pwm_neutral[wheel_name]) time.sleep(0.1) - - self.wiggle() + self.wiggle() def wiggle(self): time.sleep(0.1) @@ -104,11 +101,11 @@ def wiggle(self): time.sleep(0.3) def setSteering(self, steering_command): - # Loop through pin dictionary. The items key is the wheel_name and the value the pin. + # Loop through pin dictionary. The items key is the wheel_name and the value the pin. for wheel_name, motor_pin in self.pins['steer'].items(): duty_cycle = int( self.steering_pwm_neutral[wheel_name] + steering_command[wheel_name]/90.0 * self.steering_pwm_range) - + self.pwm.set_pwm(motor_pin, 0, duty_cycle) def setDriving(self, driving_command): @@ -116,12 +113,12 @@ def setDriving(self, driving_command): for wheel_name, motor_pin in self.pins['drive'].items(): duty_cycle = int(self.driving_pwm_neutral + driving_command[wheel_name]/100.0 * self.driving_pwm_range * self.wheel_directions[wheel_name]) - + self.pwm.set_pwm(motor_pin, 0, duty_cycle) def stopMotors(self): # Set driving wheels to neutral position to stop them duty_cycle = int(self.driving_pwm_neutral) - + for wheel_name, motor_pin in self.pins['drive'].items(): self.pwm.set_pwm(motor_pin, 0, duty_cycle) diff --git a/src/robot_node.py b/src/robot_node.py index babb9cd..a58166f 100755 --- a/src/robot_node.py +++ b/src/robot_node.py @@ -13,17 +13,17 @@ def joy_callback(message): cmds = MotorCommands() - if message.motors_enabled is True: exomy.setLocomotionMode(message.locomotion_mode) - cmds.motor_angles = exomy.joystickToSteeringAngle(message.vel, message.steering) - cmds.motor_speeds = exomy.joystickToVelocity(message.vel, message.steering) + cmds.motor_angles = exomy.joystickToSteeringAngle( + message.vel, message.steering) + cmds.motor_speeds = exomy.joystickToVelocity( + message.vel, message.steering) else: cmds.motor_angles = exomy.joystickToSteeringAngle(0, 0) cmds.motor_speeds = exomy.joystickToVelocity(0, 0) - robot_pub.publish(cmds) @@ -31,7 +31,8 @@ def joy_callback(message): rospy.init_node('robot_node') rospy.loginfo("Starting the robot node") global robot_pub - joy_sub = rospy.Subscriber("/rover_command", RoverCommand, joy_callback, queue_size=1) + joy_sub = rospy.Subscriber( + "/rover_command", RoverCommand, joy_callback, queue_size=1) rate = rospy.Rate(10) diff --git a/src/rover.py b/src/rover.py index 71e64b7..835246a 100755 --- a/src/rover.py +++ b/src/rover.py @@ -24,7 +24,6 @@ def __init__(self): self.wheel_x = 12.0 self.wheel_y = 20.0 - max_steering_angle = 45 self.ackermann_r_min = abs( self.wheel_y) / math.tan(max_steering_angle * math.pi / 180.0) + self.wheel_x @@ -37,12 +36,13 @@ def setLocomotionMode(self, locomotion_mode_command): ''' if(self.locomotion_mode != locomotion_mode_command): self.locomotion_mode = locomotion_mode_command - rospy.loginfo('Set locomotion mode to: %s', LocomotionMode(locomotion_mode_command).name) + rospy.loginfo('Set locomotion mode to: %s', + LocomotionMode(locomotion_mode_command).name) def joystickToSteeringAngle(self, driving_command, steering_command): ''' Converts the steering command [angle of joystick] to angles for the different motors - + :param int driving_command: Drive speed command range from -100 to 100 :param int stering_command: Turning radius command with the values 0(left) +90(forward) -90(backward) +-180(right) ''' @@ -112,12 +112,11 @@ def joystickToSteeringAngle(self, driving_command, steering_command): return steering_angles if(self.locomotion_mode == LocomotionMode.ACKERMANN.value): - + # No steering if robot is not driving if(driving_command is 0): return steering_angles - # Scale between min and max Ackermann radius if math.cos(math.radians(steering_command)) == 0: r = self.ackermann_r_max @@ -125,7 +124,7 @@ def joystickToSteeringAngle(self, driving_command, steering_command): r = self.ackermann_r_max - \ abs(math.cos(math.radians(steering_command))) * \ ((self.ackermann_r_max-self.ackermann_r_min)) - + # No steering if r == self.ackermann_r_max: return steering_angles @@ -162,10 +161,10 @@ def joystickToSteeringAngle(self, driving_command, steering_command): wheel_direction = 0 if(steering_command > 0): wheel_direction = steering_command - 90 - - elif(steering_command <= 0): + + elif(steering_command <= 0): wheel_direction = steering_command + 90 - + wheel_direction = np.clip(wheel_direction, -75, 75) steering_angles[self.FL] = wheel_direction @@ -174,7 +173,6 @@ def joystickToSteeringAngle(self, driving_command, steering_command): steering_angles[self.CR] = wheel_direction steering_angles[self.RL] = wheel_direction steering_angles[self.RR] = wheel_direction - return steering_angles @@ -209,13 +207,13 @@ def joystickToVelocity(self, driving_command, steering_command): if (self.locomotion_mode == LocomotionMode.ACKERMANN.value): v = driving_command if(steering_command < 0): - v *=-1 + v *= -1 # Scale between min and max Ackermann radius radius = self.ackermann_r_max - \ abs(math.cos(math.radians(steering_command))) * \ ((self.ackermann_r_max-self.ackermann_r_min)) - + if (v == 0): return motor_speeds @@ -233,15 +231,15 @@ def joystickToVelocity(self, driving_command, steering_command): r2 = rmax_float r3 = r1 r4 = math.sqrt(a+c) - r5 = abs(radius) - self.wheel_x + r5 = abs(radius) - self.wheel_x r6 = r4 - v1 = int(v) - v2 = int(v*r2/r1) + v1 = int(v) + v2 = int(v*r2/r1) v3 = v1 - v4 = int(v*r4/r1) + v4 = int(v*r4/r1) v5 = int(v*r5/r1) - v6 = v4 + v6 = v4 if (steering_command > 90 or steering_command < -90): motor_speeds = [v1, v2, v3, v4, v5, v6] @@ -252,52 +250,49 @@ def joystickToVelocity(self, driving_command, steering_command): if (self.locomotion_mode == LocomotionMode.POINT_TURN.value): deg = steering_command - if(driving_command is not 0): + if(driving_command is not 0): # Left turn if(deg < 85 and deg > -85): motor_speeds[self.FL] = -50 - motor_speeds[self.FR] = 50 + motor_speeds[self.FR] = 50 motor_speeds[self.CL] = -50 - motor_speeds[self.CR] = 50 + motor_speeds[self.CR] = 50 motor_speeds[self.RL] = -50 - motor_speeds[self.RR] = 50 + motor_speeds[self.RR] = 50 # Right turn elif(deg > 95 or deg < -95): - motor_speeds[self.FL] = 50 + motor_speeds[self.FL] = 50 motor_speeds[self.FR] = -50 - motor_speeds[self.CL] = 50 + motor_speeds[self.CL] = 50 motor_speeds[self.CR] = -50 - motor_speeds[self.RL] = 50 + motor_speeds[self.RL] = 50 motor_speeds[self.RR] = -50 else: # Stop - motor_speeds[self.FL] = 0 - motor_speeds[self.FR] = 0 - motor_speeds[self.CL] = 0 - motor_speeds[self.CR] = 0 - motor_speeds[self.RL] = 0 - motor_speeds[self.RR] = 0 - - + motor_speeds[self.FL] = 0 + motor_speeds[self.FR] = 0 + motor_speeds[self.CL] = 0 + motor_speeds[self.CR] = 0 + motor_speeds[self.RL] = 0 + motor_speeds[self.RR] = 0 + return motor_speeds if(self.locomotion_mode == LocomotionMode.CRABBING.value): if(driving_command > 0): if(steering_command > 0): - motor_speeds[self.FL] = 50 - motor_speeds[self.FR] = 50 - motor_speeds[self.CL] = 50 - motor_speeds[self.CR] = 50 - motor_speeds[self.RL] = 50 - motor_speeds[self.RR] = 50 + motor_speeds[self.FL] = 50 + motor_speeds[self.FR] = 50 + motor_speeds[self.CL] = 50 + motor_speeds[self.CR] = 50 + motor_speeds[self.RL] = 50 + motor_speeds[self.RR] = 50 elif(steering_command <= 0): - motor_speeds[self.FL] = -50 - motor_speeds[self.FR] = -50 - motor_speeds[self.CL] = -50 - motor_speeds[self.CR] = -50 - motor_speeds[self.RL] = -50 - motor_speeds[self.RR] = -50 - - + motor_speeds[self.FL] = -50 + motor_speeds[self.FR] = -50 + motor_speeds[self.CL] = -50 + motor_speeds[self.CR] = -50 + motor_speeds[self.RL] = -50 + motor_speeds[self.RR] = -50 return motor_speeds From 297240d3e20fa5d5beebbe79b0a6d9be3035a809 Mon Sep 17 00:00:00 2001 From: Maximilian Ehrhardt Date: Tue, 25 Aug 2020 11:25:49 +0200 Subject: [PATCH 16/20] Fix button array size in web gui --- gui/index.html | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/gui/index.html b/gui/index.html index f7c3072..f8bf0f8 100644 --- a/gui/index.html +++ b/gui/index.html @@ -54,15 +54,15 @@ // events triggered earlier than 50ms after last publication will be dropped if (publishImmidiately) { - publishImmidiately = false; - joy_event() - window.clearInterval(watchdog_handle) - watchdog_handle=window.setInterval(joy_event, 50) - - setTimeout(function () { - publishImmidiately = true; - }, 50); - } + publishImmidiately = false; + joy_event() + window.clearInterval(watchdog_handle) + watchdog_handle = window.setInterval(joy_event, 50) + + setTimeout(function () { + publishImmidiately = true; + }, 50); + } //joy_event() //window.clearInterval() //window.setInterval(joy_event, 500) @@ -72,7 +72,7 @@ window.clearInterval(watchdog_handle) axes = [0, 0] joy_event() - console.log('End function called') + console.log('End function called') }); } window.onload = function () { @@ -101,27 +101,27 @@ buttonY_clicked = function () { axes = [0, 0] - buttons = [0, 0, 0, 1]; + buttons = [0, 0, 0, 1, 0, 0, 0, 0, 0, 0]; joy_event(); } buttonX_clicked = function () { axes = [0, 0] - buttons = [1, 0, 0, 0]; + buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } buttonB_clicked = function () { axes = [0, 0] - buttons = [0, 0, 1, 0]; + buttons = [0, 0, 1, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } buttonA_clicked = function () { axes = [0, 0] - buttons = [0, 1, 0, 0]; + buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } var axes = [0, 0]; - var buttons = [0, 0, 0, 0]; + var buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; @@ -153,4 +153,4 @@ - + \ No newline at end of file From f5973ff528f71830840840eaad1389597ad8923f Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Tue, 25 Aug 2020 16:31:11 +0200 Subject: [PATCH 17/20] Update GUI button mapping and improve visuals. --- gui/index.html | 28 ++++++++++++++-------------- gui/style.css | 15 +++++++++------ 2 files changed, 23 insertions(+), 20 deletions(-) diff --git a/gui/index.html b/gui/index.html index f8bf0f8..5d8dc5c 100644 --- a/gui/index.html +++ b/gui/index.html @@ -100,28 +100,28 @@ } buttonY_clicked = function () { - axes = [0, 0] - buttons = [0, 0, 0, 1, 0, 0, 0, 0, 0, 0]; + axex = [0, 0, 0, 0, 0, 0] + buttons = [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } buttonX_clicked = function () { - axes = [0, 0] - buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0]; + axes = [0, 0, 0, 0, 0, 0] + buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } buttonB_clicked = function () { - axes = [0, 0] - buttons = [0, 0, 1, 0, 0, 0, 0, 0, 0, 0]; + axes = [0, 0, 0, 0, 0, 0] + buttons = [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } buttonA_clicked = function () { - axes = [0, 0] - buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0]; + axes = [0, 0, 0, 0, 0, 0] + buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } - var axes = [0, 0]; - var buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; + var axes = [0, 0, 0, 0, 0, 0]; + var buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; @@ -144,13 +144,13 @@ - \ No newline at end of file + diff --git a/gui/style.css b/gui/style.css index 075f414..62eef08 100644 --- a/gui/style.css +++ b/gui/style.css @@ -22,18 +22,21 @@ body { color: white; text-decoration: none; display: inline-block; - margin: 0 auto; + margin: 10px auto; cursor: pointer; width: 90%; box-shadow: 0 8px 16px 0 rgba(0,0,0,0.2), 0 6px 20px 0 rgba(0,0,0,0.19); + font-size: 18px; + font-weight: bold; + border-radius: 8px; } .buttonY{ background-color: orange; grid-row: 1; } -.buttonB{ - background-color: red; +.buttonX{ + background-color: blue; grid-row: 2; } .buttonA{ @@ -51,9 +54,9 @@ body { } .image_container { position: absolute; - top: 25%; - height: 100%; - width: 100%; + top: 20%; + height: 80%; + width: 80%; } .joy_container { height: 100%; From 02d0e75c64ae1c01bc3238f68326fd92c4788570 Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Wed, 26 Aug 2020 19:52:41 +0200 Subject: [PATCH 18/20] Add button to en-/disable motors. Fixed bug where buttons would not be reset to all 0 zeros after press. --- gui/index.html | 27 ++++++++++++++++++--------- gui/style.css | 8 ++++++++ 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/gui/index.html b/gui/index.html index 5d8dc5c..cf63ef2 100644 --- a/gui/index.html +++ b/gui/index.html @@ -46,7 +46,7 @@ var max_distance = 75; var x = -Math.cos(nipple.angle.radian) * nipple.distance / max_distance; var y = Math.sin(nipple.angle.radian) * nipple.distance / max_distance; - axes = [x, y] + axes = [x, y, 0, 0, 0, 0] // nipplejs is triggering events when joystick moves each pixel // we need delay between consecutive messege publications to @@ -70,7 +70,7 @@ self.manager.on('end', function () { window.clearInterval(watchdog_handle) - axes = [0, 0] + axes = [0, 0, 0, 0, 0, 0] joy_event() console.log('End function called') }); @@ -97,33 +97,42 @@ joy_listener.publish(joy); console.log(axes) console.log(buttons) + // The buttons variable is reset to all 0s after the command was sent. + // The axes message shold persists since the axes message is + // only updated if the joystick is moved. + buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; } buttonY_clicked = function () { - axex = [0, 0, 0, 0, 0, 0] + axes = [0, 0, 0, 0, 0, 0]; buttons = [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } buttonX_clicked = function () { - axes = [0, 0, 0, 0, 0, 0] + axes = [0, 0, 0, 0, 0, 0]; buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } buttonB_clicked = function () { - axes = [0, 0, 0, 0, 0, 0] + axes = [0, 0, 0, 0, 0, 0]; buttons = [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } buttonA_clicked = function () { - axes = [0, 0, 0, 0, 0, 0] + axes = [0, 0, 0, 0, 0, 0]; buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; joy_event(); } + buttonStart_clicked = function () { + axes = [0, 0, 0, 0, 0, 0]; + buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0]; + joy_event(); + } + var axes = [0, 0, 0, 0, 0, 0]; var buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; - @@ -133,8 +142,7 @@
-
-
+
@@ -147,6 +155,7 @@ +
diff --git a/gui/style.css b/gui/style.css index 62eef08..ad9ce13 100644 --- a/gui/style.css +++ b/gui/style.css @@ -44,6 +44,14 @@ body { grid-row: 3; } +.buttonStart{ + background-color: red; + grid-row: 4; + border-radius: 50%; + width: 40%; + font-size: 12px; +} + .button_container { position: absolute; display: grid; From 49c2424c6e79633852a1e253ce2e65936094c102 Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Wed, 26 Aug 2020 20:23:14 +0200 Subject: [PATCH 19/20] Simplify javascript and make motors button square --- gui/index.html | 49 +++++++++++++++---------------------------------- gui/style.css | 4 ++-- 2 files changed, 17 insertions(+), 36 deletions(-) diff --git a/gui/index.html b/gui/index.html index cf63ef2..1b1909d 100644 --- a/gui/index.html +++ b/gui/index.html @@ -16,6 +16,10 @@ url: 'ws://' + host_url + ':9090' }); + // Initialize Axes and Buttons arrays + var axes = [0, 0, 0, 0, 0, 0]; + var buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; + ros.on('connection', function () { document.getElementById("status").innerHTML = "Connected"; }); @@ -97,41 +101,18 @@ joy_listener.publish(joy); console.log(axes) console.log(buttons) - // The buttons variable is reset to all 0s after the command was sent. - // The axes message shold persists since the axes message is - // only updated if the joystick is moved. - buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; } - buttonY_clicked = function () { - axes = [0, 0, 0, 0, 0, 0]; - buttons = [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0]; - joy_event(); - } - buttonX_clicked = function () { + button_clicked = function (button_index) { + // The desired button index can be found by listening to the /joy ros message + // or by checking in the wiki for the gamepad mapping. + // Set axes to 0 to prevent driving during mode change. axes = [0, 0, 0, 0, 0, 0]; - buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; + buttons[button_index] = 1; joy_event(); + // After the command is sent set the index back to 0 + buttons[button_index] = 0; } - buttonB_clicked = function () { - axes = [0, 0, 0, 0, 0, 0]; - buttons = [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]; - joy_event(); - } - buttonA_clicked = function () { - axes = [0, 0, 0, 0, 0, 0]; - buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; - joy_event(); - } - - buttonStart_clicked = function () { - axes = [0, 0, 0, 0, 0, 0]; - buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0]; - joy_event(); - } - - var axes = [0, 0, 0, 0, 0, 0]; - var buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; @@ -152,10 +133,10 @@ diff --git a/gui/style.css b/gui/style.css index ad9ce13..d460fa3 100644 --- a/gui/style.css +++ b/gui/style.css @@ -47,9 +47,9 @@ body { .buttonStart{ background-color: red; grid-row: 4; - border-radius: 50%; - width: 40%; + width: 50%; font-size: 12px; + } .button_container { From 38e7dae432a55fe95b0f35a1bec88614a93a56d2 Mon Sep 17 00:00:00 2001 From: Miro Voellmy Date: Wed, 26 Aug 2020 20:27:22 +0200 Subject: [PATCH 20/20] Convert tab to spaces --- gui/index.html | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gui/index.html b/gui/index.html index 1b1909d..11ab154 100644 --- a/gui/index.html +++ b/gui/index.html @@ -134,9 +134,9 @@