From 235e3439ed3ba22a84fe0e3b62d48d6bb54f606b Mon Sep 17 00:00:00 2001 From: Cameron Brown Date: Wed, 23 Oct 2024 14:38:36 -0400 Subject: [PATCH] work --- .../navigator_path_planner/params.py | 2 +- NaviGator/gnc/navigator_thrust_mapper/' | 141 ------------------ .../nodes/thrust_mapper.py | 7 +- .../launch/perception/classifier.launch | 117 +++++++-------- .../navigator_missions/entrance_gate2.py | 2 +- .../src/pcodar_controller.cpp | 4 +- 6 files changed, 68 insertions(+), 205 deletions(-) delete mode 100644 NaviGator/gnc/navigator_thrust_mapper/' diff --git a/NaviGator/gnc/navigator_path_planner/navigator_path_planner/params.py b/NaviGator/gnc/navigator_path_planner/navigator_path_planner/params.py index 98b5c3161..8ed5c529b 100644 --- a/NaviGator/gnc/navigator_path_planner/navigator_path_planner/params.py +++ b/NaviGator/gnc/navigator_path_planner/navigator_path_planner/params.py @@ -43,7 +43,7 @@ # SPEED AND THRUST LIMITS -velmax_pos = np.array([2.4, 1.2, 0.22]) # (m/s, m/s, rad/s), body-frame forward +velmax_pos = np.array([1.0, 1.2, 0.18]) # (m/s, m/s, rad/s), body-frame forward velmax_neg = np.array([-1.2, -1.2, -0.22]) # (m/s, m/s, rad/s), body-frame backward thrust_max = np.array([220, 220, 220, 220]) # N, per thruster diff --git a/NaviGator/gnc/navigator_thrust_mapper/' b/NaviGator/gnc/navigator_thrust_mapper/' deleted file mode 100644 index d5c11a851..000000000 --- a/NaviGator/gnc/navigator_thrust_mapper/' +++ /dev/null @@ -1,141 +0,0 @@ -# ~/.bashrc: executed by bash(1) for non-login shells. -# see /usr/share/doc/bash/examples/startup-files (in the package bash-doc) -# for examples - -# If not running interactively, don't do anything -case $- in - *i*) ;; - *) return;; -esac - -# don't put duplicate lines or lines starting with space in the history. -# See bash(1) for more options -HISTCONTROL=ignoreboth - -# append to the history file, don't overwrite it -shopt -s histappend - -# for setting history length see HISTSIZE and HISTFILESIZE in bash(1) -HISTSIZE=1000 -HISTFILESIZE=2000 - -# check the window size after each command and, if necessary, -# update the values of LINES and COLUMNS. -shopt -s checkwinsize - -# If set, the pattern "**" used in a pathname expansion context will -# match all files and zero or more directories and subdirectories. -#shopt -s globstar - -# make less more friendly for non-text input files, see lesspipe(1) -[ -x /usr/bin/lesspipe ] && eval "$(SHELL=/bin/sh lesspipe)" - -# set variable identifying the chroot you work in (used in the prompt below) -if [ -z "${debian_chroot:-}" ] && [ -r /etc/debian_chroot ]; then - debian_chroot=$(cat /etc/debian_chroot) -fi - -# set a fancy prompt (non-color, unless we know we "want" color) -case "$TERM" in - xterm-color|*-256color) color_prompt=yes;; -esac - -# uncomment for a colored prompt, if the terminal has the capability; turned -# off by default to not distract the user: the focus in a terminal window -# should be on the output of commands, not on the prompt -#force_color_prompt=yes - -if [ -n "$force_color_prompt" ]; then - if [ -x /usr/bin/tput ] && tput setaf 1 >&/dev/null; then - # We have color support; assume it's compliant with Ecma-48 - # (ISO/IEC-6429). (Lack of such support is extremely rare, and such - # a case would tend to support setf rather than setaf.) - color_prompt=yes - else - color_prompt= - fi -fi - -if [ "$color_prompt" = yes ]; then - PS1='${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ ' -else - PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ ' -fi -unset color_prompt force_color_prompt - -# If this is an xterm set the title to user@host:dir -case "$TERM" in -xterm*|rxvt*) - PS1="\[\e]0;${debian_chroot:+($debian_chroot)}\u@\h: \w\a\]$PS1" - ;; -*) - ;; -esac - -# enable color support of ls and also add handy aliases -if [ -x /usr/bin/dircolors ]; then - test -r ~/.dircolors && eval "$(dircolors -b ~/.dircolors)" || eval "$(dircolors -b)" - alias ls='ls --color=auto' - #alias dir='dir --color=auto' - #alias vdir='vdir --color=auto' - - alias grep='grep --color=auto' - alias fgrep='fgrep --color=auto' - alias egrep='egrep --color=auto' -fi - -# colored GCC warnings and errors -#export GCC_COLORS='error=01;31:warning=01;35:note=01;36:caret=01;32:locus=01:quote=01' - -# some more ls aliases -alias ll='ls -alF' -alias la='ls -A' -alias l='ls -CF' - -# Add an "alert" alias for long running commands. Use like so: -# sleep 10; alert -alias alert='notify-send --urgency=low -i "$([ $? = 0 ] && echo terminal || echo error)" "$(history|tail -n1|sed -e '\''s/^\s*[0-9]\+\s*//;s/[;&|]\s*alert$//'\'')"' - -# Alias definitions. -# You may want to put all your additions into a separate file like -# ~/.bash_aliases, instead of adding them here directly. -# See /usr/share/doc/bash-doc/examples in the bash-doc package. - -if [ -f ~/.bash_aliases ]; then - . ~/.bash_aliases -fi - -# enable programmable completion features (you don't need to enable -# this, if it's already enabled in /etc/bash.bashrc and /etc/profile -# sources /etc/bash.bashrc). -if ! shopt -oq posix; then - if [ -f /usr/share/bash-completion/bash_completion ]; then - . /usr/share/bash-completion/bash_completion - elif [ -f /etc/bash_completion ]; then - . /etc/bash_completion - fi -fi - -# Setup enviroment for MIL developement -. /home/navigator/catkin_ws/src/mil/scripts/setup.bash - -export ROS_IP=192.168.37.80 -# export ROS_MASTER_URI=http://192.168.37.82:11311 - -export PATH=/usr/local/cuda-11.7/bin${PATH:+:${PATH}} -export LD_LIBRARY_PATH=/usr/local/cuda-11.7/lib64${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}} -export CUDA_HOME=/usr/local/cuda -export LD_LIBRARY_PATH=/usr/local/cudnn/lib:$LD_LIBRARY_PATH - -export NVM_DIR="$HOME/.nvm" -[ -s "$NVM_DIR/nvm.sh" ] && \. "$NVM_DIR/nvm.sh" # This loads nvm -[ -s "$NVM_DIR/bash_completion" ] && \. "$NVM_DIR/bash_completion" # This loads nvm bash_completion -export PATH="/home/navigator/.local/bin/lvim:${PATH}" -# export PYTHONPATH="/home/navigator/catkin_ws/src/mil/mil_common/txros/txros/src/txros:${PYTHONPATH}" - -# For quicker development, all Vim aliases lead to lvim. Use rvim for regular vim. -# alias vi="lvim" -alias vim="lvim" -alias nvim="lvim" - -# eval "$(zoxide init bash)" diff --git a/NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py b/NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py index b1c4014d1..0e7e5218a 100755 --- a/NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py +++ b/NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py @@ -108,8 +108,11 @@ def publish_thrusts(self) -> None: if len(thrusts) != len(self.publishers): rospy.logfatal("Number of thrusts does not equal number of publishers") return - for i in range(len(self.publishers)): - commands[i].setpoint = thrusts[i] + for i, pub in enumerate(self.publishers): + if pub.name == "/BL_motor/cmd": + commands[i].setpoint = thrusts[i] * 0.85 + else: + commands[i].setpoint = thrusts[i] if not self.is_vrx and not self.is_sim: for i in range(len(self.publishers)): self.joint_state_msg.effort[i] = commands[i].setpoint diff --git a/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch b/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch index f5d795b50..ded5088a6 100644 --- a/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch +++ b/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch @@ -1,68 +1,67 @@ - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - - + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - + + + + + + + diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py b/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py index 0a40c7d0b..18bf8fe6e 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py @@ -13,7 +13,7 @@ async def run(self, args): # Parameters: scan_code = False return_to_start = True - circle_radius = 5 + circle_radius = 10 circle_direction = "cw" yaw_offset = 1.57 self.traversal_distance = 3 diff --git a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp index e2d2d294c..f2e7cce05 100644 --- a/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp +++ b/mil_common/perception/point_cloud_object_detection_and_recognition/src/pcodar_controller.cpp @@ -223,14 +223,16 @@ void Node::velodyne_cb(const sensor_msgs::PointCloud2ConstPtr& pcloud) pub_pcl_.publish(filtered_accrued); // Skip object detection if all points where filtered out + // Get object clusters from persistent pointcloud if ((*filtered_accrued).empty()) ROS_WARN_ONCE("Filtered pointcloud had no points. Consider changing filter parameters."); - // Get object clusters from persistent pointcloud clusters_t clusters = detector_.get_clusters(filtered_accrued); + std::cout << "clusters were gotten" << std::endl; // Associate current clusters with old ones ass.associate(*objects_, *filtered_accrued, clusters, thrust_back); + std::cout << "associator associated" << std::endl; } bool Node::bounds_update_cb(const mil_bounds::BoundsConfig& config)