Skip to content

Commit

Permalink
[update]
Browse files Browse the repository at this point in the history
  • Loading branch information
JunyaoHu committed Mar 19, 2024
1 parent 2408a6e commit 6796d05
Show file tree
Hide file tree
Showing 290 changed files with 175,154 additions and 4,193 deletions.
101 changes: 101 additions & 0 deletions Dockerfile_bak_can_use
Original file line number Diff line number Diff line change
@@ -0,0 +1,101 @@
# Get an image with git and apt-get update
FROM docker.io/ros:noetic-ros-base-focal as base
ENV DEBIAN_FRONTEND=noninteractive

RUN apt-get update && \
apt-get install --yes git
RUN rosdep update

# Install mosquitto broker
RUN apt-get install --yes mosquitto

# Install our config
COPY --link ./assets/mosquitto.conf /etc/mosquitto/mosquitto.conf

# Install nginx for hosting the app
RUN apt-get install --yes nginx

# Remove default nginx config (else it will run on port 80)
RUN rm /etc/nginx/sites-enabled/*

# Install nginx config
COPY --link ./assets/nginx.conf /etc/nginx/conf.d/default.conf

# First stage: Pull the git and all submodules, other stages depend on it
FROM base as fetch

ENV DEBIAN_FRONTEND=noninteractive

COPY --link ./ /opt/open_mower_ros
#RUN git clone https://github.com/ClemensElflein/open_mower_ros /opt/open_mower_ros

WORKDIR /opt/open_mower_ros

RUN git submodule update --init --recursive


# Get slic3r_coverage_planner and build that. We will pull the finished install folder from this.
# This stage should cache most of the time, that's why it's not derived from the fetch stage, but copies stuff instead.
FROM base as slic3r

ENV DEBIAN_FRONTEND=noninteractive

# Fetch the slic3r planner from the repo (this will cache if unchanged)
COPY --link --from=fetch /opt/open_mower_ros/src/lib/slic3r_coverage_planner /opt/slic3r_coverage_planner_workspace/src

WORKDIR /opt/slic3r_coverage_planner_workspace
RUN rosdep install --from-paths src --ignore-src --simulate | \
sed --expression '1d' --expression 's/apt-get install/apt-get install --no-install-recommends --yes/g' | \
bash \
&& rm -rf /var/lib/apt/lists/*
RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && catkin_make -j`nproc`"
RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && source /opt/slic3r_coverage_planner_workspace/devel/setup.bash && catkin_make -DCMAKE_INSTALL_PREFIX=/opt/prebuilt/slic3r_coverage_planner install"


# Fetch the repo and assemble the list of dependencies. We will pull these in the next step and actually run install on them
# If the package list is the same as last time, the apt install step is cached as well which saves a lot of time.
# Since the list gets sorted, it will be the same each time and the cache will know that by file checksum in the COPY command.
# We can't use this stage as base for the next, because this stage is run every time and would therefore invalidate the cache of the next stage.
FROM fetch as dependencies

ENV DEBIAN_FRONTEND=noninteractive

WORKDIR /opt/open_mower_ros

# This creates the sorted list of apt-get install commands.
RUN apt-get update && \
rosdep install --from-paths src --ignore-src --simulate | \
sed --expression '1d' --expression 's/apt-get install/apt-get install --no-install-recommends --yes/g' | sort > /apt-install.sh


# We can't derive this from "dependencies" because "dependencies" will be rebuilt every time, but apt install should only be done if needed
FROM base as assemble

ENV DEBIAN_FRONTEND=noninteractive

#Fetch the slic3r built earlier, this only changes if slic3r was changed (probably never)
COPY --link --from=slic3r /opt/prebuilt/slic3r_coverage_planner /opt/prebuilt/slic3r_coverage_planner

#Fetch the list of packages, this only changes if new dependencies have been added (only sometimes)
COPY --link --from=dependencies /apt-install.sh /apt-install.sh
RUN apt-get update && \
bash /apt-install.sh && \
rm -rf /var/lib/apt/lists/*

# This will already have the submodules initialized, no need to clone again
COPY --link --from=fetch /opt/open_mower_ros /opt/open_mower_ros

#delete prebuilt libs (so that they won't shadow the preinstalled ones)
RUN rm -rf /opt/open_mower_ros/src/lib/slic3r_coverage_planner

#RUN git clone https://github.com/ClemensElflein/open_mower_ros /opt/open_mower_ros

WORKDIR /opt/open_mower_ros

RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && cd /opt/open_mower_ros/src && catkin_init_workspace && cd .. && source /opt/prebuilt/slic3r_coverage_planner/setup.bash && catkin_make -DCATKIN_BLACKLIST_PACKAGES=slic3r_coverage_planner"

COPY .github/assets/openmower_entrypoint.sh /openmower_entrypoint.sh
RUN chmod +x /openmower_entrypoint.sh

ENTRYPOINT ["/openmower_entrypoint.sh"]
CMD ["bash", "-c", "service nginx start; service mosquitto start; roslaunch open_mower open_mower.launch --screen"]
3 changes: 0 additions & 3 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,6 @@ docker run \


```
sudo chmod 666 /dev/ttyAMA0
cd /opt/NankaiMower/
source devel/setup.bash; source mower_config.sh
```
Expand All @@ -34,7 +32,6 @@ pip install empy==3.3.4
```
catkin_make -DCATKIN_WHITELIST_PACKAGES="mower_msgs;ntrip_client;;vesc_driver;xesc;xesc_2040_driver;xesc_driver;xesc_interface;xesc_msgs;xbot_msgs;xbot_driver_gps;vesc_driver;mower_comms"
source devel/setup.bash; source mower_config.sh
sudo chmod 666 /dev/ttyAMA0
roslaunch open_mower _comms.launch
```

Expand Down
35 changes: 22 additions & 13 deletions mower_config.sh
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,23 @@ export OM_HARDWARE_VERSION="0_13_X"

# Uncomment and set to coordinates near your future docking station, this will be your map origin.
# There might be a case that you don't need those if you using OM_USE_RELATIVE_POSITION=True
export OM_DATUM_LAT=48.0
export OM_DATUM_LONG=11.0
export OM_DATUM_LAT=38.937928
export OM_DATUM_LONG=117.357260

# NTRIP Settings
# Set to False if using external radio plugged into the Ardusimple board.
export OM_USE_NTRIP=True
#export OM_NTRIP_HOSTNAME=ntrip.gnsslab.cn
#export OM_NTRIP_PORT=2101
#export OM_NTRIP_USER=nankaicv
#export OM_NTRIP_PASSWORD=nkcvL4B@428
#export OM_NTRIP_ENDPOINT=DAEJ00KOR0
#export OM_NTRIP_HOSTNAME=ntrip.gnsslab.cn
#export OM_NTRIP_PORT=2101
#export OM_NTRIP_USER=nankaicv
#export OM_NTRIP_PASSWORD=nkcvL4B@428
#export OM_NTRIP_ENDPOINT=WHU200CHN0

export OM_NTRIP_HOSTNAME=rtk.huacenav.com
export OM_NTRIP_PORT=8001
export OM_NTRIP_USER=cytt3622
Expand Down Expand Up @@ -69,10 +80,7 @@ export OM_USE_RELATIVE_POSITION=False
# GPS protocol. Use UBX for u-blox chipsets and NMEA for everything else
export OM_GPS_PROTOCOL=UBX

# If you use a different gps board you maybe want to set a different baudrate.
export OM_GPS_BAUDRATE="921600"

# If you want to use F9R's sensor fusion, set this to true (you will also need to set DATUM_LAT and DATUM_LONG).
# If you want to use F9R's sensor fusion, set this to true (you will also need to set DATUM_LAT and DATUM_LONG.
# Consider this option unstable, since I don't have the F9R anymore, so I'm not able to test this.
# IF YOU DONT KNOW WHAT THIS IS, SET IT TO FALSE
export OM_USE_F9R_SENSOR_FUSION=False
Expand All @@ -90,9 +98,6 @@ export OM_UNDOCK_DISTANCE=2.0
# How many outlines should the mover drive. It's not recommended to set this below 4.
export OM_OUTLINE_COUNT=4

# How many outlines should the fill (lanes) overlap
export OM_OUTLINE_OVERLAP_COUNT=0

# Mowing angle offset -180 deg - +180 deg, 0 = east, -90 = north. If mowing angle offset is not absolute it gets added to the auto detected angle which is set by the first 2 m of recorded outline.
export OM_MOWING_ANGLE_OFFSET=0
export OM_MOWING_ANGLE_OFFSET_IS_ABSOLUTE=False
Expand All @@ -105,8 +110,10 @@ export OM_MOWING_ANGLE_INCREMENT=0
export OM_TOOL_WIDTH=0.13

# Voltages for battery to be considered full or empty
export OM_BATTERY_EMPTY_VOLTAGE=25.0
export OM_BATTERY_FULL_VOLTAGE=28.5
export OM_BATTERY_EMPTY_VOLTAGE=10
#25.0
export OM_BATTERY_FULL_VOLTAGE=24
#28.5

# Mower motor temperatures to stop and start mowing
export OM_MOWING_MOTOR_TEMP_HIGH=80.0
Expand All @@ -124,9 +131,11 @@ export OM_ENABLE_MOWER=true
# 1 - SEMIAUTO - mow the entire map once then wait for manual start atgain
# 0 - MANUAL - mowing requires manual start (default if unset)
export OM_AUTOMATIC_MODE=0

export OM_AUTOMATIC_START=false
export OM_OUTLINE_OFFSET=0.05

export OM_max_position_accuracy=5.00

################################
## External MQTT Broker ##
################################
Expand All @@ -138,9 +147,9 @@ export OM_OUTLINE_OFFSET=0.05
# export OM_MQTT_PORT="1883"
# export OM_MQTT_USER=""
# export OM_MQTT_PASSWORD=""
# export OM_MQTT_TOPIC_PREFIX="openmower"


# source the default values for the hardware platform.
# you only need this line on non-docker installs. in the docker, it will be done automatically.
source $(rospack find open_mower)/params/hardware_specific/$OM_MOWER/default_environment.sh

1 change: 0 additions & 1 deletion src/CMakeLists.txt

This file was deleted.

69 changes: 69 additions & 0 deletions src/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,69 @@
# toplevel CMakeLists.txt for a catkin workspace
# catkin/cmake/toplevel.cmake

cmake_minimum_required(VERSION 3.0.2)

project(Project)

set(CATKIN_TOPLEVEL TRUE)

# search for catkin within the workspace
set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
execute_process(COMMAND ${_cmd}
RESULT_VARIABLE _res
OUTPUT_VARIABLE _out
ERROR_VARIABLE _err
OUTPUT_STRIP_TRAILING_WHITESPACE
ERROR_STRIP_TRAILING_WHITESPACE
)
if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
# searching fot catkin resulted in an error
string(REPLACE ";" " " _cmd_str "${_cmd}")
message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
endif()

# include catkin from workspace or via find_package()
if(_res EQUAL 0)
set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
# include all.cmake without add_subdirectory to let it operate in same scope
include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
add_subdirectory("${_out}")

else()
# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
# or CMAKE_PREFIX_PATH from the environment
if(NOT DEFINED CMAKE_PREFIX_PATH)
if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
if(NOT WIN32)
string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
else()
set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
endif()
endif()
endif()

# list of catkin workspaces
set(catkin_search_path "")
foreach(path ${CMAKE_PREFIX_PATH})
if(EXISTS "${path}/.catkin")
list(FIND catkin_search_path ${path} _index)
if(_index EQUAL -1)
list(APPEND catkin_search_path ${path})
endif()
endif()
endforeach()

# search for catkin in all workspaces
set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
find_package(catkin QUIET
NO_POLICY_SCOPE
PATHS ${catkin_search_path}
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
unset(CATKIN_TOPLEVEL_FIND_PACKAGE)

if(NOT catkin_FOUND)
message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
endif()
endif()

catkin_workspace()
Empty file modified src/lib/ftc_local_planner/.gitignore
100755 → 100644
Empty file.
3 changes: 1 addition & 2 deletions src/lib/ftc_local_planner/CMakeLists.txt
100755 → 100644
Original file line number Diff line number Diff line change
Expand Up @@ -67,8 +67,7 @@ catkin_package(
)

add_library(ftc_local_planner
src/ftc_planner.cpp
src/recovery_behaviors.cpp)
src/ftc_planner.cpp)
target_link_libraries(ftc_local_planner ${catkin_LIBRARIES})
add_dependencies(ftc_local_planner ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_dependencies(ftc_local_planner ftc_local_planner_gencfg)
Expand Down
19 changes: 1 addition & 18 deletions src/lib/ftc_local_planner/README.md
100755 → 100644
Original file line number Diff line number Diff line change
@@ -1,23 +1,6 @@
# FTCLocalPlanner

This repository contains a very simple "follow the carrot" local planner implementation.
A very simple "follow the carrot" local planner implementation.

## Description
The planner follows a given path as exactly as possible. It calculate the position of the carrot by
taking robots velocity limits into account. If deviation between carrot and robot is above a given
threshold, the planner stops.

Also it checks for collisions of carrot with obstacles as well as collisions of robot footprint at actual robot pose. It doesn't implement obstacle aviodance but obstacle detection. Following situation will cause the
planner to exit:
- goal reached
- goal (pose) timeout reached
- deviation between carrot and robot
- collision of carrot with obstacles
- collision of robot footprint with obstacles

## Published Topics
- **`global_point`** (geometry_msgs/PoseStamped) pose of carrot
- **`global_plan`** (nav_msgs/Path) global path to follow
- **`debug_pid`** (ftc_local_planner/PID) debug information of PID calculation
- **`costmap_marker`** (visualization_msgs/Marker) debug information of costmap check

73 changes: 28 additions & 45 deletions src/lib/ftc_local_planner/cfg/FTCPlanner.cfg
Original file line number Diff line number Diff line change
Expand Up @@ -4,56 +4,39 @@ from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, d

gen = ParameterGenerator()

grp_cp = gen.add_group("ControlPoint", type="tab")
grp_cp.add("speed_fast", double_t, 0, "The max speed with which the control point is moved along the path if the straight distance is larger than speed_fast_threshold", 0.5, 0, 2.0)
grp_cp.add("speed_fast_threshold", double_t, 0, "Min straight distance in front of the robot for fast speed to be used", 1.5, 0, 5.0)
grp_cp.add("speed_fast_threshold_angle", double_t, 0, "Max angle between current pose and pose on path to still count as straight line", 5.0, 0, 45.0)
grp_cp.add("speed_slow", double_t, 0, "Slow speed", 0.2, 0, 2.0)
grp_cp.add("speed_angular", double_t, 0, "The max angular speed with which the control point is moved along the path", 20.0, 1.0, 150.0)
grp_cp.add("acceleration", double_t, 0, "Acceleration for speed transition", 1.0, 0, 10.0)

grp_pid = gen.add_group("PID", type="tab")
grp_pid.add("kp_lon", double_t, 0, "KP for speed controller lon", 1.0, 0.0, 50.0)
grp_pid.add("ki_lon", double_t, 0, "KI for speed controller lon", 0.0, 0.0, 1.0)
grp_pid.add("ki_lon_max", double_t, 0, "KI windup for speed controller lon", 10.0, 0.0, 100.0)
grp_pid.add("kd_lon", double_t, 0, "KD for speed controller lon", 0.0, 0.0, 1.0)
grp_pid.add("ki_lat", double_t, 0, "KI for speed controller lat", 0.0, 0.0, 5.0)
grp_pid.add("ki_lat_max", double_t, 0, "KI windup for speed controller lat", 10.0, 0.0, 500.0)
grp_pid.add("kp_lat", double_t, 0, "KP for speed controller lat", 1.0, 0.0, 250.0)
grp_pid.add("kd_lat", double_t, 0, "KD for speed controller lat", 0.0, 0.0, 5.0)
grp_pid.add("kp_ang", double_t, 0, "KP for angle controller", 1.0, 0.0, 50.0)
grp_pid.add("ki_ang", double_t, 0, "KI for angle controller", 0.0, 0.0, 5.0)
grp_pid.add("ki_ang_max", double_t, 0, "KI windup for speed controller angle", 10.0, 0.0, 500.0)
grp_pid.add("kd_ang", double_t, 0, "KD for angle controller", 0.0, 0.0, 5.0)

grp_bot = gen.add_group("Robot", type="tab")
grp_bot.add("max_cmd_vel_speed", double_t, 0, "Max speed to send to the controller", 2.0, 0.0, 5.0)
grp_bot.add("max_cmd_vel_ang", double_t, 0, "Max angular speed to send to the controller", 2.0, 0.0, 5.0)
grp_bot.add("max_goal_distance_error", double_t, 0, "Wait for robot to get closer than max_goal_distance_error to the goal before setting goal as finished", 1.0, 0.0, 3.0)
grp_bot.add("max_goal_angle_error", double_t, 0, "Wait for robot to get a better angle than max_goal_angle_error before setting goal as finished", 10.0, 0.0, 360.0)
grp_bot.add("goal_timeout", double_t, 0, "Timeout for max_goal_distance_error and max_goal_angle_error", 5.0, 0.0, 100.0)
grp_bot.add("max_follow_distance", double_t, 0, "Stop controller if robot is further away than max_follow_distance (i.e. crash detection)", 1.0, 0.0, 3.0)
gen.add("speed_fast", double_t, 0, "The max speed with which the control point is moved along the path if the straight distance is larger than speed_fast_threshold", 0.5, 0, 2.0)
gen.add("speed_fast_threshold", double_t, 0, "Min straight distance in front of the robot for fast speed to be used", 1.5, 0, 5.0)
gen.add("speed_fast_threshold_angle", double_t, 0, "Max angle between current pose and pose on path to still count as straight line", 5.0, 0, 45.0)
gen.add("speed_slow", double_t, 0, "Slow speed", 0.2, 0, 2.0)
gen.add("speed_angular", double_t, 0, "The max angular speed with which the control point is moved along the path", 20.0, 1.0, 150.0)
gen.add("acceleration", double_t, 0, "Acceleration for speed transition", 1.0, 0, 10.0)


gen.add("kp_lon", double_t, 0, "KP for speed controller lon", 1.0, 0.0, 50.0)
gen.add("ki_lon", double_t, 0, "KI for speed controller lon", 0.0, 0.0, 1.0)
gen.add("ki_lon_max", double_t, 0, "KI windup for speed controller lon", 10.0, 0.0, 100.0)
gen.add("kd_lon", double_t, 0, "KD for speed controller lon", 0.0, 0.0, 1.0)
gen.add("ki_lat", double_t, 0, "KI for speed controller lat", 0.0, 0.0, 5.0)
gen.add("ki_lat_max", double_t, 0, "KI windup for speed controller lat", 10.0, 0.0, 500.0)
gen.add("kp_lat", double_t, 0, "KP for speed controller lat", 1.0, 0.0, 250.0)
gen.add("kd_lat", double_t, 0, "KD for speed controller lat", 0.0, 0.0, 5.0)
gen.add("kp_ang", double_t, 0, "KP for angle controller", 1.0, 0.0, 50.0)
gen.add("ki_ang", double_t, 0, "KI for angle controller", 0.0, 0.0, 5.0)
gen.add("ki_ang_max", double_t, 0, "KI windup for speed controller angle", 10.0, 0.0, 500.0)
gen.add("kd_ang", double_t, 0, "KD for angle controller", 0.0, 0.0, 5.0)


gen.add("max_cmd_vel_speed", double_t, 0, "Max speed to send to the controller", 2.0, 0.0, 5.0)
gen.add("max_cmd_vel_ang", double_t, 0, "Max angular speed to send to the controller", 2.0, 0.0, 5.0)
gen.add("max_goal_distance_error", double_t, 0, "Wait for robot to get closer than max_goal_distance_error to the goal before setting goal as finished", 1.0, 0.0, 3.0)
gen.add("max_goal_angle_error", double_t, 0, "Wait for robot to get a better angle than max_goal_angle_error before setting goal as finished", 10.0, 0.0, 360.0)
gen.add("goal_timeout", double_t, 0, "Timeout for max_goal_distance_error and max_goal_angle_error", 5.0, 0.0, 100.0)
gen.add("max_follow_distance", double_t, 0, "Stop controller if robot is further away than max_follow_distance (i.e. crash detection)", 1.0, 0.0, 3.0)



gen.add("forward_only", bool_t, 0, "Only allow forward driving", True)
gen.add("restore_defaults", bool_t, 0, " Load default config", False)
gen.add("debug_pid", bool_t, 0, " Emit debug_pid topic", False)

# Recovery
grp_recovery = gen.add_group("recovery", type="tab")
grp_recovery.add("oscillation_recovery", bool_t, 0,
"Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).",
True)
grp_recovery.add("oscillation_v_eps", double_t, 0, "Oscillation velocity eps", 5.0, 0.0, 100.0)
grp_recovery.add("oscillation_omega_eps", double_t, 0, "Oscillation omega eps", 5.0, 0.0, 100.0)
grp_recovery.add("oscillation_recovery_min_duration", double_t, 0, "duration until recovery in s", 5.0, 0.0, 100.0)

# Obstacles
grp_obstacles = gen.add_group("obstacles", type="tab")
grp_obstacles.add("check_obstacles", bool_t, 0, "check and stop for obstacles in path", True)
grp_obstacles.add("obstacle_lookahead", int_t, 0, "how many path segments should be used for collision check", 5,0,20)
grp_obstacles.add("obstacle_footprint", bool_t, 0, "check footprint at actual pose for collision", True)
grp_obstacles.add("debug_obstacle", bool_t, 0, "debug obstacle check as marker array", True)

exit(gen.generate("ftc_local_planner", "ftc_local_planner", "FTCPlanner"))
Loading

0 comments on commit 6796d05

Please sign in to comment.