Double
/
RobotKernal-UESTC
forked from ray/RobotKernal-UESTC
1
0
Fork 0
Code Pull Requests Activity

上传PIBot ROS相关代码

LRD_Develop
詹力 2023-12-11 11:17:13 +08:00
commit 29321792b8
555 changed files with 66050 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Code/RK3588/PIBot_ROS/.gitignore vendored Normal file
View File

@ -0,0 +1,9 @@
.vscode
*.pyc
ros_ws/build
ros_ws/devel
ros_ws/install
ros_package/*
!ros_package/*.tar.gz
cartographer
ros_ws/src/CMakeLists.txt

BIN
Code/RK3588/PIBot_ROS/.pibot_init_env.sh.swp Normal file
View File

Binary file not shown.

31
Code/RK3588/PIBot_ROS/README.md Normal file
View File

@ -0,0 +1,31 @@
# PIBOT ROS Workspace v2.0
## 安装ROS
```shell
cd ~/pibot_ros/
./pibot_install_ros.sh
source ~/.bashrc
```
## init environment
for master
```shell
cd ~/pibot_ros/
./pibot_init_env.sh # select board lidar 3dsensor
source ~/.bashrc
```
for salve
```shell
cd ~/pibot_ros/
./pibot_init_env.sh # select board lidar 3dsensor
source ~/.bashrc
```
## run example
```
roslaunch pibot_bringup bringup.launch
roslaunch pibot keyboard_teleop.launch
```
then you can control your robot

15
Code/RK3588/PIBot_ROS/pibot_add_upstart.sh Normal file
View File

@ -0,0 +1,15 @@
#!/bin/bash
if [ ! -z "$PIBOT_HOME" ]; then
PIBOT_HOME_DIR=$PIBOT_HOME
else
PIBOT_HOME_DIR=~/pibot_ros
fi
sudo ln -sf $PIBOT_HOME_DIR/pibot_upstart/pibotenv /etc/pibotenv
sudo ln -sf $PIBOT_HOME_DIR/pibot_upstart/pibot_start.sh /usr/bin/pibot_start
sudo ln -sf $PIBOT_HOME_DIR/pibot_upstart/pibot_stop.sh /usr/bin/pibot_stop
sudo ln -sf $PIBOT_HOME_DIR/pibot_upstart/pibot_restart.sh /usr/bin/pibot_restart
sudo cp $PIBOT_HOME_DIR/pibot_upstart/pibot.service /etc/systemd/system/
sudo systemctl daemon-reload
sudo systemctl enable pibot
sudo systemctl is-enabled pibot

46
Code/RK3588/PIBot_ROS/pibot_ap.sh Normal file
View File

@ -0,0 +1,46 @@
#!/bin/bash
# put this cmd to startup scritps
# create pibot ap
# if [ -f /home/pibot/.pibot_ap ]; then
# create_ap wlan0 eth0 pibot_ap pibot_ap&
# else
# create_ap --fix-unmanaged
# ifconfig eth0 up
# ifconfig wlan0 up
# fi
function start() {
echo "start ap mode"
touch ~/.pibot_ap
# sudo nmcli c | awk -F' ' '{cmd="sudo nmcli c del "$1; if(NR>=2) system(cmd)}'
sudo nmcli --fields UUID con | awk '{print $1}' | while read line; do sudo nmcli con delete uuid $line; done
sudo systemctl restart create_ap
sudo systemctl enable create_ap
}
function stop() {
echo "stop ap mode"
if [ -f ~/.pibot_ap ]; then
rm ~/.pibot_ap
fi
sudo systemctl stop create_ap
sudo systemctl disable create_ap
}
case "$1" in
start )
echo "****************"
start
echo "****************"
;;
stop )
echo "****************"
stop
echo "****************"
;;
* )
echo "****************"
echo "$0 start/stop"
echo "****************"
esac

4
Code/RK3588/PIBot_ROS/pibot_clear_rosproc.sh Normal file
View File

@ -0,0 +1,4 @@
#!/bin/bash
# kill all ros&pibot process
ps -aux | grep "/opt/ros" | grep -v "grep" | awk '{print $2}' | xargs kill -9
ps -aux | grep "/pibot_ros/ros_ws" | grep -v "grep" | awk '{print $2}' | xargs kill -9

10
Code/RK3588/PIBot_ROS/pibot_create_swap.sh Normal file
View File

@ -0,0 +1,10 @@
#!/bin/bash
sudo mkdir /opt/image
cd /opt/image/
sudo touch swap
sudo dd if=/dev/zero of=/opt/image/swap bs=1024 count=2048000
sudo mkswap /opt/image/swap
sudo swapon /opt/image/swap
echo "add \"/opt/image/swap /swap swap defaults 0 0\" to \"/etc/fstab\" to make swap effecive"

362
Code/RK3588/PIBot_ROS/pibot_init_env.sh Normal file
View File

@ -0,0 +1,362 @@
#!/bin/bash
PIBOT_MODEL=
PIBOT_BOARD=
PIBOT_LIDAR=
PIBOT_3DSENSOR=
PIBOT_ADDRESS=
while getopts "m:b:l:c:a:" opt; do
case $opt in
m)
PIBOT_MODEL=$OPTARG
echo $PIBOT_MODEL;;
b)
PIBOT_BOARD=$OPTARG
echo $PIBOT_BOARD;;
l)
PIBOT_LIDAR=$OPTARG
echo $PIBOT_LIDAR;;
c)
PIBOT_3DSENSOR=$OPTARG
echo $PIBOT_3DSENSOR;;
a)
PIBOT_ADDRESS=$OPTARG
echo $PIBOT_ADDRESS;;
\?)
echo "usage: "$0 "-m {MODLE} -b {BOARD_TYPE} -L {LIDAR} -c {CAMERA} -a {ROS_iP}"
echo -e "\033[1;32m for example 1: $0 -m apollo -b stm32f1 -l rplidar -c none -a localhost\033[0m"
echo -e "\033[1;32m for example 1: $0 -m hades -b stm32f4 -l rplidar -c none -a localhost\033[0m"
exit 0 ;;
esac
done
if [ ! -z "$PIBOT_HOME" ]; then
PIBOT_HOME_DIR=$PIBOT_HOME
else
PIBOT_HOME_DIR=~/pibot_ros
fi
echo -e "\033[1;32mpibot home dir:$PIBOT_HOME_DIR"
sudo ln -sf $PIBOT_HOME_DIR/pibot_init_env.sh /usr/bin/pibot_init_env
sudo ln -sf $PIBOT_HOME_DIR/pibot_view_env.sh /usr/bin/pibot_view_env
sudo ln -sf $PIBOT_HOME_DIR/pibot_install_ros.sh /usr/bin/pibot_install_ros
if ! [ $PIBOT_ENV_INITIALIZED ]; then
echo "export PIBOT_ENV_INITIALIZED=1" >> ~/.bashrc
echo "source ~/.pibotrc" >> ~/.bashrc
fi
#rules
echo -e "\033[1;32msetup pibot modules"
echo " "
sudo rm -rf /etc/udev/rules.d/pibot.rules
sudo rm -rf /etc/udev/rules.d/rplidar.rules
sudo rm -rf /etc/udev/rules.d/ydlidar.rules
sudo cp $PIBOT_HOME_DIR/rules/98-pibot-usb.rules /etc/udev/rules.d
sudo cp $PIBOT_HOME_DIR/rules/56-orbbec-usb.rules /etc/udev/rules.d
echo " "
echo "Restarting udev"
echo ""
sudo udevadm control --reload-rules
sudo udevadm trigger
#sudo service udev reload
#sudo service udev restart
code_name=$(lsb_release -sc)
if [ "$code_name" = "trusty" ]; then
ROS_DISTRO="indigo"
elif [ "$code_name" = "xenial" ]; then
ROS_DISTRO="kinetic"
elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
ROS_DISTRO="melodic"
elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
ROS_DISTRO="noetic"
else
echo -e "\033[1;31m PIBOT not support "$code_name"\033[0m"
exit
fi
content="#source ros
if [ ! -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then
echo \"please run cd $PIBOT_HOME_DIR && ./pibot_install_ros.sh to install ros sdk\"
else
source /opt/ros/${ROS_DISTRO}/setup.bash
fi
"
echo "${content}" > ~/.pibotrc
#LOCAL_IP=`ifconfig eth0|grep "inet addr:"|awk -F":" '{print $2}'|awk '{print $1}'`
#LOCAL_IP=`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print $2}' | awk -F"/" '{print $1}'`
#if [ ! ${LOCAL_IP} ]; then
# echo "please check network"
# exit
#fi
LOCAL_IP=`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print $2}' | awk -F"/" '{print $1}'`
echo "LOCAL_IP=\`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print \$2}' | awk -F"/" '{print \$1}'\`" >> ~/.pibotrc
if [ ! ${LOCAL_IP} ]; then
echo -e "\033[1;31muse 127.0.0.1 as local ip\033[0m"
LOCAL_IP=127.0.0.1
fi
if [ "$PIBOT_MODEL" == "" ]; then
echo -e "\033[1;34mplease specify pibot model:\033[1;32m
(0: apollo(2wd-diff),
1: apolloX(2wd-diff),
2: zeus(3wd-omni),
3: hera(4wd-diff),
4: hades(4wd-mecanum),
5: hadesX(4wd-mecanum),
other: user defined)\033[1;33m"
read -p "" PIBOT_INPUT
PIBOT_MODEL_TYPE="diff"
if [ "$PIBOT_INPUT" = "0" ]; then
PIBOT_MODEL='apollo'
elif [ "$PIBOT_INPUT" = "1" ]; then
PIBOT_MODEL='apolloX'
elif [ "$PIBOT_INPUT" = "2" ]; then
PIBOT_MODEL='zeus'
PIBOT_MODEL_TYPE="omni"
elif [ "$PIBOT_INPUT" = "3" ]; then
PIBOT_MODEL='hera'
elif [ "$PIBOT_INPUT" = "4" ]; then
PIBOT_MODEL='hades'
PIBOT_MODEL_TYPE="omni"
elif [ "$PIBOT_INPUT" = "5" ]; then
PIBOT_MODEL='hadesX'
PIBOT_MODEL_TYPE="omni"
else
PIBOT_MODEL=$PIBOT_INPUT
fi
fi
if [ $PIBOT_MODEL == 'apollo' ] || [ $PIBOT_MODEL == 'apolloX' ] || [ $PIBOT_MODEL == 'hera' ]; then
PIBOT_MODEL_TYPE="diff"
else
PIBOT_MODEL_TYPE="omni"
fi
if [ "$PIBOT_BOARD" == "" ]; then
echo -e "\033[1;34mplease specify pibot driver board type:\033[1;32m
(0: arduino(mega2560),
1: stm32f103,
2: stm32f407,
other: user defined)\033[1;33m"
read -p "" PIBOT_INPUT
if [ "$PIBOT_INPUT" = "0" ]; then
PIBOT_BOARD='arduino'
elif [ "$PIBOT_INPUT" = "1" ]; then
PIBOT_BOARD='stm32f1'
elif [ "$PIBOT_INPUT" = "2" ]; then
PIBOT_BOARD='stm32f4'
else
PIBOT_BOARD=$PIBOT_INPUT
fi
fi
if [ $PIBOT_BOARD == 'arduino' ] || [ $PIBOT_BOARD == 'stm32f1' ]; then
PIBOT_DRIVER_BAUDRATE=115200
else
PIBOT_DRIVER_BAUDRATE=921600
fi
PIBOT_FAKE_LIDAR=0
if [ "$PIBOT_LIDAR" == "" ]; then
echo -e "\033[1;34mplease specify your pibot lidar:\033[1;32m
(0: not config,
1: rplidar(a1,a2),
2: rplidar(a3),
3: eai(x4),
4: eai(g4),
5: eai(tg15/tg30/tg50),
6: xtion,
7: astra,
8: kinectV1,
9: kinectV2,
10: rplidar(s1),
other: user defined)\033[1;33m"
read -p "" PIBOT_INPUT
if [ "$PIBOT_INPUT" = "0" ]; then
PIBOT_LIDAR='none'
elif [ "$PIBOT_INPUT" = "1" ]; then
PIBOT_LIDAR='rplidar'
elif [ "$PIBOT_INPUT" = "2" ]; then
PIBOT_LIDAR='rplidar-a3'
elif [ "$PIBOT_INPUT" = "3" ]; then
PIBOT_LIDAR='eai-x4'
elif [ "$PIBOT_INPUT" = "4" ]; then
PIBOT_LIDAR='eai-g4'
elif [ "$PIBOT_INPUT" = "5" ]; then
PIBOT_LIDAR='eai-tgx'
elif [ "$PIBOT_INPUT" = "6" ]; then
PIBOT_LIDAR='xtion'
PIBOT_FAKE_LIDAR=1
elif [ "$PIBOT_INPUT" = "7" ]; then
PIBOT_LIDAR='astra'
PIBOT_FAKE_LIDAR=1
elif [ "$PIBOT_INPUT" = "8" ]; then
PIBOT_LIDAR='kinectV1'
PIBOT_FAKE_LIDAR=1
elif [ "$PIBOT_INPUT" = "9" ]; then
PIBOT_LIDAR='kinectV2'
PIBOT_FAKE_LIDAR=1
elif [ "$PIBOT_INPUT" = "10" ]; then
PIBOT_LIDAR='rplidar-s1'
else
PIBOT_LIDAR=$PIBOT_INPUT
fi
fi
if [ "$PIBOT_LIDAR" = "xtion" ]; then
PIBOT_FAKE_LIDAR=1
elif [ "$PIBOT_LIDAR" = "astra" ]; then
PIBOT_FAKE_LIDAR=1
elif [ "$PIBOT_LIDAR" = "kinectV1" ]; then
PIBOT_FAKE_LIDAR=1
elif [ "$PIBOT_LIDAR" = "kinectV2" ]; then
PIBOT_FAKE_LIDAR=1
ln -s $PWD/third_party/iai_kinect2 $PWD/ros_ws/src/
else
if [ -f $PWD/ros_ws/src/iai_kinect2 ]; then
rm $PWD/ros_ws/src/iai_kinect2
fi
fi
if [ $PIBOT_FAKE_LIDAR = 1 ]; then
echo "fake lidar: $PIBOT_LIDAR"
PIBOT_3DSENSOR='none'
else
if [ "$PIBOT_3DSENSOR" == "" ]; then
echo -e "\033[1;34mplease specify your pibot 3dsensor:\033[1;32m
(0: not config,
1: xtion,
2: astra,
3: kinectV1,
4: kinectV2,
5: d435i,
other: user defined)\033[1;33m"
read -p "" PIBOT_INPUT
if [ "$PIBOT_INPUT" = "0" ]; then
PIBOT_3DSENSOR='none'
elif [ "$PIBOT_INPUT" = "1" ]; then
PIBOT_3DSENSOR='xtion'
elif [ "$PIBOT_INPUT" = "2" ]; then
PIBOT_3DSENSOR='astra'
elif [ "$PIBOT_INPUT" = "3" ]; then
PIBOT_3DSENSOR='kinectV1'
elif [ "$PIBOT_INPUT" = "4" ]; then
PIBOT_3DSENSOR='kinectV2'
elif [ "$PIBOT_INPUT" = "5" ]; then
PIBOT_3DSENSOR='d435i'
else
PIBOT_3DSENSOR=$PIBOT_INPUT
fi
fi
fi
echo "export PIBOT_HOME=${PIBOT_HOME_DIR}" >> ~/.pibotrc
echo "export ROS_IP=\`echo \$LOCAL_IP\`" >> ~/.pibotrc
echo "export ROS_HOSTNAME=\`echo \$LOCAL_IP\`" >> ~/.pibotrc
echo "export PIBOT_MODEL=${PIBOT_MODEL}" >> ~/.pibotrc
echo "export PIBOT_MODEL_TYPE=${PIBOT_MODEL_TYPE}" >> ~/.pibotrc
echo "export PIBOT_LIDAR=${PIBOT_LIDAR}" >> ~/.pibotrc
echo "export PIBOT_3DSENSOR=${PIBOT_3DSENSOR}" >> ~/.pibotrc
echo "export PIBOT_BOARD=${PIBOT_BOARD}" >> ~/.pibotrc
echo "export PIBOT_DRIVER_BAUDRATE=${PIBOT_DRIVER_BAUDRATE}" >> ~/.pibotrc
echo "export PIBOT_MAPS_DIR=~/maps" >> ~/.pibotrc
echo "export PIBOT_ASTRA_PID=0x\`lsusb | grep "2bc5:05" | awk '{print \$6}' | awk -F":" '{print \$2}'\`" >> ~/.pibotrc
if [ "$PIBOT_ADDRESS" == "" ]; then
echo -e "\033[1;34mplease specify the current machine(ip:$LOCAL_IP) type:\033[1;32m
(0: pibot board,
\033[31m1: control PC or Virtual PC\033[1;34m)\033[1;33m"
read PIBOT_INPUT
elif [ $PIBOT_ADDRESS == $LOCAL_IP ] || [ $PIBOT_ADDRESS == "localhost" ]; then
PIBOT_INPUT="0"
else
PIBOT_INPUT=$PIBOT_ADDRESS
fi
if [ "$PIBOT_INPUT" == "0" ]; then
ROS_MASTER_IP_STR="\`echo \$LOCAL_IP\`"
ROS_MASTER_IP=`echo $LOCAL_IP`
else
if [ "$PIBOT_ADDRESS" != "$LOCAL_IP" ]; then
echo -e "\033[1;34mplase specify the pibot board ip for commnication(e.g. 192.168.12.1):"
read -p "" PIBOT_INPUT
else
PIBOT_INPUT=$PIBOT_ADDRESS
fi
ROS_MASTER_IP_STR=`echo $PIBOT_INPUT`
ROS_MASTER_IP=`echo $PIBOT_INPUT`
fi
echo "export ROS_MASTER_URI=`echo http://${ROS_MASTER_IP_STR}:11311`" >> ~/.pibotrc
echo -e "\033[1;35m*****************************************************************"
echo "model: " $PIBOT_MODEL
echo "lidar: " $PIBOT_LIDAR
echo "local_ip: " ${LOCAL_IP}
echo "onboard_ip: " ${ROS_MASTER_IP}
echo ""
echo -e "please execute \033[1;36;4msource ~/.bashrc\033[1;35m to make the configure effective\033[0m"
echo -e "\033[1;35m*****************************************************************\033[0m"
#echo "source $PIBOT_HOME_DIR/ros_ws/devel/setup.bash" >> ~/.pibotrc
content="#source pibot
if [ ! -f $PIBOT_HOME_DIR/ros_ws/devel/setup.bash ]; then
echo -e \"please run \033[1;36;4mcd $PIBOT_HOME_DIR/ros_ws && catkin_make\033[0m to compile pibot sdk\"
else
source $PIBOT_HOME_DIR/ros_ws/devel/setup.bash
fi
"
echo "${content}" >> ~/.pibotrc
#alias
echo "alias pibot_bringup='roslaunch pibot_bringup bringup.launch'" >> ~/.pibotrc
echo "alias pibot_bringup_with_imu='roslaunch pibot_bringup bringup_with_imu.launch'" >> ~/.pibotrc
echo "alias pibot_lidar='roslaunch pibot_lidar ${PIBOT_LIDAR}.launch'" >> ~/.pibotrc
echo "alias pibot_base='roslaunch pibot_bringup robot.launch'" >> ~/.pibotrc
echo "alias pibot_base_with_imu='roslaunch pibot_bringup robot.launch use_imu:=true'" >> ~/.pibotrc
echo "alias pibot_control='roslaunch pibot_bringup keyboard_teleop.launch'" >> ~/.pibotrc
echo "alias pibot_joystick='roslaunch pibot_bringup joystick.launch'" >> ~/.pibotrc
echo "alias pibot_holonomic_joystick='roslaunch pibot_bringup joystick.launch holonomic:=true'" >> ~/.pibotrc
echo "alias pibot_configure='rosrun rqt_reconfigure rqt_reconfigure'" >> ~/.pibotrc
echo "alias pibot_simulator='roslaunch pibot_simulator nav.launch'" >> ~/.pibotrc
echo "alias pibot_gmapping='roslaunch pibot_navigation gmapping.launch'" >> ~/.pibotrc
echo "alias pibot_gmapping_with_imu='roslaunch pibot_navigation gmapping.launch use_imu:=true'" >> ~/.pibotrc
echo "alias pibot_save_map='roslaunch pibot_navigation save_map.launch'" >> ~/.pibotrc
echo "alias pibot_naviagtion='roslaunch pibot_navigation nav.launch'" >> ~/.pibotrc
echo "alias pibot_naviagtion_with_imu='roslaunch pibot_navigation nav.launch use_imu:=true'" >> ~/.pibotrc
echo "alias pibot_view='roslaunch pibot_navigation view_nav.launch'" >> ~/.pibotrc
echo "alias pibot_cartographer='roslaunch pibot_navigation cartographer.launch'" >> ~/.pibotrc
echo "alias pibot_cartographer_with_odom='roslaunch pibot_navigation cartographer_with_odom.launch'" >> ~/.pibotrc
echo "alias pibot_view_cartographer='roslaunch pibot_navigation view_cartographer.launch'" >> ~/.pibotrc
echo "alias pibot_hector_mapping='roslaunch pibot_navigation hector_mapping.launch'" >> ~/.pibotrc
echo "alias pibot_hector_mapping_without_imu='roslaunch pibot_navigation hector_mapping_without_odom.launch'" >> ~/.pibotrc
echo "alias pibot_karto_slam='roslaunch pibot_navigation karto_slam.launch'" >> ~/.pibotrc
echo "alias pibot_3d_mapping='roslaunch pibot_slam_3d rtabmap.launch use_imu:=false localization:=false'" >> ~/.pibotrc
echo "alias pibot_3d_mapping_with_imu='roslaunch pibot_slam_3d rtabmap.launch use_imu:=true localization:=false'" >> ~/.pibotrc
echo "alias pibot_3d_nav='roslaunch pibot_slam_3d rtabmap.launch use_imu:=false localization:=true'" >> ~/.pibotrc
echo "alias pibot_3d_nav_with_imu='roslaunch pibot_slam_3d rtabmap.launch use_imu:=true localization:=true'" >> ~/.pibotrc
echo "alias pibot_3d_view_mapping='roslaunch pibot_slam_3d view.launch localization:=true'" >> ~/.pibotrc
echo "alias pibot_3d_view_nav='roslaunch pibot_slam_3d view.launch localization:=false'" >> ~/.pibotrc

141
Code/RK3588/PIBot_ROS/pibot_install_ros.sh Normal file
View File

@ -0,0 +1,141 @@
#!/bin/bash
if [ ! -z "$PIBOT_HOME" ]; then
PIBOT_HOME_DIR=$PIBOT_HOME
else
PIBOT_HOME_DIR=~/pibot_ros
fi
echo -e "\033[1;32mpibot home dir:$PIBOT_HOME_DIR"
# http://wiki.ros.org/ROS/Installation/UbuntuMirrors
sudo sh -c 'echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" >> /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt-get update
code_name=$(lsb_release -sc)
if [ "$code_name" = "trusty" ]; then
ROS_DISTRO="indigo"
elif [ "$code_name" = "xenial" ]; then
ROS_DISTRO="kinetic"
elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
ROS_DISTRO="melodic"
elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
ROS_DISTRO="noetic"
else
echo "PIBOT not support "$code_name
exit
fi
echo "ros distro:" $ROS_DISTRO
sudo apt-get install -y git cmake unzip vim build-essential udev inetutils-ping iproute2 hostapd
sudo apt-get -y --allow-unauthenticated install ros-${ROS_DISTRO}-ros-base ros-${ROS_DISTRO}-slam-gmapping ros-${ROS_DISTRO}-navigation \
ros-${ROS_DISTRO}-xacro ros-${ROS_DISTRO}-robot-state-publisher \
ros-${ROS_DISTRO}-joint-state-publisher ros-${ROS_DISTRO}-teleop-twist-* ros-${ROS_DISTRO}-control-msgs \
ros-${ROS_DISTRO}-kdl-parser-py ros-${ROS_DISTRO}-tf2-geometry-msgs ros-${ROS_DISTRO}-hector-mapping \
ros-${ROS_DISTRO}-robot-pose-ekf ros-${ROS_DISTRO}-slam-karto ros-${ROS_DISTRO}-hector-geotiff ros-${ROS_DISTRO}-hector-trajectory-server \
ros-${ROS_DISTRO}-usb-cam ros-${ROS_DISTRO}-image-transport ros-${ROS_DISTRO}-image-transport-plugins \
ros-${ROS_DISTRO}-depthimage-to-laserscan ros-${ROS_DISTRO}-openni2* \
ros-${ROS_DISTRO}-robot-upstart ros-${ROS_DISTRO}-tf-conversions \
ros-${ROS_DISTRO}-realsense2-camera ros-${ROS_DISTRO}-libuvc* \
ros-${ROS_DISTRO}-camera-calibration ros-${ROS_DISTRO}-rtabmap* \
ros-${ROS_DISTRO}-web-video-server ros-${ROS_DISTRO}-roslint ros-${ROS_DISTRO}-laser-filters
if [ "$ROS_DISTRO" = "noetic" ]; then
sudo ln -sf /usr/bin/python3 /usr/bin/python
# third
cd $PIBOT_HOME_DIR/third_party/libuvc && mkdir -p build && cd build && cmake .. && sudo make install
cd $PIBOT_HOME_DIR
if [ ! -d $PWD/ros_package ]; then
mkdir ros_package
fi
cd ros_package
# astra ros package
if [ -f $PWD/astra.tar.gz ]; then
tar xzvf $PWD/astra.tar.gz
else
git clone https://github.com/orbbec/ros_astra_launch.git
git clone https://github.com/orbbec/ros_astra_camera.git
fi
# frontier_exploration ros package
# if [ -f $PWD/frontier_exploration.tar.gz ]; then
# tar xzvf $PWD/frontier_exploration.tar.gz
# else
# git clone -b $ROS_DISTRO-devel https://github.com/paulbovbel/frontier_exploration.git
# fi
# camera_umd
# if [ -f $PWD/camera_umd.tar.gz ]; then
# tar xzvf $PWD/camera_umd.tar.gz
# else
# git clone https://github.com/ros-drivers/camera_umd.git
# fi
cd ..
echo "ln -sf $PWD/ros_package ros_ws/src/ros_package"
if [ -f ros_ws/src/ros_package ]; then
rm ros_ws/src/ros_package
fi
ln -snf $PWD/ros_package ros_ws/src/ros_package
sudo apt-get -y --allow-unauthenticated install python3-pip python3-serial
elif [ "$ROS_DISTRO" = "melodic" ]; then
# third
cd $PIBOT_HOME_DIR/third_party/libuvc && mkdir -p build && cd build && cmake .. && sudo make install
cd $PIBOT_HOME_DIR
if [ ! -d $PWD/ros_package ]; then
mkdir ros_package
fi
cd ros_package
# astra ros package
if [ -f $PWD/astra.tar.gz ]; then
tar xzvf $PWD/astra.tar.gz
else
git clone https://github.com/orbbec/ros_astra_launch.git
git clone https://github.com/orbbec/ros_astra_camera.git
fi
# frontier_exploration ros package
# if [ -f $PWD/frontier_exploration.tar.gz ]; then
# tar xzvf $PWD/frontier_exploration.tar.gz
# else
# git clone -b $ROS_DISTRO-devel https://github.com/paulbovbel/frontier_exploration.git
# fi
cd ..
echo "ln -sf $PWD/ros_package ros_ws/src/ros_package"
if [ -f ros_ws/src/ros_package ]; then
rm ros_ws/src/ros_package
fi
ln -snf $PWD/ros_package ros_ws/src/ros_package
sudo apt-get -y --allow-unauthenticated install python-pip python-serial \
ros-${ROS_DISTRO}-freenect-* ros-${ROS_DISTRO}-orocos-kdl ros-${ROS_DISTRO}-camera-umd ros-${ROS_DISTRO}-cartographer-ros
else
sudo apt-get -y --allow-unauthenticated install python-pip python-serial \
ros-${ROS_DISTRO}-freenect-* ros-${ROS_DISTRO}-orocos-kdl ros-${ROS_DISTRO}-camera-umd ros-${ROS_DISTRO}-cartographer-ros\
ros-${ROS_DISTRO}-astra-launch ros-${ROS_DISTRO}-astra-camera ros-${ROS_DISTRO}-frontier-exploration
fi
read -s -n1 -p "install ros gui tools?(y/N)"
if [ "$REPLY" = "y" -o "$REPLY" = "Y" ]; then
sudo apt-get -y --allow-unauthenticated install ros-${ROS_DISTRO}-rviz ros-${ROS_DISTRO}-rqt-reconfigure ros-${ROS_DISTRO}-rqt-tf-tree \
ros-${ROS_DISTRO}-image-view
if [ "$ROS_DISTRO" = "noetic" ]; then
echo "please run ros_package/make_cartographer.sh to compile cartographer"
else
sudo apt-get -y --allow-unauthenticated install ros-${ROS_DISTRO}-cartographer-rviz
fi
fi

4
Code/RK3588/PIBot_ROS/pibot_remove_upstart.sh Normal file
View File

@ -0,0 +1,4 @@
#!/bin/bash
sudo systemctl disable pibot
sudo systemctl is-enabled pibot

11
Code/RK3588/PIBot_ROS/pibot_upstart/pibot.service Normal file
View File

@ -0,0 +1,11 @@
[Unit]
Description=this is pibot service
[Service]
Type=simple
ExecStart=/usr/bin/pibot_start
ExecStop=/usr/bin/pibot_stop
ExecRestart=/usr/bin/pibot_restart
[Install]
WantedBy=multi-user.target

4
Code/RK3588/PIBot_ROS/pibot_upstart/pibot_restart.sh Normal file
View File

@ -0,0 +1,4 @@
#!/bin/bash
/usr/bin/pibot-stop
sleep 2
/usr/bin/pibot-start

33
Code/RK3588/PIBot_ROS/pibot_upstart/pibot_start.sh Normal file
View File

@ -0,0 +1,33 @@
#!/bin/bash
log_file=/tmp/pibot-upstart.log
echo "$DATE: pibot-start" >> $log_file
pibotenv=/etc/pibotenv
. $pibotenv
code_name=$(lsb_release -sc)
if [ "$code_name" = "trusty" ]; then
ROS_DISTRO="indigo"
elif [ "$code_name" = "xenial" ]; then
ROS_DISTRO="kinetic"
elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
ROS_DISTRO="melodic"
elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
ROS_DISTRO="noetic"
else
echo "PIBOT not support $code_name" >> $log_file
exit
fi
echo "SYS_DIST: $code_name" >> $log_file
echo "ROS_DIST: $ROS_DISTRO" >> $log_file
echo "LOCAL_IP: $LOCAL_IP" >> $log_file
echo "ROS_MASTER_URI: $ROS_MASTER_URI" >> $log_file
echo "ROS_IP: $ROS_IP" >> $log_file
echo "HOSTNAME: $ROS_HOSTNAME" >> $log_file
echo "PIBOT_MODEL: $PIBOT_MODEL" >> $log_file
echo "PIBOT_LIDAR: $PIBOT_LIDAR" >> $log_file
echo "PIBOT_BOARD: $PIBOT_BOARD" >> $log_file
roslaunch pibot_navigation gmapping.launch

38
Code/RK3588/PIBot_ROS/pibot_upstart/pibot_stop.sh Normal file
View File

@ -0,0 +1,38 @@
#!/bin/bash
log_file=/tmp/pibot-upstart.log
echo "$DATE: pibot-stop" >> $log_file
pibotenv=/etc/pibotenv
. $pibotenv
code_name=$(lsb_release -sc)
if [ "$code_name" = "trusty" ]; then
ROS_DISTRO="indigo"
elif [ "$code_name" = "xenial" ]; then
ROS_DISTRO="kinetic"
elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
ROS_DISTRO="melodic"
elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
ROS_DISTRO="noetic"
else
echo "PIBOT not support $code_name" >> $log_file
exit
fi
LOCAL_IP=`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print $2}' | awk -F"/" '{print $1}'`
echo $LOCAL_IP
echo "SYS_DIST: $code_name" >> $log_file
echo "ROS_DIST: $ROS_DISTRO" >> $log_file
echo "LOCAL_IP: $LOCAL_IP" >> $log_file
echo "ROS_MASTER_URI: $ROS_MASTER_URI" >> $log_file
echo "ROS_IP: $ROS_IP" >> $log_file
echo "HOSTNAME: $ROS_HOSTNAME" >> $log_file
echo "PIBOT_MODEL: $PIBOT_MODEL" >> $log_file
echo "PIBOT_LIDAR: $PIBOT_LIDAR" >> $log_file
echo "PIBOT_BOARD: $PIBOT_BOARD" >> $log_file
for i in $( rosnode list ); do
rosnode kill $i;
done
killall roslaunch

22
Code/RK3588/PIBot_ROS/pibot_upstart/pibotenv Normal file
View File

@ -0,0 +1,22 @@
#source ros
source /opt/ros/kinetic/setup.bash
if [ ! -z "$PIBOT_HOME" ]; then
PIBOT_HOME_DIR=$PIBOT_HOME
else
PIBOT_HOME_DIR=/home/pibot/pibot_ros
fi
LOCAL_IP=192.168.12.1
export ROS_IP=$LOCAL_IP
export ROS_HOSTNAME=$LOCAL_IP
export PIBOT_MODEL=hades
export PIBOT_LIDAR=rplidar
export PIBOT_3DSENSOR=none
export PIBOT_BOARD=stm32f4
export PIBOT_DRIVER_BAUDRATE=921600
export ROS_MASTER_URI=http://$LOCAL_IP:11311
#source pibot
source $PIBOT_HOME_DIR/ros_ws/devel/setup.bash

31
Code/RK3588/PIBot_ROS/pibot_view_env.sh Normal file
View File

@ -0,0 +1,31 @@
#!/bin/bash
code_name=$(lsb_release -sc)
if [ "$code_name" = "trusty" ]; then
ROS_DISTRO="indigo"
elif [ "$code_name" = "xenial" ]; then
ROS_DISTRO="kinetic"
elif [ "$code_name" = "bionic" ] || [ "$code_name" = "stretch" ]; then
ROS_DISTRO="melodic"
elif [ "$code_name" = "focal" ] || [ "$code_name" = "buster" ]; then
ROS_DISTRO="noetic"
else
echo -e "\033[1;31m PIBOT not support "$code_name"\033[0m"
exit
fi
LOCAL_IP=`ip addr | grep 'state UP' -A2 | tail -n1 | awk '{print $2}' | awk -F"/" '{print $1}'`
echo -e "\033[1;33mPIBOT_ENV_INITIALIZED: "$PIBOT_ENV_INITIALIZED
echo -e "\033[1;34mSYS_DIST: "$code_name
echo -e "\033[1;33mROS_DIST: "$ROS_DISTRO
echo -e "\033[1;34mLOCAL_IP: "$LOCAL_IP
echo -e "\033[1;33mROS_MASTER_URI: "$ROS_MASTER_URI
echo -e "\033[1;34mROS_IP: "$ROS_IP
echo -e "\033[1;33mROS_HOSTNAME: "$ROS_HOSTNAME
echo -e "\033[1;34mPIBOT_MODEL: "$PIBOT_MODEL
echo -e "\033[1;33mPIBOT_LIDAR: "$PIBOT_LIDAR
echo -e "\033[1;34mPIBOT_BOARD: "$PIBOT_BOARD
echo -e "\033[1;33mPIBOT_3DSENSOR: "$PIBOT_3DSENSOR
echo -e "\033[1;35m*****************************************************************\033[0m"

17
Code/RK3588/PIBot_ROS/pypibot/pypibot/__init__.py Normal file
View File

@ -0,0 +1,17 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
import pypibot.assistant
import pypibot.log as Logger
import pypibot.err
log=Logger.log
import sys
def createNamedLog(name):
return Logger.NamedLog(name)
class Object():
pass
isDebug="-d" in sys.argv
import platform
isWindows=False
if platform.system()=='Windows':
isWindows=True

234
Code/RK3588/PIBot_ROS/pypibot/pypibot/assistant.py Normal file
View File

@ -0,0 +1,234 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
import os, sys, inspect
import datetime
import signal
import threading
import time
# function: get directory of current script, if script is built
# into an executable file, get directory of the excutable file
def current_file_directory():
path = os.path.realpath(sys.path[0]) # interpreter starter's path
if os.path.isfile(path): # starter is excutable file
path = os.path.dirname(path)
path = os.path.abspath(path) # return excutable file's directory
else: # starter is python script
caller_file = inspect.stack()[0][1] # function caller's filename
path = os.path.abspath(os.path.dirname(caller_file))# return function caller's file's directory
if path[-1]!=os.path.sep:path+=os.path.sep
return path
"""格式化字符串"""
def formatString(string,*argv):
string=string%argv
if string.find('$(scriptpath)')>=0:
string=string.replace('$(scriptpath)',current_file_directory())
if string.find('$(filenumber2)')>=0:
i=0
path=""
while True:
path=string.replace('$(scriptpath)',"%02d"%i)
if not os.path.lexists(path):break
i+=1
string=path
#8位日期（20140404）
if string.find('$(Date8)')>=0:
now=datetime.datetime.now()
string=string.replace('$(Date8)', now.strftime("%Y%m%d"))
#6位日期（140404）
if string.find('$(Date6)')>=0:
now=datetime.datetime.now()
string=string.replace('$(Date6)', now.strftime("%y%m%d"))
#6位时间（121212）
if string.find('$(Time6)')>=0:
now=datetime.datetime.now()
string=string.replace('$(Time6)', now.strftime("%H%M%S"))
#4位时间（1212）
if string.find('$(Time4)')>=0:
now=datetime.datetime.now()
string=string.replace('$(Time4)', now.strftime("%H%M"))
#文件编号2位（必须在最后）
if string.find('$(filenumber2)')>=0:
i=0
path=""
while True:
path=string.replace('$(filenumber2)',"%02d"%i)
if not os.path.lexists(path):break
i+=1
string=path
#文件编号3位（必须在最后）
if string.find('$(filenumber3)')>=0:
i=0
path=""
while True:
path=string.replace('$(filenumber3)',"%03d"%i)
if not os.path.lexists(path):break
i+=1
string=path
return string
"""
取得进程列表
格式：(PID,cmd)
"""
def getProcessList():
processList = []
try:
for line in os.popen("ps xa"):
fields = line.split()
# spid = fields[0]
pid = 0
try:pid = int(fields[0])
except:None
cmd = line[27:-1]
# print "PS:%d,%s"%(npid,process)
if pid != 0 and len(cmd) > 0:
processList.append((pid, cmd))
except Exception as e:
print("getProcessList except:%s" % (e))
return processList
def killCommand(cmd):
try:
processList = getProcessList()
for p in processList:
if p[1].find(cmd) != -1:
pid = p[0]
os.kill(pid, signal.SIGKILL)
except Exception as e:
print("killCommand ‘%s’ except:%s" % (cmd,e))
def check_pid(pid):
""" Check For the existence of a unix pid. """
if pid == 0:return False
try:
os.kill(pid, 0)
except OSError:
return False
else:
return True
SF=formatString
#全局异常捕获
def excepthook(excType, excValue, tb):
'''''write the unhandle exception to log'''
from log import log
import traceback
log.e('Unhandled Error: %s',''.join(traceback.format_exception(excType, excValue, tb)))
sys.exit(-1)
#sys.__excepthook__(type, value, trace)
#sys.__excepthook__(excType, excValue, tb)
_defaultGlobalExcept=sys.excepthook
def enableGlobalExcept(enable=True):
if enable:
sys.excepthook = excepthook
else:
sys.excepthook=_defaultGlobalExcept
# 默认启动全局异常处理
enableGlobalExcept()
#创建线程
def createThread(name,target,args=(),autoRun=True,daemon=True):
from log import log
def threadProc():
log.i("thread %s started!",name)
try:
target(*args)
log.i("thread %s ended!",name)
except Exception as e:
log.e("thread %s crash!err=%s",name,e)
thd=threading.Thread(name=name,target=threadProc)
thd.setDaemon(daemon)
if autoRun:thd.start()
return thd
#定时器
class Timer():
def __init__(self, timer_proc, args=(),first=0,period=0,name="Timer"):
self.name=name
self.first=first
self.period=period
self.args=args
self.timer_proc=timer_proc
self.thdWork=None
self.stopFlag=False
from log import NamedLog
self.log=NamedLog(name)
def run(self):
if self.first>0:
time.sleep(self.first)
try:
self.timer_proc(*self.args)
except Exception as e:
self.log.error("timer proc crash!err=%s",e)
while self.period>0 and not self.stopFlag:
time.sleep(self.period)
try:
self.timer_proc(*self.args)
except Exception as e:
self.log.error("timer proc crash!err=%s",e)
def start(self):
if self.isAlive():
self.log.d("already running!")
return True
self.stopFlag=False
self.thdWork=threading.Thread(name=self.name,target=self.run)
self.thdWork.setDaemon(True)
self.thdWork.start()
def stop(self,timeout=3):
if self.isAlive():
self.stopFlag=True
try:
self.thdWork.join(timeout)
except Exception as e:
self.log.e("stop timeout!")
def isAlive(self):
return self.thdWork and self.thdWork.isAlive()
#计时器
class Ticker():
def __init__(self):
self.reset()
# 片段,可以判断时长是否在一个特定毫秒段内
self.section=[]
def reset(self):
self.tick=time.time()
self.end=0
def stop(self):
self.end=time.time()
@property
def ticker(self):
if self.end==0:
return int((time.time()-self.tick)*1000)
else:
return int((self.end-self.tick)*1000)
def addSection(self,a,b):
a,b=int(a),int(b)
assert a<b
self.section.append((a,b))
def removeSection(self,a,b):
a,b=int(a),int(b)
self.section.remove((a,b))
def removeAllSectioin(self):
self.section=[]
def inSection(self):
tick=self.ticker
for (a,b) in self.section:
if tick>=a and tick<=b:
return True
return False
def __call__(self):
return self.ticker
def waitExit():
import log
log.log.i("start waiting to exit...")
try:
while(True):
time.sleep(1)
except Exception as e:
log.log.w("recv exit sign!")
def is_python3():
return sys.hexversion > 0x03000000

56
Code/RK3588/PIBot_ROS/pypibot/pypibot/configer.py Normal file
View File

@ -0,0 +1,56 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
import ConfigParser
from log import PLOG
import os
def getdefaultfilename():
pass
def openconfiger(filename):
return configer(filename)
class configer:
def __init__(self,fullfilepath):
self._filepath=fullfilepath
if not os.path.isdir(os.path.dirname(fullfilepath)):
os.makedirs(os.path.dirname(fullfilepath))
self._conf=ConfigParser.ConfigParser()
if os.path.isfile(fullfilepath):
try:
self._conf.readfp(open(fullfilepath,"r"))
except Exception,e:
PLOG.error("配置文件'%s'打开失败,err=%s"%(self._filepath,e))
def save(self):
try:
self._conf.write(open(self._filepath,"w"))
except Exception,e:
PLOG.error("配置文件'%s'保存失败,err=%s"%(self._filepath,e))
def changeConfValue(self,section,option,value):
if self._conf.has_section(section):
self._conf.set(section,option,value)
else:
self._conf.add_section(section)
self._conf.set(section,option,value)
def _readvalue(self,fn,section,option,default):
result=default
if self._conf.has_section(section):
if self._conf.has_option(section,option):
result=fn(section,option)
PLOG.debug("Option[%s][%s]=%s"%(section,option,str(result)))
else:
self._conf.set(section,option,str(default))
result=default
else:
self._conf.add_section(section)
self._conf.set(section,option,str(default))
result=default
return result
def getstr(self,section,option,default=""):
return self._readvalue(self._conf.get, section, option, default)
def getint(self,section,option,default=0):
return self._readvalue(self._conf.getint, section, option, default)
def getfloat(self,section,option,default=0.0):
return self._readvalue(self._conf.getfloat, section, option, default)
def getboolean(self,section,option,default=False):
return self._readvalue(self._conf.getboolean, section, option, default)

140
Code/RK3588/PIBot_ROS/pypibot/pypibot/daemon.py Normal file
View File

@ -0,0 +1,140 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys, os, time, atexit
from signal import SIGTERM
def check_pid(pid):
""" Check For the existence of a unix pid. """
if pid == 0:return False
try:
os.kill(pid, 0)
except OSError:
return False
else:
return True
class daemon:
"""
A generic daemon class.
Usage: subclass the Daemon class and override the run() method
"""
def __init__(self, pidfile, stdin='/dev/null', stdout='/dev/null', stderr='/dev/null'):
self.stdin = stdin
self.stdout = stdout
self.stderr = stderr
self.pidfile = pidfile
def daemonize(self):
"""
do the UNIX double-fork magic, see Stevens' "Advanced
Programming in the UNIX Environment" for details (ISBN 0201563177)
http://www.erlenstar.demon.co.uk/unix/faq_2.html#SEC16
"""
try:
pid = os.fork()
if pid > 0:
# exit first parent
sys.exit(0)
except OSError, e:
sys.stderr.write("fork #1 failed: %d (%s)\n" % (e.errno, e.strerror))
sys.exit(1)
# decouple from parent environment
os.chdir("/")
os.setsid()
os.umask(0)
# do second fork
try:
pid = os.fork()
if pid > 0:
# exit from second parent
sys.exit(0)
except OSError, e:
sys.stderr.write("fork #2 failed: %d (%s)\n" % (e.errno, e.strerror))
sys.exit(1)
# redirect standard file descriptors
sys.stdout.flush()
sys.stderr.flush()
si = file(self.stdin, 'r')
so = file(self.stdout, 'a+')
se = file(self.stderr, 'a+', 0)
os.dup2(si.fileno(), sys.stdin.fileno())
os.dup2(so.fileno(), sys.stdout.fileno())
os.dup2(se.fileno(), sys.stderr.fileno())
# write pidfile
atexit.register(self.delpid)
pid = str(os.getpid())
file(self.pidfile, 'w+').write("%s\n" % pid)
def delpid(self):
os.remove(self.pidfile)
def start(self):
"""
Start the daemon
"""
# Check for a pidfile to see if the daemon already runs
try:
pf = file(self.pidfile, 'r')
pid = int(pf.read().strip())
pf.close()
except IOError:
pid = None
if pid and check_pid(pid):
message = "pidfile %s already exist. Daemon already running?\n"
sys.stderr.write(message % self.pidfile)
sys.exit(1)
print("daemon start...")
# Start the daemon
self.daemonize()
self.run()
def stop(self):
"""
Stop the daemon
"""
# Get the pid from the pidfile
try:
pf = file(self.pidfile, 'r')
pid = int(pf.read().strip())
pf.close()
except IOError:
pid = None
if not pid:
message = "pidfile %s does not exist. Daemon not running?\n"
sys.stderr.write(message % self.pidfile)
return # not an error in a restart
# Try killing the daemon process
try:
while 1:
os.kill(pid, SIGTERM)
time.sleep(0.1)
except OSError, err:
err = str(err)
if err.find("No such process") > 0:
if os.path.exists(self.pidfile):
os.remove(self.pidfile)
else:
print(str(err))
sys.exit(1)
def restart(self):
"""
Restart the daemon
"""
self.stop()
self.start()
def run(self):
"""
You should override this method when you subclass Daemon. It will be called after the process has been
daemonized by start() or restart().
"""

58
Code/RK3588/PIBot_ROS/pypibot/pypibot/err.py Normal file
View File

@ -0,0 +1,58 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
# 异常类
class PibotError(Exception):
def __init__(self, errcode, errmsg):
self.errcode = errcode
self.errmsg = errmsg
#Exception.__init__(self,self.__str__())
def msg(self, msg):
if not msg is None:return PibotError(self.errcode, msg)
return PibotError(8,"unknow error")
def __str__(self):
return "PibotError:%s(%d)"%(self.errmsg,self.errcode)
@property
def message(self):
return str(self)
# 声明
# 成功
success=PibotError(0,"null")
# 通用失败
fail=PibotError(1,"fail")
# 参数无效
invalidParameter=PibotError(2,"invalid parameter")
# 不支持
noSupport=PibotError(3,"no support")
# 不存在
noExist=PibotError(4,"no exist")
# 超时
timeout=PibotError(5,"timeout")
# 繁忙
busy=PibotError(6,"busy")
# 缺少参数
missParameter=PibotError(7,"miss parameter")
# 系统错误(通用错误)
systemError=PibotError(8,"system error")
# 密码错误
invalidPassword=PibotError(9,"invalid password")
# 编码失败
encodeFailed=PibotError(10,"encode failed")
# 数据库操作失败
dbOpertationFailed=PibotError(11,"db error")
# 已占用
occupied=PibotError(12,"occupied")
# session不存在
noSession = PibotError(13,'cannot find session')
#没有找到
noFound = PibotError(14, "no found")
#已经存在
existed = PibotError(15, "existed")
#已经锁定
locked = PibotError(16, "locked")
#已经过期
expired = PibotError(17, "is expired")
#无效的参数
invalidParameter = PibotError(18, "invalid parameter")

259
Code/RK3588/PIBot_ROS/pypibot/pypibot/log.py Normal file
View File

@ -0,0 +1,259 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
import sys,os
import datetime
import threading
import pypibot.assistant as assistant
import platform
if assistant.is_python3():
import _thread
else:
import thread
import traceback
"""
%a Locale’s abbreviated weekday name.
%A Locale’s full weekday name.
%b Locale’s abbreviated month name.
%B Locale’s full month name.
%c Locale’s appropriate date and time representation.
%d Day of the month as a decimal number [01,31].
%H Hour (24-hour clock) as a decimal number [00,23].
%I Hour (12-hour clock) as a decimal number [01,12].
%j Day of the year as a decimal number [001,366].
%m Month as a decimal number [01,12].
%M Minute as a decimal number [00,59].
%p Locale’s equivalent of either AM or PM. (1)
%S Second as a decimal number [00,61]. (2)
%U Week number of the year (Sunday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Sunday are considered to be in week 0. (3)
%w Weekday as a decimal number [0(Sunday),6].
%W Week number of the year (Monday as the first day of the week) as a decimal number [00,53]. All days in a new year preceding the first Monday are considered to be in week 0. (3)
%x Locale’s appropriate date representation.
%X Locale’s appropriate time representation.
%y Year without century as a decimal number [00,99].
%Y Year with century as a decimal number.
%Z Time zone name (no characters if no time zone exists).
%% A literal '%' character.
"""
isWindows=False
if platform.system()=='Windows':
isWindows=True
defaultEncodeing="utf8"
if "-utf8" in sys.argv:
defaultEncodeing="utf-8"
if "-gbk" in sys.argv:
defaultEncodeing="gbk"
TRACE=5
DEBUG=4
INFORMATION=3
WARNING=2
ERROR=1
NONE=0
MAX_MSG_SIZE = 4096
def getLevelFromString(level):
level=level.lower()
if level=='t' or level=='trace':return 5
elif level=='d' or level=='debug':return 4
elif level=='i' or level=='info':return 3
elif level=='w' or level=='wran':return 2
elif level=='e' or level=='error':return 1
else :return 0
def getLevelString(level):
if level==TRACE:return "T"
elif level==DEBUG:return "D"
elif level==INFORMATION:return "I"
elif level==WARNING:return "W"
elif level==ERROR:return "E"
else:return "N"
class PibotLog:
def __init__(self):
self.isEnableControlLog=True
self.fileTemple=None
self.filePath=""
self.level=5
self._lock=threading.RLock()
self.fd=None
self.fd_day=-1
def setLevel(self,level):
self.level=getLevelFromString(level)
def enableControllog(self,enable):
self.isEnableControlLog=enable
def enableFileLog(self,fileName):
self.fileTemple=fileName
self.updateFilelog()
def updateFilelog(self):
fn=assistant.SF(self.fileTemple)
if fn!=self.filePath:
self.i("new log file:%s",fn)
if self.fd:
self.fd.close()
self.fd=None
self.fd_day=-1
self.filePath=""
try:
path=os.path.dirname(fn)
if path!="" and not os.path.isdir(path):os.makedirs(path)
self.fd=open(fn,mode="w")
try:
linkfn = fn.split(".log.", 1)[0] + ".INFO"
if os.path.exists(linkfn):
os.remove(linkfn)
(filepath, tempfilename) = os.path.split(fn)
os.symlink(tempfilename, linkfn)
except:
pass
self.fd_day=datetime.datetime.now().day
self.filePath=fn
except Exception as e:
print("open file fail!file=%s,err=%s"%(fn,e))
def _output(self,level,msg,args):
if self.level<level:return
try:
if len(args)>0:
t=[]
for arg in args:
if isinstance(arg,Exception):
t.append(traceback.format_exc(arg).decode('utf-8'))
elif isinstance(arg,bytes) :
t.append(arg.decode('utf-8'))
else:
t.append(arg)
args=tuple(t)
try:
msg=msg%args
except:
try:
for arg in args:
msg=msg+str(arg)+" "
except Exception as e:
msg=msg+"==LOG ERROR==>%s"%(e)
if len(msg)>MAX_MSG_SIZE:msg=msg[0:MAX_MSG_SIZE]
now=datetime.datetime.now()
msg=msg+"\r\n"
if assistant.is_python3():
id = _thread.get_ident()
else:
id = thread.get_ident()
s="[%s] %04d-%02d-%02d %02d:%02d:%02d.%03d (0x%04X):%s"%(getLevelString(level),now.year,now.month,now.day,now.hour,now.minute,now.second,now.microsecond/1000,(id>>8)&0xffff,msg)
prefix=''
suffix=''
if not isWindows:
suffix='\033[0m'
if level==TRACE:
prefix='\033[0;37m'
elif level==DEBUG:
prefix='\033[1m'
elif level==INFORMATION:
prefix='\033[0;32m'
elif level==WARNING:
prefix='\033[0;33m'
elif level==ERROR:
prefix='\033[0;31m'
else:
pass
self._lock.acquire()
try:
if self.isEnableControlLog:
sys.stdout.write((prefix+s+suffix))
if self.fd:
if self.fd_day!=now.day:
self.updateFilelog()
if assistant.is_python3():
self.fd.write(s)
else:
self.fd.write(s.encode('utf-8'))
self.fd.flush()
finally:
self._lock.release()
except Exception as e:
if assistant.is_python3():
print(e)
else:
print("PibotLog._output crash!err=%s"%traceback.format_exc(e))
def trace(self,msg,*args):
self._output(TRACE,msg,args)
def t(self,msg,*args):
self._output(TRACE,msg,args)
def debug(self,msg,*args):
self._output(DEBUG, msg,args)
def d(self,msg,*args):
self._output(DEBUG, msg,args)
def info(self,msg,*args):
self._output(INFORMATION, msg,args)
def i(self,msg,*args):
self._output(INFORMATION, msg,args)
def warn(self,msg,*args):
self._output(WARNING, msg,args)
def w(self,msg,*args):
self._output(WARNING, msg,args)
def error(self,msg,*args):
self._output(ERROR, msg,args)
def e(self,msg,*args):
self._output(ERROR, msg,args)
def _log(self,level,msg,args):
self._output(level, msg,args)
def createNamedLog(self,name):
return NamedLog(name)
log=PibotLog()
class NamedLog:
def __init__(self,name=None):
global log
self.name=''
if name:
self.name='['+name+']'
self.log=log
def trace(self,msg,*args):
msg=self.name+msg
self.log._log(TRACE,msg,args)
def t(self,msg,*args):
msg=self.name+msg
self.log._log(TRACE,msg,args)
def debug(self,msg,*args):
msg=self.name+msg
self.log._log(DEBUG, msg,args)
def d(self,msg,*args):
msg=self.name+msg
self.log._log(DEBUG, msg,args)
def info(self,msg,*args):
msg=self.name+msg
self.log._log(INFORMATION, msg,args)
def i(self,msg,*args):
msg=self.name+msg
self.log._log(INFORMATION, msg,args)
def warn(self,msg,*args):
msg=self.name+msg
self.log._log(WARNING, msg,args)
def w(self,msg,*args):
msg=self.name+msg
self.log._log(WARNING, msg,args)
def error(self,msg,*args):
msg=self.name+msg
self.log._log(ERROR, msg,args)
def e(self,msg,*args):
msg=self.name+msg
self.log._log(ERROR, msg,args)
if __name__ == "__main__":
log.trace("1%s","hello")
log.debug("2%d",12)
try:
raise Exception("EXC")
except Exception as e:
log.info("3%s",e)
log.warn("1")
log.error("1")
#log.enableFileLog("$(scriptpath)test_$(Date8)_$(filenumber2).log")
log.trace("1")
log.debug("1")
log.info("1")
log.warn("1")
log.error("1")
log=NamedLog("test")
log.d("1")
log.i("1")
log.warn("1")
log.error("1=%d,%s",100,e)

35
Code/RK3588/PIBot_ROS/pypibot/pypibot/mapconvert.py Normal file
View File

@ -0,0 +1,35 @@
import cv2
import numpy as np
def map_server_to_cv2(data):
# print(data.info.height, data.info.width)
data = np.reshape(data.data, (data.info.height, data.info.width))
data = np.flipud(data)
# -1 --> 205
# 0 --> 255
# 100--> 0
image = np.where(data == -1, 205, 255 - (255.0 * data / 100)).astype(np.uint8)
#cv2.imshow("cv2_map", data)
return image
def cv2_to_map_server(image):
image = np.flipud(image)
# 205 --> -1
# 255 --> 0
# 0 --> 100
data = np.where(image == 205, -1, (255 - image) * 100.0 / 255).astype(np.int8).flatten()
return list(data)
# pub_data = OccupancyGrid()
# pub_data.header.frame_id = "map"
# pub_data.header.stamp = rospy.Time.now()
# pub_data.info = map_data.info
# pub_data.data = list(data)
# print(len(pub_data.data), pub_data.info.height * pub_data.info.width)
# map_publisher.publish(pub_data)

38
Code/RK3588/PIBot_ROS/pypibot/pypibot/proxy.py Normal file
View File

@ -0,0 +1,38 @@
from pypibot import log
import threading
import zmq
class MqProxy:
def __init__(self, sub_addr, pub_addr):
self.thd = None
self.sub_addr = sub_addr
self.pub_addr = pub_addr
def _run(self):
context = zmq.Context()
frontend = context.socket(zmq.XSUB)
frontend.bind(self.sub_addr)
# frontend.bind("tcp://*:5556")
backend = context.socket(zmq.XPUB)
backend.bind(self.pub_addr)
# backend.bind("tcp://*:5557")
try:
zmq.proxy(frontend, backend)
except KeyboardInterrupt:
pass
frontend.close()
backend.close()
context.term()
def start(self):
if self.thd is None:
log.i("mq proxy starting...")
self.thd=threading.Thread(target=self._run, name="proxy")
self.thd.setDaemon(True)
self.thd.start()
def stop(self):
pass

240
Code/RK3588/PIBot_ROS/pypibot/transport/dataholder.py Normal file
View File

@ -0,0 +1,240 @@
import struct
params_size=29
# main board
class MessageID:
ID_GET_VERSION = 0
ID_SET_ROBOT_PARAMETER = 1
ID_GET_ROBOT_PARAMETER = 2
ID_INIT_ODOM = 3
ID_SET_VEL = 4
ID_GET_ODOM = 5
ID_GET_PID_DEBUG = 6
ID_GET_IMU = 7
ID_GET_ENCODER_COUNT = 8
ID_SET_MOTOR_PWM = 9
class RobotMessage:
def pack(self):
return b''
def unpack(self):
return True
class RobotFirmwareInfo(RobotMessage):
def __init__(self):
self.version = ''
self.build_time = ''
def unpack(self, data):
try:
upk = struct.unpack('16s16s', bytes(data))
except:
return False
[self.version, self.build_time] = upk
return True
class RobotImuType:
IMU_TYPE_GY65 = 49
IMU_TYPE_GY85 = 69
IMU_TYPE_GY87 = 71
class RobotModelType:
MODEL_TYPE_2WD_DIFF = 1
MODEL_TYPE_4WD_DIFF = 2
MODEL_TYPE_3WD_OMNI = 101
MODEL_TYPE_4WD_OMNI = 102
MODEL_TYPE_4WD_MECANUM = 201
class RobotParameters():
def __init__(self, wheel_diameter=0, \
wheel_track=0, \
encoder_resolution=0, \
do_pid_interval=0, \
kp=0, \
ki=0, \
kd=0, \
ko=0, \
cmd_last_time=0, \
max_v_liner_x=0, \
max_v_liner_y=0, \
max_v_angular_z=0, \
imu_type=0, \
motor_ratio=0, \
model_type=0, \
motor_nonexchange_flag=255, \
encoder_nonexchange_flag=255, \
):
self.wheel_diameter = wheel_diameter
self.wheel_track = wheel_track
self.encoder_resolution = encoder_resolution
self.do_pid_interval = do_pid_interval
self.kp = kp
self.ki = ki
self.kd = kd
self.ko = ko
self.cmd_last_time = cmd_last_time
self.max_v_liner_x = max_v_liner_x
self.max_v_liner_y = max_v_liner_y
self.max_v_angular_z = max_v_angular_z
self.imu_type = imu_type
self.motor_ratio = motor_ratio
self.model_type = model_type
self.motor_nonexchange_flag = motor_nonexchange_flag
self.encoder_nonexchange_flag = encoder_nonexchange_flag
reserve=b'\xff'
self.reserve=b''
for i in range(64-params_size):
self.reserve+=reserve
robotParam = RobotParameters()
class GetRobotParameters(RobotMessage):
def __init__(self):
self.param = robotParam
def unpack(self, data):
#print(bytes(data), len(bytes(data)))
upk = struct.unpack('<3H1B8H1B1H3B%ds'%(64-params_size), bytes(data))
[self.param.wheel_diameter,
self.param.wheel_track,
self.param.encoder_resolution,
self.param.do_pid_interval,
self.param.kp,
self.param.ki,
self.param.kd,
self.param.ko,
self.param.cmd_last_time,
self.param.max_v_liner_x,
self.param.max_v_liner_y,
self.param.max_v_angular_z,
self.param.imu_type,
self.param.motor_ratio,
self.param.model_type,
self.param.motor_nonexchange_flag,
self.param.encoder_nonexchange_flag,
self.param.reserve] = upk
return True
class SetRobotParameters(RobotMessage):
def __init__(self):
self.param = robotParam
def pack(self):
data = [self.param.wheel_diameter,
self.param.wheel_track,
self.param.encoder_resolution,
self.param.do_pid_interval,
self.param.kp,
self.param.ki,
self.param.kd,
self.param.ko,
self.param.cmd_last_time,
self.param.max_v_liner_x,
self.param.max_v_liner_y,
self.param.max_v_angular_z,
self.param.imu_type,
self.param.motor_ratio,
self.param.model_type,
self.param.motor_nonexchange_flag,
self.param.encoder_nonexchange_flag,
self.param.reserve]
print(data)
pk = struct.pack('<3H1B8H1B1H3B%ds'%(64-(3*2+1+8*2+1+2+3)), *data)
return pk
def unpack(self, data):
return True
class RobotVel(RobotMessage):
def __init__(self):
self.v_liner_x = 0
self.v_liner_y = 0
self.v_angular_z = 0
def pack(self):
data = [self.v_liner_x,
self.v_liner_y,
self.v_angular_z]
pk = struct.pack('3h', *data)
return pk
def unpack(self, data):
return True
#todo the rest of the message classes
class RobotOdom(RobotMessage):
def __init__(self):
self.v_liner_x = 0
self.v_liner_y = 0
self.v_angular_z = 0
self.x = 0
self.y = 0
self.yaw = 0
def unpack(self, data):
try:
upk = struct.unpack('<3H2l1H', bytes(data))
except:
return False
[self.v_liner_x,
self.v_liner_y,
self.v_angular_z,
self.x,
self.y,
self.yaw] = upk
return True
class RobotPIDData(RobotMessage):
pass
class RobotIMU(RobotMessage):
def __init__(self):
self.imu = [0]*9
def unpack(self, data):
try:
upk = struct.unpack('<9f', bytes(data))
except:
return False
self.imu = upk
return True
class RobotEncoderCount(RobotMessage):
def __init__(self):
self.encoder = [0]*4
def unpack(self, data):
try:
upk = struct.unpack('<4f', bytes(data))
except:
return False
self.encoder = upk
return True
class RobotMotorPWM(RobotMessage):
def __init__(self):
self.pwm = [0]*4
def pack(self):
pk = struct.pack('4h', *self.pwm)
return pk
def unpack(self, data):
return True
BoardDataDict = {MessageID.ID_GET_VERSION:RobotFirmwareInfo(),
MessageID.ID_GET_ROBOT_PARAMETER:GetRobotParameters(),
MessageID.ID_SET_ROBOT_PARAMETER:SetRobotParameters(),
MessageID.ID_SET_VEL:RobotVel(),
MessageID.ID_GET_ODOM:RobotOdom(),
MessageID.ID_GET_PID_DEBUG: RobotPIDData(),
MessageID.ID_GET_IMU: RobotIMU(),
MessageID.ID_GET_ENCODER_COUNT: RobotEncoderCount(),
MessageID.ID_SET_MOTOR_PWM: RobotMotorPWM(),
}

115
Code/RK3588/PIBot_ROS/pypibot/transport/main.py Normal file
View File

@ -0,0 +1,115 @@
import platform
import sys
sys.path.append("..")
import pypibot
from pypibot import log
from transport import Transport
from dataholder import MessageID
import params
import time
import signal
#for linux
port="/dev/pibot"
#for windows
#port="com3"
pypibot.assistant.enableGlobalExcept()
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
log.setLevel("i")
run_flag = True
def exit(signum, frame):
global run_flag
run_flag = False
if __name__ == '__main__':
signal.signal(signal.SIGINT, exit)
mboard = Transport(port, params.pibotBaud)
if not mboard.start():
log.error("can not open %s"%port)
sys.exit()
DataHolder = mboard.getDataHolder()
for num in range(0,3):
log.info("****************get robot version*****************")
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
p = mboard.request(MessageID.ID_GET_VERSION)
if p:
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
break
else:
log.error('read firmware version err\r\n')
import time
time.sleep(1)
if num == 2:
log.error('please check connection or baudrate\r\n')
sys.exit()
# get robot parameter
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
if p:
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
%(robotParam.param.model_type, \
robotParam.param.wheel_diameter, \
robotParam.param.wheel_track, \
robotParam.param.encoder_resolution
))
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
%(robotParam.param.do_pid_interval, \
robotParam.param.kp, \
robotParam.param.ki, \
robotParam.param.kd, \
robotParam.param.ko))
log.info("cmd_last_time:%d imu_type:%d" \
%(robotParam.param.cmd_last_time,\
robotParam.param.imu_type
))
log.info("max_v:%d %d %d\r\n" \
%(robotParam.param.max_v_liner_x,\
robotParam.param.max_v_liner_y, \
robotParam.param.max_v_angular_z
))
log.info("motor flag:%d encoder flag: %d\r\n" \
%(robotParam.param.motor_nonexchange_flag,\
robotParam.param.encoder_nonexchange_flag
))
else:
log.error('get params err\r\n')
quit(1)
log.info("****************get odom&imu*****************")
while run_flag:
robotOdom = DataHolder[MessageID.ID_GET_ODOM]
p = mboard.request(MessageID.ID_GET_ODOM)
if p:
log.info('request get odom success, vx=%d vy=%d vangular=%d, x=%d y=%d yaw=%d'%(robotOdom.v_liner_x, \
robotOdom.v_liner_y, \
robotOdom.v_angular_z, \
robotOdom.x, \
robotOdom.y, \
robotOdom.yaw))
else:
log.error('get odom err')
quit(1)
robotIMU = DataHolder[MessageID.ID_GET_IMU].imu
p = mboard.request(MessageID.ID_GET_IMU)
if p:
log.info('get imu success, imu=[%f %f %f %f %f %f %f %f %f]'%(robotIMU[0], robotIMU[1], robotIMU[2], \
robotIMU[3], robotIMU[4], robotIMU[5], \
robotIMU[6], robotIMU[7], robotIMU[8]))
else:
log.error('get imu err')
quit(1)
time.sleep(0.1)

75
Code/RK3588/PIBot_ROS/pypibot/transport/params.py Normal file
View File

@ -0,0 +1,75 @@
import dataholder
import os
from dataholder import RobotImuType
from dataholder import RobotModelType
pibotModel = os.environ['PIBOT_MODEL']
boardType = os.environ['PIBOT_BOARD']
pibotBaud = os.environ['PIBOT_DRIVER_BAUDRATE']
print(pibotModel)
print(boardType)
print(pibotBaud)
pibotParam = dataholder.RobotParameters()
if pibotModel == "apollo" and boardType == "arduino":
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
75, 2500, 0, 10, \
250, 40, 0, 200, \
RobotImuType.IMU_TYPE_GY85, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "apollo" and boardType == "stm32f1":
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
320, 2700, 0, 10, \
250, 50, 0, 200, \
RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "apollo" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(65, 175, 44, 10, \
320, 2700, 0, 10, \
250, 40, 0, 200, \
RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "zeus" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(58, 230, 44, 10, \
320, 2700, 0, 10, \
250, 50, 50, 250, \
RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_3WD_OMNI)
elif pibotModel == "hades" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(76, 470, 44, 10, \
320, 2700, 0, 10, \
250, 50, 50, 250, \
RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_4WD_MECANUM)
elif pibotModel == "hadesX" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(150, 565, 44, 10, \
250, 2750, 0, 10, \
250, 50, 50, 250, \
RobotImuType.IMU_TYPE_GY87, 72, \
RobotModelType.MODEL_TYPE_4WD_MECANUM)
elif pibotModel == "hera" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(82, 338, 44, 10, \
320, 2700, 0, 10, \
250, 50, 50, 250, \
RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_4WD_DIFF)
elif pibotModel == "apolloX" and boardType == "arduino":
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
75, 2500, 0, 10, \
250, 40, 0, 200, \
RobotImuType.IMU_TYPE_GY85, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "apolloX" and boardType == "stm32f1":
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
250, 1200, 0, 10, \
250, 50, 0, 200, \
RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF)
elif pibotModel == "apolloX" and boardType == "stm32f4":
pibotParam = dataholder.RobotParameters(96, 350, 68, 10, \
250, 1200, 0, 10, \
250, 50, 0, 200, \
RobotImuType.IMU_TYPE_GY87, 90, \
RobotModelType.MODEL_TYPE_2WD_DIFF)

90
Code/RK3588/PIBot_ROS/pypibot/transport/set_default_params.py Normal file
View File

@ -0,0 +1,90 @@
import platform
import sys
sys.path.append("..")
import pypibot
from pypibot import log
from transport import Transport
from dataholder import MessageID
import params
#for linux
port="/dev/pibot"
#for windows
#port="com3"
pypibot.assistant.enableGlobalExcept()
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
log.setLevel("i")
if __name__ == '__main__':
mboard = Transport(port, params.pibotBaud)
if not mboard.start():
log.error("can not open %s"%port)
sys.exit()
DataHolder = mboard.getDataHolder()
for num in range(0,3):
log.info("****************get robot version*****************")
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
p = mboard.request(MessageID.ID_GET_VERSION)
if p:
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
break
else:
log.error('read firmware version err\r\n')
import time
time.sleep(1)
if num == 2:
log.error('please check connection or baudrate\r\n')
sys.exit()
# set robot parameter
log.info("****************set robot parameter*****************")
DataHolder[MessageID.ID_SET_ROBOT_PARAMETER].param = params.pibotParam
p = mboard.request(MessageID.ID_SET_ROBOT_PARAMETER)
if p:
log.info('set parameter success')
else:
log.error('set parameter err')
quit(1)
# get robot parameter
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
if p:
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
%(robotParam.param.model_type, \
robotParam.param.wheel_diameter, \
robotParam.param.wheel_track, \
robotParam.param.encoder_resolution
))
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
%(robotParam.param.do_pid_interval, \
robotParam.param.kp, \
robotParam.param.ki, \
robotParam.param.kd, \
robotParam.param.ko))
log.info("cmd_last_time:%d imu_type:%d" \
%(robotParam.param.cmd_last_time,\
robotParam.param.imu_type
))
log.info("max_v:%d %d %d\r\n" \
%(robotParam.param.max_v_liner_x,\
robotParam.param.max_v_liner_y, \
robotParam.param.max_v_angular_z
))
log.info("motor flag:%d encoder flag: %d\r\n" \
%(robotParam.param.motor_nonexchange_flag,\
robotParam.param.encoder_nonexchange_flag
))
else:
log.error('get param err\r\n')
quit(1)

122
Code/RK3588/PIBot_ROS/pypibot/transport/test_motors.py Normal file
View File

@ -0,0 +1,122 @@
import platform
import sys
sys.path.append("..")
import pypibot
from pypibot import log
from transport import Transport
from dataholder import MessageID
import params
import signal
#for linux
port="/dev/pibot"
#for windows
#port="com3"
pypibot.assistant.enableGlobalExcept()
#log.enableFileLog(log_dir + "ros_$(Date8)_$(filenumber2).log")
log.setLevel("i")
run_flag = True
def exit(signum, frame):
global run_flag
run_flag = False
if __name__ == '__main__':
signal.signal(signal.SIGINT, exit)
mboard = Transport(port, params.pibotBaud)
if not mboard.start():
log.error("can not open %s"%port)
sys.exit()
pwm=[0]*4
for i in range(len(sys.argv)-1):
pwm[i] = int(sys.argv[i+1])
DataHolder = mboard.getDataHolder()
for num in range(0,3):
log.info("****************get robot version*****************")
boardVersion = DataHolder[MessageID.ID_GET_VERSION]
p = mboard.request(MessageID.ID_GET_VERSION)
if p:
log.info("firmware version:%s buildtime:%s\r\n"%(boardVersion.version.decode(), boardVersion.build_time.decode()))
break
else:
log.error('read firmware version err\r\n')
import time
time.sleep(1)
if num == 2:
log.error('please check connection or baudrate\r\n')
sys.exit()
# get robot parameter
robotParam = DataHolder[MessageID.ID_GET_ROBOT_PARAMETER]
p = mboard.request(MessageID.ID_GET_ROBOT_PARAMETER)
if p:
log.info("model_type:%d wheel_diameter:%d wheel_track:%d encoder_resolution:%d" \
%(robotParam.param.model_type, \
robotParam.param.wheel_diameter, \
robotParam.param.wheel_track, \
robotParam.param.encoder_resolution
))
log.info("do_pid_interval:%d kp:%d ki:%d kd:%d ko:%d" \
%(robotParam.param.do_pid_interval, \
robotParam.param.kp, \
robotParam.param.ki, \
robotParam.param.kd, \
robotParam.param.ko))
log.info("cmd_last_time:%d imu_type:%d" \
%(robotParam.param.cmd_last_time,\
robotParam.param.imu_type
))
log.info("max_v:%d %d %d\r\n" \
%(robotParam.param.max_v_liner_x,\
robotParam.param.max_v_liner_y, \
robotParam.param.max_v_angular_z
))
log.info("motor flag:%d encoder flag: %d\r\n" \
%(robotParam.param.motor_nonexchange_flag,\
robotParam.param.encoder_nonexchange_flag
))
else:
log.error('get params err\r\n')
quit(1)
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = pwm
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
if p:
log.info('set pwm success')
else:
log.error('set pwm err')
quit(1)
log.info("****************get encoder count*****************")
while run_flag:
robotEncoder = DataHolder[MessageID.ID_GET_ENCODER_COUNT].encoder
p = mboard.request(MessageID.ID_GET_ENCODER_COUNT)
if p:
log.info('encoder count: %s'%(('\t\t').join([str(x) for x in robotEncoder])))
else:
log.error('get encoder count err')
quit(1)
import time
time.sleep(0.5)
DataHolder[MessageID.ID_SET_MOTOR_PWM].pwm = [0]*4
p = mboard.request(MessageID.ID_SET_MOTOR_PWM)
if p:
log.info('set pwm success')
else:
log.error('set pwm err')
quit(1)

197
Code/RK3588/PIBot_ROS/pypibot/transport/transport.py Normal file
View File

@ -0,0 +1,197 @@
import sys
sys.path.append("..")
import pypibot
from pypibot import log
from pypibot import assistant
import serial
import threading
import struct
import time
from dataholder import MessageID, BoardDataDict
FIX_HEAD = 0x5a
class Recstate():
WAITING_HD = 0
WAITING_MSG_ID = 1
RECEIVE_LEN = 2
RECEIVE_PACKAGE = 3
RECEIVE_CHECK = 4
def checksum(d):
sum = 0
if assistant.is_python3():
for i in d:
sum += i
sum = sum&0xff
else:
for i in d:
sum += ord(i)
sum = sum&0xff
return sum
class Transport:
def __init__(self, port, baudrate=921600):
self._Port = port
self._Baudrate = baudrate
self._KeepRunning = False
self.receive_state = Recstate.WAITING_HD
self.rev_msg = []
self.rev_data = []
self.wait_event = threading.Event()
def getDataHolder(self):
return BoardDataDict
def start(self):
try:
self._Serial = serial.Serial(port=self._Port, baudrate=self._Baudrate, timeout=0.2)
self._KeepRunning = True
self._ReceiverThread = threading.Thread(target=self._Listen)
self._ReceiverThread.setDaemon(True)
self._ReceiverThread.start()
return True
except:
return False
def Stop(self):
self._KeepRunning = False
time.sleep(0.1)
self._Serial.close()
def _Listen(self):
while self._KeepRunning:
if self.receiveFiniteStates(self._Serial.read()):
self.packageAnalysis()
def receiveFiniteStates(self, s):
if len(s) == 0:
return False
val = s[0]
val_int = val
if not assistant.is_python3():
val_int = ord(val)
if self.receive_state == Recstate.WAITING_HD:
if val_int == FIX_HEAD:
log.trace('got head')
self.rev_msg = []
self.rev_data =[]
self.rev_msg.append(val)
self.receive_state = Recstate.WAITING_MSG_ID
elif self.receive_state == Recstate.WAITING_MSG_ID:
log.trace('got msg id')
self.rev_msg.append(val)
self.receive_state = Recstate.RECEIVE_LEN
elif self.receive_state == Recstate.RECEIVE_LEN:
log.trace('got len:%d', val_int)
self.rev_msg.append(val)
if val_int == 0:
self.receive_state = Recstate.RECEIVE_CHECK
else:
self.receive_state = Recstate.RECEIVE_PACKAGE
elif self.receive_state == Recstate.RECEIVE_PACKAGE:
# if assistant.is_python3():
# self.rev_data.append((chr(val)).encode('latin1'))
# else:
self.rev_data.append(val)
r = False
if assistant.is_python3():
r = len(self.rev_data) == int(self.rev_msg[-1])
else:
r = len(self.rev_data) == ord(self.rev_msg[-1])
if r:
self.rev_msg.extend(self.rev_data)
self.receive_state = Recstate.RECEIVE_CHECK
elif self.receive_state == Recstate.RECEIVE_CHECK:
log.trace('got check')
self.receive_state = Recstate.WAITING_HD
if val_int == checksum(self.rev_msg):
log.trace('got a complete message')
return True
else:
self.receive_state = Recstate.WAITING_HD
# continue receiving
return False
def packageAnalysis(self):
if assistant.is_python3():
in_msg_id = int(self.rev_msg[1])
else:
in_msg_id = ord(self.rev_msg[1])
if assistant.is_python3():
log.debug("recv body:" + " ".join("{:02x}".format(c) for c in self.rev_data))
r = BoardDataDict[in_msg_id].unpack(bytes(self.rev_data))
else:
log.debug("recv body:" + " ".join("{:02x}".format(ord(c)) for c in self.rev_data))
r = BoardDataDict[in_msg_id].unpack(''.join(self.rev_data))
if r:
self.res_id = in_msg_id
if in_msg_id<100:
self.set_response()
else:#notify
log.debug('msg %d'%self.rev_msg[4],'data incoming')
pass
else:
log.debug ('error unpacking pkg')
def request(self, id, timeout=0.5):
if not self.write(id):
log.debug ('Serial send error!')
return False
if self.wait_for_response(timeout):
if id == self.res_id:
log.trace ('OK')
else:
log.error ('Got unmatched response!')
else:
log.error ('Request got no response!')
return False
# clear response
self.res_id = None
return True
def write(self, id):
cmd = self.make_command(id)
if assistant.is_python3():
log.d("write:" + " ".join("{:02x}".format(c) for c in cmd))
else:
log.d("write:" + " ".join("{:02x}".format(ord(c)) for c in cmd))
self._Serial.write(cmd)
return True
def wait_for_response(self, timeout):
self.wait_event.clear()
return self.wait_event.wait(timeout)
def set_response(self):
self.wait_event.set()
def make_command(self, id):
#print(DataDict[id])
data = BoardDataDict[id].pack()
l = [FIX_HEAD, id, len(data)]
head = struct.pack("3B", *l)
body = head + data
if assistant.is_python3():
return body + chr(checksum(body)).encode('latin1')
else:
return body + chr(checksum(body))
if __name__ == '__main__':
mboard = Transport('com10')
if not mboard.start():
import sys
sys.exit()
p = mboard.request(MessageID.ID_GET_VERSION)
log.i("result=%s"%p)
print('Version =[',mboard.getDataHolder()[MessageID.ID_GET_VERSION].version.decode(), mboard.getDataHolder()[MessageID.ID_GET_VERSION].build_time.decode(),"]\r\n")

BIN
Code/RK3588/PIBot_ROS/ros_package/abseil-cpp.tar.gz Normal file
View File

Binary file not shown.

BIN
Code/RK3588/PIBot_ROS/ros_package/astra.tar.gz Normal file
View File

Binary file not shown.

BIN
Code/RK3588/PIBot_ROS/ros_package/cartographer.tar.gz Normal file
View File

Binary file not shown.

BIN
Code/RK3588/PIBot_ROS/ros_package/cartographer_ros.tar.gz Normal file
View File

Binary file not shown.

BIN
Code/RK3588/PIBot_ROS/ros_package/ceres-solver.tar.gz Normal file
View File

Binary file not shown.

BIN
Code/RK3588/PIBot_ROS/ros_package/frontier_exploration.tar.gz Normal file
View File

Binary file not shown.

BIN
Code/RK3588/PIBot_ROS/ros_package/protobuf.tar.gz Normal file
View File

Binary file not shown.

1
Code/RK3588/PIBot_ROS/ros_ws/.catkin_workspace Normal file
View File

@ -0,0 +1 @@
# This file currently only serves to mark the location of a catkin workspace for tool integration

1
Code/RK3588/PIBot_ROS/ros_ws/src/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
ros_package

5
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/.gitignore vendored Normal file
View File

@ -0,0 +1,5 @@
*.pyc
.project
.cproject
.pydevproject
.vscode

25
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/LICENSE Normal file
View File

@ -0,0 +1,25 @@
Copyright (c) 2008-2011 Vanadium Labs LLC.
All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

3
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/README.md Normal file
View File

@ -0,0 +1,3 @@
## Arbotix Drivers
This repository contains the Arbotix ROS drivers, catkinized, and ready for ROS Noetic.

30
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix/CHANGELOG.rst Normal file
View File

@ -0,0 +1,30 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package arbotix
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.11.0 (2020-12-29)
-------------------
0.10.0 (2014-07-14)
-------------------
0.9.2 (2014-02-12)
------------------
0.9.1 (2014-01-28)
------------------
0.9.0 (2013-08-22)
------------------
0.8.2 (2013-03-28)
------------------
* update metapackage
0.8.1 (2013-03-09)
------------------
0.8.0 (2013-02-21)
------------------
* fix package.xml
* add metapackage for arbotix

4
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix/CMakeLists.txt Normal file
View File

@ -0,0 +1,4 @@
cmake_minimum_required(VERSION 3.0.2)
project(arbotix)
find_package(catkin REQUIRED)
catkin_metapackage()

23
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix/package.xml Normal file
View File

@ -0,0 +1,23 @@
<package>
<name>arbotix</name>
<version>0.11.0</version>
<description>ArbotiX Drivers</description>
<author>Michael Ferguson</author>
<maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/arbotix</url>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>arbotix_msgs</run_depend>
<run_depend>arbotix_python</run_depend>
<run_depend>arbotix_sensors</run_depend>
<run_depend>arbotix_controllers</run_depend>
<run_depend>arbotix_firmware</run_depend>
<export>
<metapackage/>
</export>
</package>

42
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_controllers/CHANGELOG.rst Normal file
View File

@ -0,0 +1,42 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package arbotix_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.11.0 (2020-12-29)
-------------------
* Update all python shebangs to python3 + rosdep dependency (`#48 <https://github.com/vanadiumlabs/arbotix_ros/issues/48>`_)
Co-authored-by: Murat Calis <mc@pirate-robotics.net>
* Merge pull request `#22 <https://github.com/vanadiumlabs/arbotix_ros/issues/22>`_ from corot/indigo-devel
roslib.load_manifest should not be used on catkin packages
* roslib.load_manifest should not be used on catkin packages according to http://wiki.ros.org/PyStyleGuide
* Contributors: Jorge Santos, Michael Ferguson, calismurat
0.10.0 (2014-07-14)
-------------------
* Set queue_size=5 on all publishers
* Check if command exceeds opening limits
* Contributors: Jorge Santos
0.9.2 (2014-02-12)
------------------
* cleanup gripper controllers, mark deprecations
* Contributors: Michael Ferguson
0.9.1 (2014-01-28)
------------------
0.9.0 (2013-08-22)
------------------
* fix joint_states subscriber
* fix name of singlesided model
* add new gripper action controller
0.8.2 (2013-03-28)
------------------
0.8.1 (2013-03-09)
------------------
0.8.0 (2013-02-21)
------------------
* import drivers and catkinize

14
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_controllers/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 3.0.2)
project(arbotix_controllers)
find_package(catkin REQUIRED)
catkin_package()
install(
PROGRAMS
bin/gripper_controller
bin/one_side_gripper_controller.py
bin/parallel_gripper_controller.py
bin/parallel_single_servo_controller.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

211
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_controllers/bin/gripper_controller Normal file
View File

@ -0,0 +1,211 @@
#!/usr/bin/env python
"""
gripper_controller - action based controller for grippers.
Copyright (c) 2011-2014 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy, actionlib
import thread
from control_msgs.msg import GripperCommandAction
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64
from math import asin
class TrapezoidGripperModel:
""" A simple gripper with two opposing servos to open/close non-parallel jaws. """
def __init__(self):
# trapezoid model: base width connecting each gripper's rotation point
# + length of gripper fingers to computation point
# = compute angles based on a desired width at comp. point
self.pad_width = rospy.get_param('~pad_width', 0.01)
self.finger_length = rospy.get_param('~finger_length', 0.02)
self.min_opening = rospy.get_param('~min_opening', 0.0)
self.max_opening = rospy.get_param('~max_opening', 0.09)
self.center_l = rospy.get_param('~center_left', 0.0)
self.center_r = rospy.get_param('~center_right', 0.0)
self.invert_l = rospy.get_param('~invert_left', False)
self.invert_r = rospy.get_param('~invert_right', False)
self.left_joint = rospy.get_param('~joint_left', 'l_gripper_joint')
self.right_joint = rospy.get_param('~joint_right', 'r_gripper_joint')
# publishers
self.l_pub = rospy.Publisher(self.left_joint+'/command', Float64, queue_size=5)
self.r_pub = rospy.Publisher(self.right_joint+'/command', Float64, queue_size=5)
def setCommand(self, command):
# check limits
if command.position > self.max_opening or command.position < self.min_opening:
rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
command.position, self.max_opening, self.min_opening)
return False
# compute angles
angle = asin((command.position - self.pad_width)/(2*self.finger_length))
if self.invert_l:
l = -angle + self.center_l
else:
l = angle + self.center_l
if self.invert_r:
r = angle + self.center_r
else:
r = -angle + self.center_r
# publish msgs
lmsg = Float64(l)
rmsg = Float64(r)
self.l_pub.publish(lmsg)
self.r_pub.publish(rmsg)
return True
def getPosition(self, js):
left = right = 0
for i in range(len(js.name)):
if js.name[i] == self.left_joint:
left = js.position[i]
elif js.name[i] == self.right_joint:
right = js.position[i]
# TODO
return 0.0
def getEffort(self, joint_states):
return 1.0
class ParallelGripperModel:
""" One servo to open/close parallel jaws, typically via linkage. """
def __init__(self):
self.center = rospy.get_param('~center', 0.0)
self.scale = rospy.get_param('~scale', 1.0)
self.joint = rospy.get_param('~joint', 'gripper_joint')
# publishers
self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
def setCommand(self, command):
self.pub.publish((command.position * self.scale) + self.center)
def getPosition(self, joint_states):
return 0.0
def getEffort(self, joint_states):
return 1.0
class OneSideGripperModel:
""" Simplest of grippers, one servo opens or closes to achieve a particular size opening. """
def __init__(self):
self.pad_width = rospy.get_param('~pad_width', 0.01)
self.finger_length = rospy.get_param('~finger_length', 0.02)
self.min_opening = rospy.get_param('~min_opening', 0.0)
self.max_opening = rospy.get_param('~max_opening', 0.09)
self.center = rospy.get_param('~center', 0.0)
self.invert = rospy.get_param('~invert', False)
self.joint = rospy.get_param('~joint', 'gripper_joint')
# publishers
self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
def setCommand(self, command):
""" Take an input command of width to open gripper. """
# check limits
if command.position > self.max_opening or command.position < self.min_opening:
rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
command.position, self.max_opening, self.min_opening)
return False
# compute angle
angle = asin((command.position - self.pad_width)/(2*self.finger_length))
# publish message
if self.invert:
self.pub.publish(-angle + self.center)
else:
self.pub.publish(angle + self.center)
def getPosition(self, joint_states):
# TODO
return 0.0
def getEffort(self, joint_states):
# TODO
return 1.0
class GripperActionController:
""" The actual action callbacks. """
def __init__(self):
rospy.init_node('gripper_controller')
# setup model
try:
model = rospy.get_param('~model')
except:
rospy.logerr('no model specified, exiting')
exit()
if model == 'dualservo':
self.model = TrapezoidGripperModel()
elif model == 'parallel':
self.model = ParallelGripperModel()
elif model == 'singlesided':
self.model = OneSideGripperModel()
else:
rospy.logerr('unknown model specified, exiting')
exit()
# subscribe to joint_states
rospy.Subscriber('joint_states', JointState, self.stateCb)
# subscribe to command and then spin
self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
self.server.start()
rospy.spin()
def actionCb(self, goal):
""" Take an input command of width to open gripper. """
rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
# send command to gripper
self.model.setCommand(goal.command)
# publish feedback
while True:
if self.server.is_preempt_requested():
self.server.set_preemtped()
rospy.loginfo('Gripper Controller: Preempted.')
return
# TODO: get joint position, break when we have reached goal
break
self.server.set_succeeded()
rospy.loginfo('Gripper Controller: Succeeded.')
def stateCb(self, msg):
self.state = msg
if __name__=='__main__':
try:
GripperActionController()
except rospy.ROSInterruptException:
rospy.loginfo('Hasta la Vista...')

212
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_controllers/bin/gripper_controller.py Normal file
View File

@ -0,0 +1,212 @@
#!/usr/bin/env python
"""
gripper_controller - action based controller for grippers.
Copyright (c) 2011-2014 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy, actionlib
import thread
from control_msgs.msg import GripperCommandAction
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64
from math import asin
class TrapezoidGripperModel:
""" A simple gripper with two opposing servos to open/close non-parallel jaws. """
def __init__(self):
# trapezoid model: base width connecting each gripper's rotation point
# + length of gripper fingers to computation point
# = compute angles based on a desired width at comp. point
self.pad_width = rospy.get_param('~pad_width', 0.01)
self.finger_length = rospy.get_param('~finger_length', 0.02)
self.min_opening = rospy.get_param('~min_opening', 0.0)
self.max_opening = rospy.get_param('~max_opening', 0.09)
self.center_l = rospy.get_param('~center_left', 0.0)
self.center_r = rospy.get_param('~center_right', 0.0)
self.invert_l = rospy.get_param('~invert_left', False)
self.invert_r = rospy.get_param('~invert_right', False)
self.left_joint = rospy.get_param('~joint_left', 'l_gripper_joint')
self.right_joint = rospy.get_param('~joint_right', 'r_gripper_joint')
# publishers
self.l_pub = rospy.Publisher(self.left_joint+'/command', Float64, queue_size=5)
self.r_pub = rospy.Publisher(self.right_joint+'/command', Float64, queue_size=5)
def setCommand(self, command):
# check limits
if command.position > self.max_opening or command.position < self.min_opening:
rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
command.position, self.max_opening, self.min_opening)
return False
# compute angles
angle = asin((command.position - self.pad_width)/(2*self.finger_length))
if self.invert_l:
l = -angle + self.center_l
else:
l = angle + self.center_l
if self.invert_r:
r = angle + self.center_r
else:
r = -angle + self.center_r
# publish msgs
lmsg = Float64(l)
rmsg = Float64(r)
self.l_pub.publish(lmsg)
self.r_pub.publish(rmsg)
return True
def getPosition(self, js):
left = right = 0
for i in range(len(js.name)):
if js.name[i] == self.left_joint:
left = js.position[i]
elif js.name[i] == self.right_joint:
right = js.position[i]
# TODO
return 0.0
def getEffort(self, joint_states):
return 1.0
class ParallelGripperModel:
""" One servo to open/close parallel jaws, typically via linkage. """
def __init__(self):
self.center = rospy.get_param('~center', 0.0)
self.scale = rospy.get_param('~scale', 1.0)
self.joint = rospy.get_param('~joint', 'gripper_joint')
# publishers
self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
def setCommand(self, command):
self.pub.publish((command.position * self.scale) + self.center)
def getPosition(self, joint_states):
return 0.0
def getEffort(self, joint_states):
return 1.0
class OneSideGripperModel:
""" Simplest of grippers, one servo opens or closes to achieve a particular size opening. """
def __init__(self):
self.pad_width = rospy.get_param('~pad_width', 0.01)
self.finger_length = rospy.get_param('~finger_length', 0.02)
self.min_opening = rospy.get_param('~min_opening', 0.0)
self.max_opening = rospy.get_param('~max_opening', 0.09)
self.center = rospy.get_param('~center', 0.0)
self.invert = rospy.get_param('~invert', False)
self.joint = rospy.get_param('~joint', 'gripper_joint')
# publishers
self.pub = rospy.Publisher(self.joint+'/command', Float64, queue_size=5)
def setCommand(self, command):
""" Take an input command of width to open gripper. """
# check limits
if command.position > self.max_opening or command.position < self.min_opening:
rospy.logerr("Command (%f) exceeds opening limits (%f, %f)",
command.position, self.max_opening, self.min_opening)
return False
# compute angle
angle = asin((command.position - self.pad_width)/(2*self.finger_length))
# publish message
if self.invert:
self.pub.publish(-angle + self.center)
else:
self.pub.publish(angle + self.center)
def getPosition(self, joint_states):
# TODO
return 0.0
def getEffort(self, joint_states):
# TODO
return 1.0
class GripperActionController:
""" The actual action callbacks. """
def __init__(self):
rospy.init_node('gripper_controller')
# setup model
try:
model = rospy.get_param('~model')
except:
rospy.logerr('no model specified, exiting')
exit()
if model == 'dualservo':
self.model = TrapezoidGripperModel()
elif model == 'parallel':
self.model = ParallelGripperModel()
elif model == 'singlesided':
self.model = OneSideGripperModel()
else:
rospy.logerr('unknown model specified, exiting')
exit()
# subscribe to joint_states
rospy.Subscriber('joint_states', JointState, self.stateCb)
# subscribe to command and then spin
self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
self.server.start()
rospy.spin()
def actionCb(self, goal):
""" Take an input command of width to open gripper. """
rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
# send command to gripper
self.model.setCommand(goal.command)
# publish feedback
while True:
if self.server.is_preempt_requested():
self.server.set_preemtped()
rospy.loginfo('Gripper Controller: Preempted.')
return
# TODO: get joint position, break when we have reached goal
break
self.server.set_succeeded()
rospy.loginfo('Gripper Controller: Succeeded.')
def stateCb(self, msg):
self.state = msg
if __name__=='__main__':
try:
rospy.logwarn("Please use gripper_controller (no .py)")
GripperActionController()
except rospy.ROSInterruptException:
rospy.loginfo('Hasta la Vista...')

74
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_controllers/bin/one_side_gripper_controller.py Normal file
View File

@ -0,0 +1,74 @@
#!/usr/bin/env python
"""
one_side_gripper_controller.py - controls a gripper built with one servo
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
import thread
from std_msgs.msg import Float64
from math import asin
class OneSideGripperController:
""" A simple controller that operates a servos to
open/close to a particular size opening. """
def __init__(self):
rospy.init_node("one_side_gripper_controller")
rospy.logwarn("one_side_gripper_controller.py is deprecated and will be removed in ROS Indigo, please use gripper_controller")
self.pad_width = rospy.get_param("~pad_width", 0.01)
self.finger_length = rospy.get_param("~finger_length", 0.02)
self.center = rospy.get_param("~center", 0.0)
self.invert = rospy.get_param("~invert", False)
# publishers
self.pub = rospy.Publisher("gripper_joint/command", Float64, queue_size=5)
# subscribe to command and then spin
rospy.Subscriber("~command", Float64, self.commandCb)
rospy.spin()
def commandCb(self, msg):
""" Take an input command of width to open gripper. """
# check limits
#if msg.data > self.max_opening or msg.data < self.min_opening:
# rospy.logerr("Command exceeds limits.")
# return
# compute angle
angle = asin((msg.data - self.pad_width)/(2*self.finger_length))
# publish message
if self.invert:
self.pub.publish(-angle + self.center)
else:
self.pub.publish(angle + self.center)
if __name__=="__main__":
try:
OneSideGripperController()
except rospy.ROSInterruptException:
rospy.loginfo("Hasta la Vista...")

99
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_action_controller.py Normal file
View File

@ -0,0 +1,99 @@
#!/usr/bin/env python
"""
parallel_gripper_controller.py - controls a gripper built of two servos
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy, actionlib
import thread
from control_msgs.msg import GripperCommandAction
from std_msgs.msg import Float64
from math import asin
class ParallelGripperActionController:
""" A simple controller that operates two opposing servos to
open/close to a particular size opening. """
def __init__(self):
rospy.init_node('gripper_controller')
rospy.logwarn("parallel_gripper_action_controller.py is deprecated and will be removed in ROS Indigo, please use gripper_controller")
# trapezoid model: base width connecting each gripper's rotation point
# + length of gripper fingers to computation point
# = compute angles based on a desired width at comp. point
self.pad_width = rospy.get_param('~pad_width', 0.01)
self.finger_length = rospy.get_param('~finger_length', 0.02)
self.min_opening = rospy.get_param('~min', 0.0)
self.max_opening = rospy.get_param('~max', 2*self.finger_length)
self.center_l = rospy.get_param('~center_left', 0.0)
self.center_r = rospy.get_param('~center_right', 0.0)
self.invert_l = rospy.get_param('~invert_left', False)
self.invert_r = rospy.get_param('~invert_right', False)
# publishers
self.l_pub = rospy.Publisher('l_gripper_joint/command', Float64, queue_size=5)
self.r_pub = rospy.Publisher('r_gripper_joint/command', Float64, queue_size=5)
# subscribe to command and then spin
self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False)
self.server.start()
rospy.spin()
def actionCb(self, goal):
""" Take an input command of width to open gripper. """
rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position)
command = goal.command.position
# check limits
if command > self.max_opening:
command = self.max_opening
if command < self.min_opening:
command = self.min_opening
# compute angles
angle = asin((command - self.pad_width)/(2*self.finger_length))
if self.invert_l:
l = -angle + self.center_l
else:
l = angle + self.center_l
if self.invert_r:
r = angle + self.center_r
else:
r = -angle + self.center_r
# publish msgs
lmsg = Float64(l)
rmsg = Float64(r)
self.l_pub.publish(lmsg)
self.r_pub.publish(rmsg)
rospy.sleep(5.0)
self.server.set_succeeded()
rospy.loginfo('Gripper Controller: Done.')
if __name__=='__main__':
try:
ParallelGripperActionController()
except rospy.ROSInterruptException:
rospy.loginfo('Hasta la Vista...')

91
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_gripper_controller.py Normal file
View File

@ -0,0 +1,91 @@
#!/usr/bin/env python
"""
parallel_gripper_controller.py - controls a gripper built of two servos
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
import thread
from std_msgs.msg import Float64
from math import asin
class ParallelGripperController:
""" A simple controller that operates two opposing servos to
open/close to a particular size opening. """
def __init__(self):
rospy.init_node("parallel_gripper_controller")
rospy.logwarn("parallel_gripper_controller.py is deprecated and will be removed in ROS Indigo, please use gripper_controller")
# trapezoid model: base width connecting each gripper's rotation point
# + length of gripper fingers to computation point
# = compute angles based on a desired width at comp. point
self.pad_width = rospy.get_param("~pad_width", 0.01)
self.finger_length = rospy.get_param("~finger_length", 0.02)
self.min_opening = rospy.get_param("~min", 0.0)
self.max_opening = rospy.get_param("~max", 2*self.finger_length)
self.center_l = rospy.get_param("~center_left", 0.0)
self.center_r = rospy.get_param("~center_right", 0.0)
self.invert_l = rospy.get_param("~invert_left", False)
self.invert_r = rospy.get_param("~invert_right", False)
# publishers
self.l_pub = rospy.Publisher("l_gripper_joint/command", Float64, queue_size=5)
self.r_pub = rospy.Publisher("r_gripper_joint/command", Float64, queue_size=5)
# subscribe to command and then spin
rospy.Subscriber("~command", Float64, self.commandCb)
rospy.spin()
def commandCb(self, msg):
""" Take an input command of width to open gripper. """
# check limits
if msg.data > self.max_opening or msg.data < self.min_opening:
rospy.logerr("Command exceeds limits.")
return
# compute angles
angle = asin((msg.data - self.pad_width)/(2*self.finger_length))
if self.invert_l:
l = -angle + self.center_l
else:
l = angle + self.center_l
if self.invert_r:
r = angle + self.center_r
else:
r = -angle + self.center_r
# publish msgs
lmsg = Float64(l)
rmsg = Float64(r)
self.l_pub.publish(lmsg)
self.r_pub.publish(rmsg)
if __name__=="__main__":
try:
ParallelGripperController()
except rospy.ROSInterruptException:
rospy.loginfo("Hasta la Vista...")

133
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_controllers/bin/parallel_single_servo_controller.py Normal file
View File

@ -0,0 +1,133 @@
#!/usr/bin/env python
"""
parallel_single_servo_controller.py - controls a single-servo parallel-jaw gripper
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy, tf
import thread
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from math import asin
class ParallelGripperController:
""" A simple controller that operates a single servo parallel jaw gripper. """
def __init__(self):
rospy.init_node("gripper_controller")
# TODO: load calibration data. Form: opening->servo angle
self.calib = { 0.0000 : 1.8097, 0.0159: 1.2167, 0.0254 : 0.8997, 0.0381 : 0.4499, 0.042 : 0.1943 }
#self.calib = { 0.0000 : 866, 0.0159: 750, 0.0254 : 688, 0.0381 : 600, 0.042 : 550 }
# parameters
self.min = rospy.get_param("~min", 0.0)
self.max = rospy.get_param("~max", 0.042)
self.center = rospy.get_param("~center", 512)
self.invert = rospy.get_param("~invert", False)
# publishers
self.commandPub = rospy.Publisher("gripper_joint/command", Float64, queue_size=5)
self.br = tf.TransformBroadcaster()
# current width of gripper
self.width = 0.0
# subscribe to command and then spin
rospy.Subscriber("~command", Float64, self.commandCb)
rospy.Subscriber("joint_states", JointState, self.stateCb)
r = rospy.Rate(15)
while not rospy.is_shutdown():
# output tf
self.br.sendTransform((0, -self.width/2.0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"gripper_left_link",
"gripper_link")
self.br.sendTransform((0, self.width/2.0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"gripper_right_link",
"gripper_link")
r.sleep()
def getCommand(self, width):
""" Get servo command for an opening width. """
keys = self.calib.keys(); keys.sort()
# find end points of segment
low = keys[0];
high = keys[-1]
for w in keys[1:-1]:
if w > low and w < width:
low = w
if w < high and w > width:
high = w
# linear interpolation
scale = (width-low)/(high-low)
return ((self.calib[high]-self.calib[low])*scale) + self.calib[low]
def getWidth(self, command):
""" Get opening width for a particular servo command. """
reverse_calib = dict()
for k, v in self.calib.items():
reverse_calib[v] = k
keys = reverse_calib.keys(); keys.sort()
# find end points of segment
low = keys[0]
high = keys[-1]
for c in keys[1:-1]:
if c > low and c < command:
low = c
if c < high and c > command:
high = c
# linear interpolation
scale = (command-low)/(high-low)
return ((reverse_calib[high]-reverse_calib[low])*scale) + reverse_calib[low]
def commandCb(self, msg):
""" Take an input command of width to open gripper. """
# check limits
if msg.data > self.max or msg.data < self.min:
rospy.logerr("Command exceeds limits.")
return
# compute angle
self.commandPub.publish( Float64( self.getCommand(msg.data) ) )
def stateCb(self, msg):
""" The callback that listens for joint_states. """
try:
index = msg.name.index("gripper_joint")
except ValueError:
return
self.width = self.getWidth(msg.position[index])
if __name__=="__main__":
try:
ParallelGripperController()
except rospy.ROSInterruptException:
rospy.loginfo("Hasta la Vista...")

17
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_controllers/package.xml Normal file
View File

@ -0,0 +1,17 @@
<package>
<name>arbotix_controllers</name>
<version>0.11.0</version>
<description>
Extends the arbotix_python package with a number of more sophisticated ROS wrappers for common devices.
</description>
<author>Michael Ferguson</author>
<maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/arbotix_controllers</url>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>arbotix_python</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>tf</run_depend>
</package>

28
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_firmware/CHANGELOG.rst Normal file
View File

@ -0,0 +1,28 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package arbotix_firmware
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.11.0 (2020-12-29)
-------------------
0.10.0 (2014-07-14)
-------------------
0.9.2 (2014-02-12)
------------------
0.9.1 (2014-01-28)
------------------
0.9.0 (2013-08-22)
------------------
0.8.2 (2013-03-28)
------------------
0.8.1 (2013-03-09)
------------------
0.8.0 (2013-02-21)
------------------
* import drivers and catkinize

5
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_firmware/CMakeLists.txt Normal file
View File

@ -0,0 +1,5 @@
cmake_minimum_required(VERSION 3.0.2)
project(arbotix_firmware)
find_package(catkin REQUIRED)
catkin_package()

13
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_firmware/package.xml Normal file
View File

@ -0,0 +1,13 @@
<package>
<name>arbotix_firmware</name>
<version>0.11.0</version>
<description>
Firmware source code for ArbotiX ROS bindings.
</description>
<author>Michael Ferguson</author>
<maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/arbotix_firmware</url>
<buildtool_depend>catkin</buildtool_depend>
</package>

148
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_firmware/src/diff_controller.h Normal file
View File

@ -0,0 +1,148 @@
/*
ArbotiX Firmware for ROS driver
Copyright (c) 2009-2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <Arduino.h>
#include <math.h>
/* Register Storage */
int left_pwm;
int right_pwm;
int left_speed;
int right_speed;
/* PID Parameters */
int Kp;
int Kd;
int Ki;
int Ko; // output scale
int maxAccel; // maximum acceleration per frame (ticks)
/* PID modes */
unsigned int PIDmode;
#define PID_NONE 0
#define PID_SPEED 1
#define FRAME_RATE 33 // frame rate in millis (30Hz)
#define FRAMES 30
unsigned long f_time; // last frame
unsigned char moving = 0; // base in motion
unsigned char paused = 0; // base was in motion, can resume
#define MAXOUTPUT 255 // motor PWM
/* Setpoint Info For a Motor */
typedef struct{
int Velocity; // desired actual speed (count/frame)
long Encoder; // actual reading
long PrevEnc; // last reading
int PrevErr;
int Ierror;
int output; // last motor setting
} SetPointInfo;
SetPointInfo left, right;
/* Initialize PID parameters to something known */
void setupPID(){
// Default values for the PR-MINI
Kp = 25;
Kd = 30;
Ki = 0;
Ko = 100;
maxAccel = 50;
f_time = 0;
}
/* PID control of motor speed */
void DoPid(SetPointInfo * p){
long Perror;
long output;
Perror = p->Velocity - (p->Encoder-p->PrevEnc);
// Derivative error is the delta Perror
output = (Kp*Perror + Kd*(Perror - p->PrevErr) + Ki*p->Ierror)/Ko;
p->PrevErr = Perror;
p->PrevEnc = p->Encoder;
output += p->output;
// Accumulate Integral error *or* Limit output.
// Stop accumulating when output saturates
if (output >= MAXOUTPUT)
output = MAXOUTPUT;
else if (output <= -MAXOUTPUT)
output = -MAXOUTPUT;
else
p->Ierror += Perror;
p->output = output;
}
/* Clear accumulators */
void ClearPID(){
PIDmode = 0; moving = 0;
left.PrevErr = 0;
left.Ierror = 0;
left.output = 0;
right.PrevErr = 0;
right.Ierror = 0;
right.output = 0;
}
/* This is called by the main loop, does a X HZ PID loop. */
void updatePID(){
if((moving > 0) && (PIDmode > 0)){ // otherwise, just return
unsigned long j = millis();
if(j > f_time){
// update encoders
left.Encoder = Encoders.left;
right.Encoder = Encoders.right;
// do PID update on PWM
DoPid(&left);
DoPid(&right);
// set updated motor outputs
if(PIDmode > 0){
drive.set(left.output, right.output);
}
// update timing
f_time = j + FRAME_RATE;
}
}
}
void clearAll(){
PIDmode = 0;
left.Encoder = 0;
right.Encoder = 0;
left.PrevEnc = 0;
right.PrevEnc = 0;
left.output = 0;
right.output = 0;
Encoders.Reset();
}

76
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_firmware/src/ros.h Normal file
View File

@ -0,0 +1,76 @@
/*
Common Definitions for ROS driver ArbotiX Firmware
Copyright (c) 2008-2012 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/* ArbotiX (id:253) Instruction Definitions */
#define ARB_SIZE_POSE 7 // pose size: a single param for size of pose
#define ARB_LOAD_POSE 8 // load pose: index, then pose positions (# of params = 2*pose_size)
#define ARB_LOAD_SEQ 9 // seq size: a single param for the size of the seq
#define ARB_PLAY_SEQ 10 // load seq: index/times (# of params = 3*seq_size)
#define ARB_LOOP_SEQ 11 // play seq: no params
//#define ARB_TEST 25 // hardware test: no params
#define ARB_CONTROL_SETUP 26 // write ids: id of controller, params (usually ids of servos, # of params = pose_size + 1)
#define ARB_CONTROL_WRITE 27 // write positions: positions in order of servos (# of params = 2*pose_size)
#define ARB_CONTROL_STAT 28 // retrieve status: id of controller
#define ARB_SYNC_READ 0x84
/* ArbotiX (id:253) Register Table Definitions */
#define REG_MODEL_NUMBER_L 0
#define REG_MODEL_NUMBER_H 1
#define REG_VERSION 2
#define REG_ID 3
#define REG_BAUD_RATE 4
#define REG_DIGITAL_IN0 5 // First block of digital pins to read
#define REG_DIGITAL_IN1 6
#define REG_DIGITAL_IN2 7
#define REG_DIGITAL_IN3 8
#define REG_RESCAN 15
#define REG_RETURN_LEVEL 16
#define REG_ALARM_LED 17
#define REG_ANA_BASE 18 // First Analog Port
#define REG_SERVO_BASE 26 // Up to 10 servos, each uses 2 bytes (L, then H), pulse width (0, 1000-2000ms)
#define REG_MOVING 46
#define REG_DIGITAL_OUT0 47 // First digital pin to write
// base + index, bit 1 = value (0,1), bit 0 = direction (0,1)
#define REG_RESERVED 79 // 79 -- 99 are reserved for future use
#define REG_USER 100 //
/* Packet Decoding */
int mode = 0; // where we are in the frame
unsigned char id = 0; // id of this frame
unsigned char length = 0; // length of this frame
unsigned char ins = 0; // instruction of this frame
unsigned char params[143]; // parameters (match RX-64 buffer size)
unsigned char index = 0; // index in param buffer
int checksum; // checksum

548
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_firmware/src/ros.ino Normal file
View File

@ -0,0 +1,548 @@
/*
ArbotiX Firmware for ROS driver
Copyright (c) 2008-2012 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/* Build Configuration */
#define USE_BASE // Enable support for a mobile base
#define USE_HW_SERVOS // Enable only 2/8 servos, but using hardware control
#define CONTROLLER_COUNT 5
/* Hardware Constructs */
#include <ax12.h>
#include <BioloidController.h>
BioloidController controllers[CONTROLLER_COUNT];
#include "ros.h"
#ifdef USE_HW_SERVOS
#include <HServo.h>
HServo servos[2];
int servo_vals[2]; // in millis
#else
#include <Servo.h>
Servo servos[10];
int servo_vals[10]; // in millis
#endif
#ifdef USE_BASE
#include <Motors2.h>
Motors2 drive = Motors2();
#include <EncodersAB.h>
#include "diff_controller.h"
#endif
/* Register Storage */
unsigned char baud = 7; // ?
unsigned char ret_level = 1; // ?
unsigned char alarm_led = 0; // ?
/* Pose & Sequence Structures */
typedef struct{
unsigned char pose; // index of pose to transition to
int time; // time for transition
} sp_trans_t;
int poses[30][AX12_MAX_SERVOS]; // poses [index][servo_id-1]
sp_trans_t sequence[50]; // sequence
int seqPos; // step in current sequence
#include "user_hooks.h"
/*
* Setup Functions
*/
void scan(){
#if defined(AX_RX_SWITCHED)
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++){
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}
}
#endif
}
void setup(){
Serial.begin(115200);
ax12Init(1000000);
#ifdef USE_BASE
drive.init();
Encoders.Begin();
setupPID();
#endif
#if defined(AX_RX_SWITCHED)
delay(1000);
scan();
#endif
userSetup();
pinMode(0,OUTPUT); // status LED
}
/*
* Handle Write Requests to ArbotiX Registers
*/
unsigned char handleWrite(){
int addr = params[0]; // address to write
int bytes = length-3; // # of bytes left to write
int k = 1; // index in parameters of value to write
while(bytes > 0){
if(addr < REG_BAUD_RATE){
return ERR_INSTRUCTION;
}else if(addr == REG_BAUD_RATE){
UBRR1L = params[k];
}else if(addr < REG_RESCAN){
return ERR_INSTRUCTION; // can't write digital inputs
}else if(addr == REG_RESCAN){
scan();
}else if(addr == REG_RETURN_LEVEL){
ret_level = params[k];
}else if(addr == REG_ALARM_LED){
// TODO:
}else if(addr < REG_SERVO_BASE){
return ERR_INSTRUCTION; // error - analog are read only
}else if(addr < REG_MOVING){
// write servo
int s = addr - REG_SERVO_BASE;
#ifdef USE_HW_SERVO
if( s >= 4 ){
#else
if( s >= 20){
#endif
return ERR_INSTRUCTION;
}else{
if( s%2 == 0 ){ // low byte
s = s/2;
servo_vals[s] = params[k];
}else{ // high byte
s = s/2;
servo_vals[s] += (params[k]<<8);
if(servo_vals[s] > 500 && servo_vals[s] < 2500){
servos[s].writeMicroseconds(servo_vals[s]);
if(!servos[s].attached())
servos[s].attach(s);
}else if(servo_vals[s] == 0){
servos[s].detach();
}
}
}
}else if(addr == REG_MOVING){
return ERR_INSTRUCTION;
}else if(addr < REG_RESERVED){
// write digital pin
int pin = addr - REG_DIGITAL_OUT0;
#ifdef SERVO_STIK
if(pin < 8)
pin = pin+24;
else
pin = pin+5; // servo stick 8 = D13...
#endif
if(params[k] & 0x02) // high
digitalWrite(pin, HIGH);
else
digitalWrite(pin, LOW);
if(params[k] & 0x01) // output
pinMode(pin, OUTPUT);
else
pinMode(pin, INPUT);
}else{
int ret = userWrite(addr, params[k]);
if(ret > ERR_NONE) return ret;
}
addr++;k++;bytes--;
}
return ERR_NONE;
}
/*
* Handle a read from ArbotiX registers.
*/
int handleRead(){
int checksum = 0;
int addr = params[0];
int bytes = params[1];
unsigned char v;
while(bytes > 0){
if(addr == REG_MODEL_NUMBER_L){
v = 44;
}else if(addr == REG_MODEL_NUMBER_H){
v = 1; // 300
}else if(addr == REG_VERSION){
v = 0;
}else if(addr == REG_ID){
v = 253;
}else if(addr == REG_BAUD_RATE){
v = 34; // 56700
}else if(addr == REG_DIGITAL_IN0){
// digital 0-7
#ifdef SERVO_STIK
v = PINA;
#else
v = PINB;
#endif
}else if(addr == REG_DIGITAL_IN1){
// digital 8-15
#ifdef SERVO_STIK
v = (PINB>>1);
#else
v = PIND;
#endif
}else if(addr == REG_DIGITAL_IN2){
// digital 16-23
v = PINC;
}else if(addr == REG_DIGITAL_IN3){
// digital 24-31
v = PINA;
}else if(addr == REG_RETURN_LEVEL){
v = ret_level;
}else if(addr == REG_ALARM_LED){
// TODO
}else if(addr < REG_SERVO_BASE){
// send analog reading
int x = analogRead(addr-REG_ANA_BASE)>>2;
x += analogRead(addr-REG_ANA_BASE)>>2;
x += analogRead(addr-REG_ANA_BASE)>>2;
x += analogRead(addr-REG_ANA_BASE)>>2;
v = x/4;
}else if(addr < REG_MOVING){
// send servo position
v = 0;
}else{
v = userRead(addr);
}
checksum += v;
Serial.write(v);
addr++;bytes--;
}
return checksum;
}
int doPlaySeq(){
seqPos = 0; int i;
while(sequence[seqPos].pose != 0xff){
int p = sequence[seqPos].pose;
// are we HALT?
if(Serial.read() == 'H') return 1;
// load pose
for(i=0; i<controllers[0].poseSize; i++)
controllers[0].setNextPose(i+1,poses[p][i]);
controllers[0].interpolateSetup(sequence[seqPos].time);
while(controllers[0].interpolating)
controllers[0].interpolateStep();
// next transition
seqPos++;
}
return 0;
}
/*
* Send status packet
*/
void statusPacket(int id, int err){
Serial.write(0xff);
Serial.write(0xff);
Serial.write(id);
Serial.write(2);
Serial.write(err);
Serial.write(255-((id+2+err)%256));
}
/*
* decode packets: ff ff id length ins params checksum
* same as ax-12 table, except, we define new instructions for Arbotix
*/
void loop(){
int i;
// process messages
while(Serial.available() > 0){
// We need to 0xFF at start of packet
if(mode == 0){ // start of new packet
if(Serial.read() == 0xff){
mode = 2;
digitalWrite(0,HIGH-digitalRead(0));
}
//}else if(mode == 1){ // another start byte
// if(Serial.read() == 0xff)
// mode = 2;
// else
// mode = 0;
}else if(mode == 2){ // next byte is index of servo
id = Serial.read();
if(id != 0xff)
mode = 3;
}else if(mode == 3){ // next byte is length
length = Serial.read();
checksum = id + length;
mode = 4;
}else if(mode == 4){ // next byte is instruction
ins = Serial.read();
checksum += ins;
index = 0;
mode = 5;
}else if(mode == 5){ // read data in
params[index] = Serial.read();
checksum += (int) params[index];
index++;
if(index + 1 == length){ // we've read params & checksum
mode = 0;
if((checksum%256) != 255){
// return an error packet: FF FF id Len Err=bad checksum, params=None check
statusPacket(id, ERR_CHECKSUM);
}else if(id == 253){ // ID = 253, ArbotiX instruction
switch(ins){
case AX_WRITE_DATA:
// send return packet
statusPacket(id,handleWrite());
break;
case AX_READ_DATA:
checksum = id + params[1] + 2;
Serial.write(0xff);
Serial.write(0xff);
Serial.write(id);
Serial.write((unsigned char)2+params[1]);
Serial.write((unsigned char)0);
// send actual data
checksum += handleRead();
Serial.write(255-((checksum)%256));
break;
case ARB_SIZE_POSE: // Pose Size = 7, followed by single param: size of pose
statusPacket(id,0);
if(controllers[0].poseSize == 0)
controllers[0].setup(18);
controllers[0].poseSize = params[0];
controllers[0].readPose();
break;
case ARB_LOAD_POSE: // Load Pose = 8, followed by index, then pose positions (# of param = 2*pose_size)
statusPacket(id,0);
for(i=0; i<controllers[0].poseSize; i++)
poses[params[0]][i] = params[(2*i)+1]+(params[(2*i)+2]<<8);
break;
case ARB_LOAD_SEQ: // Load Seq = 9, followed by index/times (# of parameters = 3*seq_size)
statusPacket(id,0);
for(i=0;i<(length-2)/3;i++){
sequence[i].pose = params[(i*3)];
sequence[i].time = params[(i*3)+1] + (params[(i*3)+2]<<8);
}
break;
case ARB_PLAY_SEQ: // Play Seq = A, no params
statusPacket(id,0);
doPlaySeq();
break;
case ARB_LOOP_SEQ: // Play Seq until we recieve a 'H'alt
statusPacket(id,0);
while(doPlaySeq() > 0);
break;
// ARB_TEST is deprecated and removed
case ARB_CONTROL_SETUP: // Setup a controller
statusPacket(id,0);
if(params[0] < CONTROLLER_COUNT){
controllers[params[0]].setup(length-3);
for(int i=0; i<length-3; i++){
controllers[params[0]].setId(i, params[i+1]);
}
#ifdef USE_BASE
}else if(params[0] == 10){
Kp = params[1];
Kd = params[2];
Ki = params[3];
Ko = params[4];
#endif
}
break;
case ARB_CONTROL_WRITE: // Write values to a controller
statusPacket(id,0);
if(params[0] < CONTROLLER_COUNT){
for(int i=0; i<length-4; i+=2){
controllers[params[0]].setNextPose(controllers[params[0]].getId(i/2), params[i+1]+(params[i+2]<<8));
}
controllers[params[0]].readPose();
controllers[params[0]].interpolateSetup(params[length-3]*33);
#ifdef USE_BASE
}else if(params[0] == 10){
left_speed = params[1];
left_speed += (params[2]<<8);
right_speed = params[3];
right_speed += (params[4]<<8);
if((left_speed == 0) && (right_speed == 0)){
drive.set(0,0);
ClearPID();
}else{
if((left.Velocity == 0) && (right.Velocity == 0)){
PIDmode = 1; moving = 1;
left.PrevEnc = Encoders.left;
right.PrevEnc = Encoders.right;
}
}
left.Velocity = left_speed;
right.Velocity = right_speed;
#endif
}
break;
case ARB_CONTROL_STAT: // Read status of a controller
if(params[0] < CONTROLLER_COUNT){
Serial.write((unsigned char)0xff);
Serial.write((unsigned char)0xff);
Serial.write((unsigned char)id);
Serial.write((unsigned char)3);
Serial.write((unsigned char)0);
checksum = controllers[params[0]].interpolating;
Serial.write((unsigned char)checksum);
checksum += id + 3;
Serial.write((unsigned char)255-((checksum)%256));
#ifdef USE_BASE
}else if(params[0] == 10){
checksum = id + 2 + 8;
Serial.write((unsigned char)0xff);
Serial.write((unsigned char)0xff);
Serial.write((unsigned char)id);
Serial.write((unsigned char)2+8);
Serial.write((unsigned char)0); // error level
int v = ((unsigned long)Encoders.left>>0)%256;
Serial.write((unsigned char)v);
checksum += v;
v = ((unsigned long)Encoders.left>>8)%256;
Serial.write((unsigned char)v);
checksum += v;
v = ((unsigned long)Encoders.left>>16)%256;
Serial.write((unsigned char)v);
checksum += v;
v = ((unsigned long)Encoders.left>>24)%256;
Serial.write((unsigned char)v);
checksum += v;
v = ((unsigned long)Encoders.right>>0)%256;
Serial.write((unsigned char)v);
checksum += v;
v = ((unsigned long)Encoders.right>>8)%256;
Serial.write((unsigned char)v);
checksum += v;
v = ((unsigned long)Encoders.right>>16)%256;
Serial.write((unsigned char)v);
checksum += v;
v = ((unsigned long)Encoders.right>>24)%256;
Serial.write((unsigned char)v);
checksum += v;
Serial.write((unsigned char)255-((checksum)%256));
#endif
}
break;
}
}else if(id == 0xFE){
// sync read or write
if(ins == ARB_SYNC_READ){
int start = params[0]; // address to read in control table
int bytes = params[1]; // # of bytes to read from each servo
int k = 2;
checksum = id + (bytes*(length-4)) + 2;
Serial.write((unsigned char)0xff);
Serial.write((unsigned char)0xff);
Serial.write((unsigned char)id);
Serial.write((unsigned char)2+(bytes*(length-4)));
Serial.write((unsigned char)0); // error code
// send actual data
for(k=2; k<length-2; k++){
if( ax12GetRegister(params[k], start, bytes) >= 0){
for(i=0;i<bytes;i++){
checksum += ax_rx_buffer[5+i];
Serial.write((unsigned char)ax_rx_buffer[5+i]);
}
}else{
for(i=0;i<bytes;i++){
checksum += 255;
Serial.write((unsigned char)255);
}
}
}
Serial.write((unsigned char)255-((checksum)%256));
}else{
// TODO: sync write pass thru
int k;
setTXall();
ax12write(0xff);
ax12write(0xff);
ax12write(id);
ax12write(length);
ax12write(ins);
for(k=0; k<length; k++)
ax12write(params[k]);
// no return
}
}else{ // ID != 253, pass thru
switch(ins){
// TODO: streamline this
case AX_READ_DATA:
ax12GetRegister(id, params[0], params[1]);
// return a packet: FF FF id Len Err params check
if(ax_rx_buffer[3] > 0){
for(i=0;i<ax_rx_buffer[3]+4;i++)
Serial.write(ax_rx_buffer[i]);
}
ax_rx_buffer[3] = 0;
break;
case AX_WRITE_DATA:
if(length == 4){
ax12SetRegister(id, params[0], params[1]);
}else{
int x = params[1] + (params[2]<<8);
ax12SetRegister2(id, params[0], x);
}
statusPacket(id,0);
break;
}
}
}
} // end mode == 5
} // end while(available)
// update joints
for(int i=0; i<5; i++)
controllers[i].interpolateStep();
#ifdef USE_BASE
// update pid
updatePID();
#endif
}

22
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_firmware/src/user_hooks.h Normal file
View File

@ -0,0 +1,22 @@
/*
* This file can be used to define custom register addresses
* Just use addresses 100 and above
*/
void userSetup()
{
// do any setup here
}
unsigned char userWrite(int addr, unsigned char param)
{
// use the register value
return ERR_INSTRUCTION;
}
/* Read one byte from register located at addr */
int userRead(int addr)
{
// return the register value
return 0;
}

37
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_msgs/CHANGELOG.rst Normal file
View File

@ -0,0 +1,37 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package arbotix_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.11.0 (2020-12-29)
-------------------
* Merge pull request `#25 <https://github.com/vanadiumlabs/arbotix_ros/issues/25>`_ from corot/indigo-devel
Implement issue https://github.com/vanadiumlabs/arbotix_ros/issues/24:
* Implement issue https://github.com/vanadiumlabs/arbotix_ros/issues/24:
Allow 16 bit values on arbotix_msgs/Analog messages, but assume 8 bits
by default
* Contributors: Michael Ferguson, corot
0.10.0 (2014-07-14)
-------------------
0.9.2 (2014-02-12)
------------------
0.9.1 (2014-01-28)
------------------
* Added set_speed service to servo controller
0.9.0 (2013-08-22)
------------------
* add new enable service
0.8.2 (2013-03-28)
------------------
* updates to cmakelists.txt and package.xml, fixes `#2 <https://github.com/vanadiumlabs/arbotix_ros/issues/2>`_
0.8.1 (2013-03-09)
------------------
0.8.0 (2013-02-21)
------------------
* import drivers and catkinize

20
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_msgs/CMakeLists.txt Normal file
View File

@ -0,0 +1,20 @@
cmake_minimum_required(VERSION 3.0.2)
project(arbotix_msgs)
find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
add_message_files(FILES
Analog.msg
Digital.msg
)
add_service_files(FILES
Enable.srv
Relax.srv
SetupChannel.srv
SetSpeed.srv
)
generate_messages(DEPENDENCIES std_msgs)
catkin_package(CATKIN_DEPENDS message_runtime std_msgs)

3
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_msgs/msg/Analog.msg Normal file
View File

@ -0,0 +1,3 @@
# Reading from a single analog IO pin.
Header header
uint16 value

14
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_msgs/msg/Digital.msg Normal file
View File

@ -0,0 +1,14 @@
# Reading or command to a single digital IO pin.
Header header
# value of pin
uint8 LOW=0
uint8 HIGH=255
uint8 value
# direction of pin
uint8 INPUT=0
uint8 OUTPUT=255
uint8 direction

21
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_msgs/package.xml Normal file
View File

@ -0,0 +1,21 @@
<package>
<name>arbotix_msgs</name>
<version>0.11.0</version>
<description>
Messages and Services definitions for the ArbotiX.
</description>
<author>Michael Ferguson</author>
<maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/arbotix_msgs</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
</package>

3
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_msgs/srv/Enable.srv Normal file
View File

@ -0,0 +1,3 @@
bool enable
---
bool state

3
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_msgs/srv/Relax.srv Normal file
View File

@ -0,0 +1,3 @@
---

2
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_msgs/srv/SetSpeed.srv Normal file
View File

@ -0,0 +1,2 @@
float64 speed
---

7
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_msgs/srv/SetupChannel.srv Normal file
View File

@ -0,0 +1,7 @@
# message to setup an IO channel
string topic_name
uint8 pin
uint8 value
uint8 rate
---

65
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/CHANGELOG.rst Normal file
View File

@ -0,0 +1,65 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package arbotix_python
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.11.0 (2020-12-29)
-------------------
* Update all python shebangs to python3 + rosdep dependency (`#48 <https://github.com/vanadiumlabs/arbotix_ros/issues/48>`_)
Co-authored-by: Murat Calis <mc@pirate-robotics.net>
* arbotix_python for ROS Noetic (`#46 <https://github.com/vanadiumlabs/arbotix_ros/issues/46>`_)
Migrated arbotix_python to work with ROS Noetic
Co-authored-by: Murat Calis <mc@pirate-robotics.net>
* Merge pull request `#31 <https://github.com/vanadiumlabs/arbotix_ros/issues/31>`_ from corot/serial_reconnect
Allow runtime connection/disconnection to/from ArbotiX board
* Merge pull request `#29 <https://github.com/vanadiumlabs/arbotix_ros/issues/29>`_ from croesmann/indigo-devel
Allow cancelling the FollowJointTrajectoryAction during execution
* Merge pull request `#33 <https://github.com/vanadiumlabs/arbotix_ros/issues/33>`_ from corot/issue_26
Issue `#26 <https://github.com/vanadiumlabs/arbotix_ros/issues/26>`_ implementation: enable/relax services on ServoController class
* Issue `#26 <https://github.com/vanadiumlabs/arbotix_ros/issues/26>`_ implementation: enable/relax services to the ServoController
class, so you don't need to call service on each servo
* Close serial port only if not fake
* Allow runtime connection/disconnection to/from ArbotiX board
* Minor formatting fix
* Fixed formatting issues
* the follow joint trajectory action can now be canceled during execution
* Fix syntax
* Merge pull request `#25 <https://github.com/vanadiumlabs/arbotix_ros/issues/25>`_ from corot/indigo-devel
Implement issue https://github.com/vanadiumlabs/arbotix_ros/issues/24:
* leng -> length
* Implement issue https://github.com/vanadiumlabs/arbotix_ros/issues/24:
Allow 16 bit values on arbotix_msgs/Analog messages, but assume 8 bits
by default
* Contributors: Christoph Rösmann, Jorge Santos Simón, Michael Ferguson, calismurat, corot
0.10.0 (2014-07-14)
-------------------
* Set queue_size=5 on all publishers
* Contributors: Jorge Santos
0.9.2 (2014-02-12)
------------------
0.9.1 (2014-01-28)
------------------
* set velocity when in sim/fake mode
* Added set_speed service to servo controller
* Added 'set spd' option to arbotix_terminal
0.9.0 (2013-08-22)
------------------
* Add new enable service
* remove roslib manifest loading
* remove old dynamixels block, closes `#6 <https://github.com/vanadiumlabs/arbotix_ros/issues/6>`_
* Warn of extra joints in joint trajectory, but only fail when missing a joint we control
0.8.2 (2013-03-28)
------------------
0.8.1 (2013-03-09)
------------------
* fix depend for proper release
0.8.0 (2013-02-21)
------------------
* fix follow controller issues with zeros in header timestamps, cleanup logging a bit
* import drivers and catkinize

15
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/CMakeLists.txt Normal file
View File

@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.0.2)
project(arbotix_python)
find_package(catkin REQUIRED)
catkin_package()
catkin_python_setup()
install(DIRECTORY demos
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(PROGRAMS bin/arbotix_driver
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

202
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/bin/arbotix_driver Normal file
View File

@ -0,0 +1,202 @@
#!/usr/bin/env python
"""
ArbotiX Node: serial connection to an ArbotiX board w/ PyPose/NUKE/ROS
Copyright (c) 2008-2011 Michael E. Ferguson. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
import sys
from arbotix_msgs.msg import *
from arbotix_msgs.srv import *
from arbotix_python.arbotix import ArbotiX, ArbotiXException
from arbotix_python.diff_controller import DiffController
from arbotix_python.follow_controller import FollowController
from arbotix_python.servo_controller import *
from arbotix_python.linear_controller import *
from arbotix_python.publishers import *
from arbotix_python.io import *
# name: [ControllerClass, pause]
controller_types = { "follow_controller" : FollowController,
"diff_controller" : DiffController,
# "omni_controller" : OmniController,
"linear_controller" : LinearControllerAbsolute,
"linear_controller_i" : LinearControllerIncremental }
###############################################################################
# Main ROS interface
class ArbotixROS(ArbotiX):
def __init__(self):
pause = False
# load configurations
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud = int(rospy.get_param("~baud", "115200"))
timeout = float(rospy.get_param("~timeout", "0.1"))
self.rate = rospy.get_param("~rate", 100.0)
self.fake = rospy.get_param("~sim", False)
self.use_sync_read = rospy.get_param("~sync_read",True) # use sync read?
self.use_sync_write = rospy.get_param("~sync_write",True) # use sync write?
# setup publishers
self.diagnostics = DiagnosticsPublisher()
self.joint_state_publisher = JointStatePublisher()
# start an arbotix driver; differ port opening to properly handle connection failures
if not self.fake:
ArbotiX.__init__(self, port, baud, timeout, open_port=True)
self.connectArbotiX()
else:
rospy.loginfo("ArbotiX being simulated.")
# setup joints
self.joints = dict()
for name in rospy.get_param("~joints", dict()).keys():
joint_type = rospy.get_param("~joints/"+name+"/type", "dynamixel")
if joint_type == "dynamixel":
self.joints[name] = DynamixelServo(self, name)
elif joint_type == "hobby_servo":
self.joints[name] = HobbyServo(self, name)
elif joint_type == "calibrated_linear":
self.joints[name] = LinearJoint(self, name)
# setup controller
self.controllers = [ServoController(self, "servos"), ]
controllers = rospy.get_param("~controllers", dict())
for name, params in controllers.items():
try:
controller = controller_types[params["type"]](self, name)
self.controllers.append( controller )
pause = pause or controller.pause
except Exception as e:
if type(e) == KeyError:
rospy.logerr("Unrecognized controller: " + params["type"])
else:
rospy.logerr(str(type(e)) + str(e))
# wait for arbotix to start up (especially after reset)
if not self.fake:
if rospy.has_param("~digital_servos") or rospy.has_param("~digital_sensors") or rospy.has_param("~analog_sensors"):
pause = True
if pause:
while self.getDigital(1) == -1 and not rospy.is_shutdown():
rospy.loginfo("ArbotiX: waiting for response...")
rospy.sleep(0.25)
rospy.loginfo("ArbotiX connected.")
for controller in self.controllers:
controller.startup()
# services for io
rospy.Service('~SetupAnalogIn',SetupChannel, self.analogInCb)
rospy.Service('~SetupDigitalIn',SetupChannel, self.digitalInCb)
rospy.Service('~SetupDigitalOut',SetupChannel, self.digitalOutCb)
# initialize digital/analog IO streams
self.io = dict()
if not self.fake:
for v,t in {"digital_servos":DigitalServo,"digital_sensors":DigitalSensor,"analog_sensors":AnalogSensor}.items():
temp = rospy.get_param("~"+v,dict())
for name in temp.keys():
pin = rospy.get_param('~'+v+'/'+name+'/pin',1)
value = rospy.get_param('~'+v+'/'+name+'/value',0)
rate = rospy.get_param('~'+v+'/'+name+'/rate',10)
leng = rospy.get_param('~'+v+'/'+name+'/length',1) # just for analog sensors
if(v != "analog_sensors"):
self.io[name] = t(name, pin, value, rate, self)
else:
self.io[name] = t(name, pin, value, rate, leng, self)
r = rospy.Rate(self.rate)
# main loop -- do all the read/write here
while not rospy.is_shutdown():
try:
# update controllers
for controller in self.controllers:
controller.update()
# update io
for io in self.io.values():
io.update()
# publish feedback
self.joint_state_publisher.update(self.joints.values(), self.controllers)
self.diagnostics.update(self.joints.values(), self.controllers)
except ArbotiXException as e:
# We assume this is a serial connection error (as is the only use of
# ArbotiXException by now...); try to reconnect to solve the issue
rospy.logerr("ArbotiX error: %s", e)
self.connectArbotiX()
r.sleep()
# do shutdown
for controller in self.controllers:
controller.shutdown()
# disconnect from the ArbotiX
if not self.fake:
self.closePort()
def analogInCb(self, req):
# TODO: Add check, only 1 service per pin
if not self.fake:
self.io[req.topic_name] = AnalogSensor(req.topic_name, req.pin, req.value, req.rate, req.leng, self)
return SetupChannelResponse()
def digitalInCb(self, req):
if not self.fake:
self.io[req.topic_name] = DigitalSensor(req.topic_name, req.pin, req.value, req.rate, self)
return SetupChannelResponse()
def digitalOutCb(self, req):
if not self.fake:
self.io[req.topic_name] = DigitalServo(req.topic_name, req.pin, req.value, req.rate, self)
return SetupChannelResponse()
def connectArbotiX(self):
iter = 0
while True:
try:
self.openPort()
rospy.loginfo("Started ArbotiX connection on port " + self._ser.port + ".")
return
except ArbotiXException as e:
if iter%4 == 0:
rospy.logerr("Unable to connect to ArbotiX: %s.", e)
rospy.sleep(0.5)
iter += 1
if __name__ == "__main__":
rospy.init_node('arbotix')
a = ArbotixROS()

206
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/bin/arbotix_gui Normal file
View File

@ -0,0 +1,206 @@
#!/usr/bin/env python
"""
A simple Controller GUI to drive robots and pose heads.
Copyright (c) 2008-2011 Michael E. Ferguson. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
import wx
from math import radians
from geometry_msgs.msg import Twist
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64
from arbotix_msgs.srv import Relax
from arbotix_python.joints import *
width = 325
class servoSlider():
def __init__(self, parent, min_angle, max_angle, name, i):
self.name = name
if name.find("_joint") > 0: # remove _joint for display name
name = name[0:-6]
self.position = wx.Slider(parent, -1, 0, int(min_angle*100), int(max_angle*100), wx.DefaultPosition, (150, -1), wx.SL_HORIZONTAL)
self.enabled = wx.CheckBox(parent, i, name+":")
self.enabled.SetValue(False)
self.position.Disable()
def setPosition(self, angle):
self.position.SetValue(int(angle*100))
def getPosition(self):
return self.position.GetValue()/100.0
class controllerGUI(wx.Frame):
TIMER_ID = 100
def __init__(self, parent, debug = False):
wx.Frame.__init__(self, parent, -1, "ArbotiX Controller GUI", style = wx.DEFAULT_FRAME_STYLE & ~ (wx.RESIZE_BORDER | wx.MAXIMIZE_BOX))
sizer = wx.GridBagSizer(5,5)
# Move Base
drive = wx.StaticBox(self, -1, 'Move Base')
drive.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
driveBox = wx.StaticBoxSizer(drive,orient=wx.VERTICAL)
self.movebase = wx.Panel(self,size=(width,width-20))
self.movebase.SetBackgroundColour('WHITE')
self.movebase.Bind(wx.EVT_MOTION, self.onMove)
wx.StaticLine(self.movebase, -1, (width/2, 0), (1,width), style=wx.LI_VERTICAL)
wx.StaticLine(self.movebase, -1, (0, width/2), (width,1))
driveBox.Add(self.movebase)
sizer.Add(driveBox,(0,0),wx.GBSpan(1,1),wx.EXPAND|wx.TOP|wx.BOTTOM|wx.LEFT,5)
self.forward = 0
self.turn = 0
self.X = 0
self.Y = 0
self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Move Servos
servo = wx.StaticBox(self, -1, 'Move Servos')
servo.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
servoBox = wx.StaticBoxSizer(servo,orient=wx.VERTICAL)
servoSizer = wx.GridBagSizer(5,5)
joint_defaults = getJointsFromURDF()
i = 0
dynamixels = rospy.get_param('/arbotix/dynamixels', dict())
self.servos = list()
self.publishers = list()
self.relaxers = list()
joints = rospy.get_param('/arbotix/joints', dict())
# create sliders and publishers
for name in sorted(joints.keys()):
# pull angles
min_angle, max_angle = getJointLimits(name, joint_defaults)
# create publisher
self.publishers.append(rospy.Publisher(name+'/command', Float64, queue_size=5))
if rospy.get_param('/arbotix/joints/'+name+'/type','dynamixel') == 'dynamixel':
self.relaxers.append(rospy.ServiceProxy(name+'/relax', Relax))
else:
self.relaxers.append(None)
# create slider
s = servoSlider(self, min_angle, max_angle, name, i)
servoSizer.Add(s.enabled,(i,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
servoSizer.Add(s.position,(i,1), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL)
self.servos.append(s)
i += 1
# add everything
servoBox.Add(servoSizer)
sizer.Add(servoBox, (0,1), wx.GBSpan(1,1), wx.EXPAND|wx.TOP|wx.BOTTOM|wx.RIGHT,5)
self.Bind(wx.EVT_CHECKBOX, self.enableSliders)
# now we can subscribe
rospy.Subscriber('joint_states', JointState, self.stateCb)
# timer for output
self.timer = wx.Timer(self, self.TIMER_ID)
self.timer.Start(50)
wx.EVT_CLOSE(self, self.onClose)
wx.EVT_TIMER(self, self.TIMER_ID, self.onTimer)
# bind the panel to the paint event
wx.EVT_PAINT(self, self.onPaint)
self.dirty = 1
self.onPaint()
self.SetSizerAndFit(sizer)
self.Show(True)
def onClose(self, event):
self.timer.Stop()
self.Destroy()
def enableSliders(self, event):
servo = event.GetId()
if event.IsChecked():
self.servos[servo].position.Enable()
else:
self.servos[servo].position.Disable()
if self.relaxers[servo]:
self.relaxers[servo]()
def stateCb(self, msg):
for servo in self.servos:
if not servo.enabled.IsChecked():
try:
idx = msg.name.index(servo.name)
servo.setPosition(msg.position[idx])
except:
pass
def onPaint(self, event=None):
# this is the wx drawing surface/canvas
dc = wx.PaintDC(self.movebase)
dc.Clear()
# draw crosshairs
dc.SetPen(wx.Pen("black",1))
dc.DrawLine(width/2, 0, width/2, width)
dc.DrawLine(0, width/2, width, width/2)
dc.SetPen(wx.Pen("red",2))
dc.SetBrush(wx.Brush('red', wx.SOLID))
dc.SetPen(wx.Pen("black",2))
dc.DrawCircle((width/2) + self.X*(width/2), (width/2) - self.Y*(width/2), 5)
def onMove(self, event=None):
if event.LeftIsDown():
pt = event.GetPosition()
if pt[0] > 0 and pt[0] < width and pt[1] > 0 and pt[1] < width:
self.forward = ((width/2)-pt[1])/2
self.turn = (pt[0]-(width/2))/2
self.X = (pt[0]-(width/2.0))/(width/2.0)
self.Y = ((width/2.0)-pt[1])/(width/2.0)
else:
self.forward = 0; self.Y = 0
self.turn = 0; self.X = 0
self.onPaint()
def onTimer(self, event=None):
# send joint updates
for s,p in zip(self.servos,self.publishers):
if s.enabled.IsChecked():
d = Float64()
d.data = s.getPosition()
p.publish(d)
# send base updates
t = Twist()
t.linear.x = self.forward/150.0; t.linear.y = 0; t.linear.z = 0
if self.forward > 0:
t.angular.x = 0; t.angular.y = 0; t.angular.z = -self.turn/50.0
else:
t.angular.x = 0; t.angular.y = 0; t.angular.z = self.turn/50.0
self.cmd_vel.publish(t)
if __name__ == '__main__':
# initialize GUI
rospy.init_node('controllerGUI')
app = wx.PySimpleApp()
frame = controllerGUI(None, True)
app.MainLoop()

188
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/bin/arbotix_terminal Normal file
View File

@ -0,0 +1,188 @@
#!/usr/bin/env python
"""
ArbotiX Terminal - command line terminal to interact with an ArbotiX
Copyright (c) 2008-2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import sys
from arbotix_python.arbotix import ArbotiX # does this look ridiculous to anyone else?
from arbotix_python.ax12 import *
# help phrases
help = ["ArbotiX Terminal V0.1",
"",
"valid commands:",
" ls [i b]- list the servos found on the bus. Optional parameters: i - highest ID to query, b - baudrate to query at.",
" mv id id2 - rename any servo with ID=id, to id2",
" baud b - set baud rate of bus to b",
" get param id - get a parameter value from a servo",
" set param id val - set parameter on servo ID=id to val",
"",
"valid parameters",
" pos - current position of a servo, 0-1023",
" spd - current goal speed of a servo, 0-1023",
" baud - baud rate",
" temp - current temperature, degrees C, READ ONLY"]
class Terminal(ArbotiX):
OKBLUE = '\033[94m'
OKGREEN = '\033[92m'
WARNING = '\033[93m'
FAIL = '\033[91m'
ENDC = '\033[0m'
def __init__(self, port = "/dev/ttyUSB0", baud = 115200):
# start
ArbotiX.__init__(self, port, baud)
print("ArbotiX Terminal --- Version 0.1")
print("Copyright 2011 Vanadium Labs LLC")
# loop
while True:
kmd = input(f">> ").split(" ")
try:
if kmd[0] == "help": # display help data
if len(kmd) > 1: # for a particular command
if kmd[1] == "ls":
print(help[3])
elif kmd[1] == "mv":
print(help[4])
elif kmd[1] == "baud":
print(help[5])
elif kmd[1] == "get":
print(help[6])
elif kmd[1] == "set":
print(help[7])
else:
print("help: unrecognized command")
else:
for h in help:
print(h)
elif kmd[0] == "ls": # list servos
self._ser.timeout = 0.25
if len(kmd) > 2:
self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))])
self.query()
self.query()
elif kmd[0] == "mv": # rename a servo
if self.write( int(kmd[1]), P_ID, [int(kmd[2]),] ) == 0:
print(self.OKBLUE+"OK"+self.ENDC)
elif kmd[0] == "baud": # set bus baud rate
self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))])
print(self.OKBLUE+"OK"+self.ENDC)
elif kmd[0] == "set":
if kmd[1] == "baud":
self.write( int(kmd[2]), P_BAUD_RATE, [self.convertBaud(int(kmd[3]))] )
print(self.OKBLUE+"OK"+self.ENDC)
elif kmd[1] == "pos" or kmd[1] == "position":
self.setPosition( int(kmd[2]), int(kmd[3]) )
print(self.OKBLUE+"OK"+self.ENDC)
elif kmd[1] == "spd" or kmd[1] == "speed":
self.setSpeed( int(kmd[2]), int(kmd[3]) )
print(self.OKBLUE+"OK"+self.ENDC)
elif kmd[0] == "get":
if kmd[1] == "temp":
value = self.getTemperature(int(kmd[2]))
if value >= 60 or value < 0:
print(self.FAIL+str(value)+self.ENDC)
elif value > 40:
print(self.WARNING+str(value)+self.ENDC)
else:
print(self.OKGREEN+str(value)+self.ENDC)
elif kmd[1] == "pos" or kmd[1] == "position":
value = self.getPosition(int(kmd[2]))
if value >= 0:
print(self.OKGREEN+str(value)+self.ENDC)
else:
print(self.FAIL+str(value)+self.ENDC)
elif kmd[1] == "spd" or kmd[1] == "speed":
value = self.getGoalSpeed(int(kmd[2]))
if value >= 0:
print(self.OKGREEN+str(value)+self.ENDC)
else:
print(self.FAIL+str(value)+self.ENDC)
except Exception as e:
print("error...", e)
def query(self, max_id = 18, baud = 1000000):
k = 0 # how many id's have we printed
for i in range(max_id):
if self.getPosition(i+1) != -1:
if k > 8:
k = 0
print("")
print(repr(i+1).rjust(4), end="\t"),
k = k + 1
else:
if k > 8:
k = 0
print("")
print(" ....", end="\t")
k = k + 1
sys.stdout.flush()
print("")
def convertBaud(self, b):
if b == 500000:
return 3
elif b == 400000:
return 4
elif b == 250000:
return 7
elif b == 200000:
return 9
elif b == 115200:
return 16
elif b == 57600:
return 34
elif b == 19200:
return 103
elif b == 9600:
return 207
else:
return 1 # default to 1Mbps
if __name__ == "__main__":
try:
if len(sys.argv) > 2:
t = Terminal(sys.argv[1], int(sys.argv[2]))
elif len(sys.argv) > 1:
t = Terminal(sys.argv[1])
else:
t = Terminal()
except KeyboardInterrupt:
print("\nExiting...")

13
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/demos/diagnostics.yaml Normal file
View File

@ -0,0 +1,13 @@
pub_rate: 1.0 # Optional
base_path: '' # Optional, prepended to all diagnostic output
analyzers:
joints:
type: GenericAnalyzer
path: 'Joints'
timeout: 5.0
contains: '_joint'
encoders:
type: GenericAnalyzer
path: 'Controllers'
timeout: 5.0
contains: '_controller'

11
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/mainpage.dox Normal file
View File

@ -0,0 +1,11 @@
/**
\mainpage
\htmlinclude manifest.html
In addition to the ROS API offered by the driver.py node, arbotix_python offers a Python API for interacting with the ArbotiX or other Dynamixel-like devices.
\section ArbotixPythonAPI ArbotiX Python API
- \link arbotix_python::arbotix::ArbotiX ArbotiX Class API \endlink
- \link arbotix_python::ax12 Dynamixel register table defintions \endlink
*/

24
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/package.xml Normal file
View File

@ -0,0 +1,24 @@
<package format="3">
<name>arbotix_python</name>
<version>0.11.0</version>
<description>
Bindings and low-level controllers for ArbotiX-powered robots.
</description>
<author>Michael Ferguson</author>
<maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/arbotix_python</url>
<buildtool_depend>catkin</buildtool_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>arbotix_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>diagnostic_msgs</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>python3-serial</exec_depend>
</package>

12
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/setup.py Normal file
View File

@ -0,0 +1,12 @@
#!/usr/bin/env python
from setuptools import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
scripts=['bin/arbotix_gui', 'bin/arbotix_terminal'],
packages=['arbotix_python'],
package_dir={'': 'src'},
)
setup(**d)

0
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/__init__.py Normal file
View File

539
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/arbotix.py Normal file
View File

@ -0,0 +1,539 @@
#!/usr/bin/env python
# Copyright (c) 2008-2011 Vanadium Labs LLC.
# All right reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of Vanadium Labs LLC nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
# Author: Michael Ferguson
## @file arbotix.py Low-level code to control an ArbotiX.
import serial, time, sys, threading
from arbotix_python.ax12 import *
from struct import unpack, pack
## @brief ArbotiX errors. Used by now to handle broken serial connections.
class ArbotiXException(Exception):
pass
## @brief This class controls an ArbotiX, USBDynamixel, or similar board through a serial connection.
class ArbotiX:
## @brief Constructs an ArbotiX instance, optionally opening the serial connection.
##
## @param port The name of the serial port to open.
##
## @param baud The baud rate to run the port at.
##
## @param timeout The timeout to use for the port. When operating over a wireless link, you may need to
## increase this.
##
## @param open Whether to open immediately the serial port.
def __init__(self, port="/dev/ttyUSB0", baud=115200, timeout=0.1, open_port=True):
self._mutex = threading._allocate_lock()
self._ser = serial.Serial()
self._ser.port = port
self._ser.baudrate = baud
self._ser.timeout = timeout
if open_port:
self._ser.open()
## The last error level read back
self.error = 0
def __write__(self, msg):
try:
self._ser.write(msg)
except serial.SerialException as e:
self._mutex.release()
raise ArbotiXException(e)
def openPort(self):
self._ser.close()
try:
self._ser.open()
except serial.SerialException as e:
raise ArbotiXException(e)
def closePort(self):
self._ser.close()
## @brief Read a dynamixel return packet in an iterative attempt.
##
## @param mode This should be 0 to start reading packet.
##
## @return The error level returned by the device.
def getPacket(self, mode, id=-1, leng=-1, error=-1, params = None):
try:
d = self._ser.read()
except Exception as e:
print(e)
return None
# need a positive byte
if not d or d == '':
return None
# now process our byte
if mode == 0: # get our first 0xFF
if d == b'\xff':
return self.getPacket(1)
else:
return self.getPacket(0)
elif mode == 1: # get our second 0xFF
if d == b'\xff':
return self.getPacket(2)
else:
return self.getPacket(0)
elif mode == 2: # get id
if d != b'\xff':
return self.getPacket(3, ord(d))
else:
return self.getPacket(0)
elif mode == 3: # get length
return self.getPacket(4, id, ord(d))
elif mode == 4: # read error
self.error = d
if leng == 2:
return self.getPacket(6, id, leng, ord(d), list())
else:
return self.getPacket(5, id, leng, ord(d), list())
elif mode == 5: # read params
params.append(ord(d))
if len(params) + 2 == leng:
return self.getPacket(6, id, leng, error, params)
else:
return self.getPacket(5, id, leng, error, params)
elif mode == 6: # read checksum
checksum = id + leng + error + sum(params) + ord(d)
if checksum % 256 != 255:
return None
return params
# fail
return None
## @brief Send an instruction to the device.
##
## @param index The ID of the servo to write.
##
## @param ins The instruction to send.
##
## @param params A list of the params to send.
##
## @param ret Whether to read a return packet.
##
## @return The return packet, if read.
def execute(self, index, ins, params, ret=True):
values = None
self._mutex.acquire()
try:
self._ser.flushInput()
except Exception as e:
print(e)
length = 2 + len(params)
checksum = 255 - ((index + length + ins + sum(params))%256)
packet = bytearray()
packet.append(0xFF)
packet.append(0xFF)
packet.append(index)
packet.append(length)
packet.append(ins)
self.__write__(packet)
for val in params:
self.__write__(bytes([val]))
self.__write__(bytes([checksum]))
if ret:
values = self.getPacket(0)
self._mutex.release()
return values
## @brief Read values of registers.
##
## @param index The ID of the servo.
##
## @param start The starting register address to begin the read at.
##
## @param length The number of bytes to read.
##
## @return A list of the bytes read, or -1 if failure.
def read(self, index, start, length):
values = self.execute(index, AX_READ_DATA, [start, length])
if values == None:
return -1
else:
return values
## @brief Write values to registers.
##
## @param index The ID of the servo.
##
## @param start The starting register address to begin writing to.
##
## @param values The data to write, in a list.
##
## @return The error level.
def write(self, index, start, values):
self.execute(index, AX_WRITE_DATA, [start] + values)
return self.error
## @brief Write values to registers on many servos.
##
## @param start The starting register address to begin writing to.
##
## @param values The data to write, in a list of lists. Format should be
## [(id1, val1, val2), (id2, val1, val2)]
def syncWrite(self, start, values):
output = list()
for i in values:
output = output + i
length = len(output) + 4 # length of overall packet
lbytes = len(values[0])-1 # length of bytes to write to a servo
self._mutex.acquire()
try:
self._ser.flushInput()
except:
pass
packet = bytearray()
packet.append(0xFF)
packet.append(0xFF)
packet.append(254)
packet.append(length)
packet.append(AX_SYNC_WRITE)
self.__write__(packet)
self.__write__(bytes([start])) # start address
self.__write__(bytes([lbytes])) # bytes to write each servo
for i in output:
self.__write__(bytes([i]))
checksum = 255 - ((254 + length + AX_SYNC_WRITE + start + lbytes + sum(output))%256)
self.__write__(bytes([checksum]))
self._mutex.release()
## @brief Read values of registers on many servos.
##
## @param servos A list of the servo IDs to read from.
##
## @param start The starting register address to begin reading at.
##
## @param length The number of bytes to read from each servo.
##
## @return A list of bytes read.
def syncRead(self, servos, start, length):
return self.execute(0xFE, AX_SYNC_READ, [start, length] + servos )
## @brief Set baud rate of a device.
##
## @param index The ID of the device to write (Note: ArbotiX is 253).
##
## @param baud The baud rate.
##
## @return The error level.
def setBaud(self, index, baud):
return self.write(index, P_BAUD_RATE, [baud, ])
## @brief Get the return level of a device.
##
## @param index The ID of the device to read.
##
## @return The return level, .
def getReturnLevel(self, index):
try:
return int(self.read(index, P_RETURN_LEVEL, 1)[0])
except:
return -1
## @brief Set the return level of a device.
##
## @param index The ID of the device to write.
##
## @param value The return level.
##
## @return The error level.
def setReturnLevel(self, index, value):
return self.write(index, P_RETURN_LEVEL, [value])
## @brief Turn on the torque of a servo.
##
## @param index The ID of the device to enable.
##
## @return The error level.
def enableTorque(self, index):
return self.write(index, P_TORQUE_ENABLE, [1])
## @brief Turn on the torque of a servo.
##
## @param index The ID of the device to disable.
##
## @return The error level.
def disableTorque(self, index):
return self.write(index, P_TORQUE_ENABLE, [0])
## @brief Set the status of the LED on a servo.
##
## @param index The ID of the device to write.
##
## @param value 0 to turn the LED off, >0 to turn it on
##
## @return The error level.
def setLed(self, index, value):
return self.write(index, P_LED, [value])
## @brief Set the position of a servo.
##
## @param index The ID of the device to write.
##
## @param value The position to go to in, in servo ticks.
##
## @return The error level.
def setPosition(self, index, value):
return self.write(index, P_GOAL_POSITION_L, [value%256, value>>8])
## @brief Set the speed of a servo.
##
## @param index The ID of the device to write.
##
## @param value The speed to write.
##
## @return The error level.
def setSpeed(self, index, value):
return self.write(index, P_GOAL_SPEED_L, [value%256, value>>8])
## @brief Get the position of a servo.
##
## @param index The ID of the device to read.
##
## @return The servo position.
def getPosition(self, index):
values = self.read(index, P_PRESENT_POSITION_L, 2)
try:
return int(values[0]) + (int(values[1])<<8)
except:
return -1
## @brief Get the speed of a servo.
##
## @param index The ID of the device to read.
##
## @return The servo speed.
def getSpeed(self, index):
values = self.read(index, P_PRESENT_SPEED_L, 2)
try:
return int(values[0]) + (int(values[1])<<8)
except:
return -1
## @brief Get the goal speed of a servo.
##
## @param index The ID of the device to read.
##
## @return The servo goal speed.
def getGoalSpeed(self, index):
values = self.read(index, P_GOAL_SPEED_L, 2)
try:
return int(values[0]) + (int(values[1])<<8)
except:
return -1
## @brief Get the voltage of a device.
##
## @param index The ID of the device to read.
##
## @return The voltage, in Volts.
def getVoltage(self, index):
try:
return int(self.read(index, P_PRESENT_VOLTAGE, 1)[0])/10.0
except:
return -1
## @brief Get the temperature of a device.
##
## @param index The ID of the device to read.
##
## @return The temperature, in degrees C.
def getTemperature(self, index):
try:
return int(self.read(index, P_PRESENT_TEMPERATURE, 1)[0])
except:
return -1
## @brief Determine if a device is moving.
##
## @param index The ID of the device to read.
##
## @return True if servo is moving.
def isMoving(self, index):
try:
d = self.read(index, P_MOVING, 1)[0]
except:
return True
return d != 0
## @brief Put a servo into wheel mode (continuous rotation).
##
## @param index The ID of the device to write.
def enableWheelMode(self, index):
self.write(index, P_CCW_ANGLE_LIMIT_L, [0,0])
## @brief Put a servo into servo mode.
##
## @param index The ID of the device to write.
##
## @param resolution The resolution of the encoder on the servo. NOTE: if using
## 12-bit resolution servos (EX-106, MX-28, etc), you must pass resolution = 12.
##
## @return
def disableWheelMode(self, index, resolution=10):
resolution = (2 ** resolution) - 1
self.write(index, P_CCW_ANGLE_LIMIT_L, [resolution%256,resolution>>8])
## Direction definition for setWheelSpeed
FORWARD = 0
## Direction definition for setWheelSpeed
BACKWARD = 1
## @brief Set the speed and direction of a servo which is in wheel mode (continuous rotation).
##
## @param index The ID of the device to write.
##
## @param direction The direction of rotation, either FORWARD or BACKWARD
##
## @param speed The speed to move at (0-1023).
##
## @return
def setWheelSpeed(self, index, direction, speed):
if speed > 1023:
speed = 1023
if direction == self.FORWARD:
# 0~1023 is forward, it is stopped by setting to 0 while rotating to CCW direction.
self.write(index, P_GOAL_SPEED_L, [speed%256, speed>>8])
else:
# 1024~2047 is backward, it is stopped by setting to 1024 while rotating to CW direction.
speed += 1024
self.write(index, P_GOAL_SPEED_L, [speed%256, speed>>8])
###########################################################################
# Extended ArbotiX Driver
## Helper definition for analog and digital access.
LOW = 0
## Helper definition for analog and digital access.
HIGH = 0xff
## Helper definition for analog and digital access.
INPUT = 0
## Helper definition for analog and digital access.
OUTPUT = 0xff
# ArbotiX-specific register table
# We do Model, Version, ID, Baud, just like the AX-12
## Register base address for reading digital ports
REG_DIGITAL_IN0 = 5
REG_DIGITAL_IN1 = 6
REG_DIGITAL_IN2 = 7
REG_DIGITAL_IN3 = 8
## Register address for triggering rescan
REG_RESCAN = 15
# 16, 17 = RETURN, ALARM
## Register address of first analog port (read only).
## Each additional port is BASE + index.
ANA_BASE = 18
## Register address of analog servos. Up to 10 servos, each
## uses 2 bytes (L, then H), pulse width (0, 1000-2000ms) (Write only)
SERVO_BASE = 26
# Address 46 is Moving, just like an AX-12
REG_DIGITAL_OUT0 = 47
## @brief Force the ArbotiX2 to rescan the Dynamixel busses.
def rescan(self):
self.write(253, self.REG_RESCAN, [1,])
## @brief Get the value of an analog input pin.
##
## @param index The ID of the pin to read (0 to 7).
##
## @param leng The number of bytes to read (1 or 2).
##
## @return 8-bit/16-bit analog value of the pin, -1 if error.
def getAnalog(self, index, leng=1):
try:
val = self.read(253, self.ANA_BASE+int(index), leng)
return sum(val[i] << (i * 8) for i in range(leng))
except:
return -1
## @brief Get the value of an digital input pin.
##
## @param index The ID of the pin to read (0 to 31).
##
## @return 0 for low, 255 for high, -1 if error.
def getDigital(self, index):
try:
if index < 32:
x = self.read(253, self.REG_DIGITAL_IN0 + int(index/8), 1)[0]
else:
return -1
except:
return -1
if x & (2**(index%8)):
return 255
else:
return 0
## @brief Get the value of an digital input pin.
##
## @param index The ID of the pin to write (0 to 31).
##
## @param value The value of the port, >0 is high.
##
## @param direction The direction of the port, >0 is output.
##
## @return -1 if error.
def setDigital(self, index, value, direction=0xff):
if index > 31: return -1
if value == 0 and direction > 0:
self.write(253, self.REG_DIGITAL_OUT0 + int(index), [1])
elif value > 0 and direction > 0:
self.write(253, self.REG_DIGITAL_OUT0 + int(index), [3])
elif value > 0 and direction == 0:
self.write(253, self.REG_DIGITAL_OUT0 + int(index), [2])
else:
self.write(253, self.REG_DIGITAL_OUT0 + int(index), [0])
return 0
## @brief Set the position of a hobby servo.
##
## @param index The ID of the servo to write (0 to 7).
##
## @param value The position of the servo in milliseconds (1500-2500).
## A value of 0 disables servo output.
##
## @return -1 if error.
def setServo(self, index, value):
if index > 7: return -1
if value != 0 and (value < 500 or value > 2500):
print("ArbotiX Error: Servo value out of range:", value)
else:
self.write(253, self._SERVO_BASE + 2*index, [value%256, value>>8])
return 0

104
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/ax12.py Normal file
View File

@ -0,0 +1,104 @@
#!/usr/bin/env python
# Copyright (c) 2008-2011 Vanadium Labs LLC.
# All right reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of Vanadium Labs LLC nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
# Author: Michael Ferguson
## @file ax12.py Definitions of AX-12 control table.
# Control Table Symbolic Constants - EEPROM AREA
P_MODEL_NUMBER_L = 0
P_MODEL_NUMBER_H = 1
P_VERSION = 2
P_ID = 3
P_BAUD_RATE = 4
P_RETURN_DELAY_TIME = 5
P_CW_ANGLE_LIMIT_L = 6
P_CW_ANGLE_LIMIT_H = 7
P_CCW_ANGLE_LIMIT_L = 8
P_CCW_ANGLE_LIMIT_H = 9
P_SYSTEM_DATA2 = 10
P_LIMIT_TEMPERATURE = 11
P_DOWN_LIMIT_VOLTAGE = 12
P_UP_LIMIT_VOLTAGE = 13
P_MAX_TORQUE_L = 14
P_MAX_TORQUE_H = 15
P_RETURN_LEVEL = 16
P_ALARM_LED = 17
P_ALARM_SHUTDOWN = 18
P_OPERATING_MODE = 19
P_DOWN_CALIBRATION_L = 20
P_DOWN_CALIBRATION_H = 21
P_UP_CALIBRATION_L = 22
P_UP_CALIBRATION_H = 23
# Control Table Symbolic Constants - RAM AREA
P_TORQUE_ENABLE = 24
P_LED = 25
P_CW_COMPLIANCE_MARGIN = 26
P_CCW_COMPLIANCE_MARGIN = 27
P_CW_COMPLIANCE_SLOPE = 28
P_CCW_COMPLIANCE_SLOPE = 29
P_GOAL_POSITION_L = 30
P_GOAL_POSITION_H = 31
P_GOAL_SPEED_L = 32
P_GOAL_SPEED_H = 33
P_TORQUE_LIMIT_L = 34
P_TORQUE_LIMIT_H = 35
P_PRESENT_POSITION_L = 36
P_PRESENT_POSITION_H = 37
P_PRESENT_SPEED_L = 38
P_PRESENT_SPEED_H = 39
P_PRESENT_LOAD_L = 40
P_PRESENT_LOAD_H = 41
P_PRESENT_VOLTAGE = 42
P_PRESENT_TEMPERATURE = 43
P_REGISTERED_INSTRUCTION = 44
P_PAUSE_TIME = 45
P_MOVING = 46
P_LOCK = 47
P_PUNCH_L = 48
P_PUNCH_H = 49
# Status Return Levels
AX_RETURN_NONE = 0
AX_RETURN_READ = 1
AX_RETURN_ALL = 2
# Instruction Set
AX_PING = 1
AX_READ_DATA = 2
AX_WRITE_DATA = 3
AX_REG_WRITE = 4
AX_ACTION = 5
AX_RESET = 6
AX_SYNC_WRITE = 131
AX_SYNC_READ = 132
AX_CONTROL_SETUP = 26
AX_CONTROL_WRITE = 27
AX_CONTROL_STAT = 28

75
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/controllers.py Normal file
View File

@ -0,0 +1,75 @@
#!/usr/bin/env python
# Copyright (c) 2010-2011 Vanadium Labs LLC.
# All right reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of Vanadium Labs LLC nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
## @file controllers.py Base class and support functions for a controllers.
## @brief Controllers interact with ArbotiX hardware.
class Controller:
## @brief Constructs a Controller instance.
##
## @param device The arbotix instance.
##
## @param name The controller name.
def __init__(self, device, name):
self.name = name
self.device = device
self.fake = device.fake
self.pause = False
# output for joint states publisher
self.joint_names = list()
self.joint_positions = list()
self.joint_velocities = list()
## @brief Start the controller, do any hardware setup needed.
def startup(self):
pass
## @brief Do any read/writes to device.
def update(self):
pass
## @brief Stop the controller, do any hardware shutdown needed.
def shutdown(self):
pass
## @brief Is the controller actively sending commands to joints?
def active(self):
return False
## @brief Get a diagnostics message for this joint.
##
## @return Diagnostics message.
def getDiagnostics(self):
msg = DiagnosticStatus()
msg.name = self.name
msg.level = DiagnosticStatus.OK
msg.message = "OK"
return msg

261
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/diff_controller.py Normal file
View File

@ -0,0 +1,261 @@
#!/usr/bin/env python
"""
diff_controller.py - controller for a differential drive
Copyright (c) 2010-2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
from math import sin,cos,pi
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from diagnostic_msgs.msg import *
from tf.broadcaster import TransformBroadcaster
from arbotix_python.ax12 import *
from arbotix_python.controllers import *
from struct import unpack
class DiffController(Controller):
""" Controller to handle movement & odometry feedback for a differential
drive mobile base. """
def __init__(self, device, name):
Controller.__init__(self, device, name)
self.pause = True
self.last_cmd = rospy.Time.now()
# parameters: rates and geometry
self.rate = rospy.get_param('~controllers/'+name+'/rate',10.0)
self.timeout = rospy.get_param('~controllers/'+name+'/timeout',1.0)
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
self.ticks_meter = float(rospy.get_param('~controllers/'+name+'/ticks_meter'))
self.base_width = float(rospy.get_param('~controllers/'+name+'/base_width'))
self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', 'base_link')
self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom')
# parameters: PID
self.Kp = rospy.get_param('~controllers/'+name+'/Kp', 5)
self.Kd = rospy.get_param('~controllers/'+name+'/Kd', 1)
self.Ki = rospy.get_param('~controllers/'+name+'/Ki', 0)
self.Ko = rospy.get_param('~controllers/'+name+'/Ko', 50)
# parameters: acceleration
self.accel_limit = rospy.get_param('~controllers/'+name+'/accel_limit', 0.1)
self.max_accel = int(self.accel_limit*self.ticks_meter/self.rate)
# output for joint states publisher
self.joint_names = ["base_l_wheel_joint","base_r_wheel_joint"]
self.joint_positions = [0,0]
self.joint_velocities = [0,0]
# internal data
self.v_left = 0 # current setpoint velocity
self.v_right = 0
self.v_des_left = 0 # cmd_vel setpoint
self.v_des_right = 0
self.enc_left = None # encoder readings
self.enc_right = None
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now() # time for determining dx/dy
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
rospy.loginfo("Started DiffController ("+name+"). Geometry: " + str(self.base_width) + "m wide, " + str(self.ticks_meter) + " ticks/m.")
def startup(self):
if not self.fake:
self.setup(self.Kp,self.Kd,self.Ki,self.Ko)
def update(self):
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
if self.fake:
x = cos(self.th)*self.dx*elapsed
y = -sin(self.th)*self.dx*elapsed
self.x += cos(self.th)*self.dx*elapsed
self.y += sin(self.th)*self.dx*elapsed
self.th += self.dr*elapsed
else:
# read encoders
try:
left, right = self.status()
except Exception as e:
rospy.logerr("Could not update encoders: " + str(e))
return
rospy.logdebug("Encoders: " + str(left) +","+ str(right))
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (left - self.enc_left)/self.ticks_meter
d_right = (right - self.enc_right)/self.ticks_meter
self.enc_left = left
self.enc_right = right
d = (d_left+d_right)/2
th = (d_right-d_left)/self.base_width
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
x = cos(th)*d
y = -sin(th)*d
self.x = self.x + (cos(self.th)*x - sin(self.th)*y)
self.y = self.y + (sin(self.th)*x + cos(self.th)*y)
if (th != 0):
self.th = self.th + th
# publish or perish
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th/2)
quaternion.w = cos(self.th/2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
if now > (self.last_cmd + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
# update motors
if not self.fake:
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.write(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def shutdown(self):
if not self.fake:
self.write(0,0)
def cmdVelCb(self,req):
""" Handle movement requests. """
self.last_cmd = rospy.Time.now()
if self.fake:
self.dx = req.linear.x # m/s
self.dr = req.angular.z # rad/s
else:
# set motor speeds in ticks per 1/30s
self.v_des_left = int( ((req.linear.x - (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
self.v_des_right = int( ((req.linear.x + (req.angular.z * self.base_width/2.0)) * self.ticks_meter) / 30.0)
def getDiagnostics(self):
""" Get a diagnostics status. """
msg = DiagnosticStatus()
msg.name = self.name
msg.level = DiagnosticStatus.OK
msg.message = "OK"
if not self.fake:
msg.values.append(KeyValue("Left", str(self.enc_left)))
msg.values.append(KeyValue("Right", str(self.enc_right)))
msg.values.append(KeyValue("dX", str(self.dx)))
msg.values.append(KeyValue("dR", str(self.dr)))
return msg
###
### Controller Specification:
###
### setup: Kp, Kd, Ki, Ko (all unsigned char)
###
### write: left_speed, right_speed (2-byte signed, ticks per frame)
###
### status: left_enc, right_enc (4-byte signed)
###
def setup(self, kp, kd, ki, ko):
success = self.device.execute(253, AX_CONTROL_SETUP, [10, kp, kd, ki, ko])
def write(self, left, right):
""" Send a closed-loop speed. Base PID loop runs at 30Hz, these values
are therefore in ticks per 1/30 second. """
left = left&0xffff
right = right&0xffff
success = self.device.execute(253, AX_CONTROL_WRITE, [10, left%256, left>>8, right%256, right>>8])
def status(self):
""" read 32-bit (signed) encoder values. """
values = self.device.execute(253, AX_CONTROL_STAT, [10])
left_values = "".join([chr(k) for k in values[0:4] ])
right_values = "".join([chr(k) for k in values[4:] ])
try:
left = unpack('=l',left_values)[0]
right = unpack('=l',right_values)[0]
return [left, right]
except:
return None

176
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/follow_controller.py Normal file
View File

@ -0,0 +1,176 @@
#!/usr/bin/env python
"""
follow_controller.py - controller for a kinematic chain
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy, actionlib
from control_msgs.msg import FollowJointTrajectoryAction
from trajectory_msgs.msg import JointTrajectory
from diagnostic_msgs.msg import *
from arbotix_python.ax12 import *
from arbotix_python.controllers import *
class FollowController(Controller):
""" A controller for joint chains, exposing a FollowJointTrajectory action. """
def __init__(self, device, name):
Controller.__init__(self, device, name)
self.interpolating = 0
# parameters: rates and joints
self.rate = rospy.get_param('~controllers/'+name+'/rate',50.0)
self.joints = rospy.get_param('~controllers/'+name+'/joints')
self.index = rospy.get_param('~controllers/'+name+'/index', len(device.controllers))
for joint in self.joints:
self.device.joints[joint].controller = self
# action server
name = rospy.get_param('~controllers/'+name+'/action_name','follow_joint_trajectory')
self.server = actionlib.SimpleActionServer(name, FollowJointTrajectoryAction, execute_cb=self.actionCb, auto_start=False)
# good old trajectory
rospy.Subscriber(self.name+'/command', JointTrajectory, self.commandCb)
self.executing = False
rospy.loginfo("Started FollowController ("+self.name+"). Joints: " + str(self.joints) + " on C" + str(self.index))
def startup(self):
self.server.start()
def actionCb(self, goal):
rospy.loginfo(self.name + ": Action goal recieved.")
traj = goal.trajectory
if set(self.joints) != set(traj.joint_names):
for j in self.joints:
if j not in traj.joint_names:
msg = "Trajectory joint names does not match action controlled joints." + str(traj.joint_names)
rospy.logerr(msg)
self.server.set_aborted(text=msg)
return
rospy.logwarn("Extra joints in trajectory")
if not traj.points:
msg = "Trajectory empy."
rospy.logerr(msg)
self.server.set_aborted(text=msg)
return
try:
indexes = [traj.joint_names.index(joint) for joint in self.joints]
except ValueError as val:
msg = "Trajectory invalid."
rospy.logerr(msg)
self.server.set_aborted(text=msg)
return
retval = self.executeTrajectory(traj) # retval: 1: successful, 0: canceled, -1: failed
if retval == 1:
self.server.set_succeeded()
rospy.loginfo(self.name + ": Done.")
elif retval == 0:
self.server.set_preempted(text="Goal canceled.")
rospy.loginfo(self.name + ": Goal canceled.")
else:
self.server.set_aborted(text="Execution failed.")
rospy.loginfo(self.name + ": Execution failed.")
def commandCb(self, msg):
# don't execute if executing an action
if self.server.is_active():
rospy.loginfo(self.name+": Received trajectory, but action is active")
return
self.executing = True
self.executeTrajectory(msg)
self.executing = False
def executeTrajectory(self, traj):
rospy.loginfo("Executing trajectory")
rospy.logdebug(traj)
# carry out trajectory
try:
indexes = [traj.joint_names.index(joint) for joint in self.joints]
except ValueError as val:
rospy.logerr("Invalid joint in trajectory.")
return -1
# get starting timestamp, MoveIt uses 0, need to fill in
start = traj.header.stamp
if start.secs == 0 and start.nsecs == 0:
start = rospy.Time.now()
r = rospy.Rate(self.rate)
last = [ self.device.joints[joint].position for joint in self.joints ]
for point in traj.points:
while rospy.Time.now() + rospy.Duration(0.01) < start:
if self.server.is_preempt_requested():
return 0
rospy.sleep(0.01)
desired = [ point.positions[k] for k in indexes ]
endtime = start + point.time_from_start
while rospy.Time.now() + rospy.Duration(0.01) < endtime:
# check that preempt has not been requested by the client
if self.server.is_preempt_requested():
return 0
err = [ (d-c) for d,c in zip(desired,last) ]
velocity = [ abs(x / (self.rate * (endtime - rospy.Time.now()).to_sec())) for x in err ]
rospy.logdebug(err)
for i in range(len(self.joints)):
if err[i] > 0.001 or err[i] < -0.001:
cmd = err[i]
top = velocity[i]
if cmd > top:
cmd = top
elif cmd < -top:
cmd = -top
last[i] += cmd
self.device.joints[self.joints[i]].setControlOutput(last[i])
else:
velocity[i] = 0
r.sleep()
return 1
def active(self):
""" Is controller overriding servo internal control? """
return self.server.is_active() or self.executing
def getDiagnostics(self):
""" Get a diagnostics status. """
msg = DiagnosticStatus()
msg.name = self.name
msg.level = DiagnosticStatus.OK
msg.message = "OK"
if self.active():
msg.values.append(KeyValue("State", "Active"))
else:
msg.values.append(KeyValue("State", "Not Active"))
return msg

87
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/io.py Normal file
View File

@ -0,0 +1,87 @@
#!/usr/bin/env python
"""
io.py - ROS wrappers for ArbotiX I/O
Copyright (c) 2010-2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
from arbotix_msgs.msg import *
class DigitalServo:
""" Class for a digital output. """
def __init__(self, name, pin, value, rate, device):
self.device = device
self.value = value
self.direction = 0
self.pin = pin
self.device.setDigital(self.pin, self.value, self.direction)
rospy.Subscriber('~'+name, Digital, self.stateCb)
self.t_delta = rospy.Duration(1.0/rate)
self.t_next = rospy.Time.now() + self.t_delta
def stateCb(self, msg):
self.value = msg.value
self.direction = msg.direction
def update(self):
if rospy.Time.now() > self.t_next:
self.device.setDigital(self.pin, self.value, self.direction)
self.t_next = rospy.Time.now() + self.t_delta
class DigitalSensor:
""" Class for a digital input. """
def __init__(self, name, pin, value, rate, device):
self.device = device
self.pin = pin
self.device.setDigital(pin, value, 0)
self.pub = rospy.Publisher('~'+name, Digital, queue_size=5)
self.t_delta = rospy.Duration(1.0/rate)
self.t_next = rospy.Time.now() + self.t_delta
def update(self):
if rospy.Time.now() > self.t_next:
msg = Digital()
msg.header.stamp = rospy.Time.now()
msg.value = self.device.getDigital(self.pin)
self.pub.publish(msg)
self.t_next = rospy.Time.now() + self.t_delta
class AnalogSensor:
""" Class for an analog input. """
def __init__(self, name, pin, value, rate, leng, device):
self.device = device
self.pin = pin
self.device.setDigital(pin, value, 0)
self.pub = rospy.Publisher('~'+name, Analog, queue_size=5)
self.t_delta = rospy.Duration(1.0/rate)
self.t_next = rospy.Time.now() + self.t_delta
self.leng = leng
def update(self):
if rospy.Time.now() > self.t_next:
msg = Analog()
msg.header.stamp = rospy.Time.now()
msg.value = self.device.getAnalog(self.pin, self.leng)
if msg.value >= 0:
self.pub.publish(msg)
self.t_next = rospy.Time.now() + self.t_delta

160
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/joints.py Normal file
View File

@ -0,0 +1,160 @@
#!/usr/bin/env python
# Copyright (c) 2010-2011 Vanadium Labs LLC.
# All right reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of Vanadium Labs LLC nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
## @file joints.py Base class and support functions for joints.
## @brief Joints hold current values.
class Joint:
## @brief Constructs a Joint instance.
##
## @param device The arbotix instance.
##
## @param name The joint name.
def __init__(self, device, name):
self.device = device
self.name = name
self.controller = None
self.position = 0.0
self.velocity = 0.0
self.last = rospy.Time.now()
## @brief Get new output, in raw data format.
##
## @param frame The frame length in seconds to interpolate forward.
##
## @return The new output, in raw data format.
def interpolate(self, frame):
return None
## @brief Set the current position from feedback data.
##
## @param raw_data The current feedback.
##
## @return The current position, in radians/meters.
def setCurrentFeedback(self, raw_data):
return None
## @brief Set the goal position.
##
## @param position The goal position, in radians/meters.
##
## @return The output position, in raw data format.
def setControlOutput(self, position):
return None
## @brief Get a diagnostics message for this joint.
##
## @return Diagnostics message.
def getDiagnostics(self):
return None
import rospy
import xml.dom.minidom
from math import pi, radians
## @brief Get joint parameters from URDF
def getJointsFromURDF():
try:
description = rospy.get_param("robot_description")
robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
joints = {}
# Find all non-fixed joints
for child in robot.childNodes:
if child.nodeType is child.TEXT_NODE:
continue
if child.localName == 'joint':
jtype = child.getAttribute('type')
if jtype == 'fixed':
continue
name = child.getAttribute('name')
if jtype == 'continuous':
minval = -pi
maxval = pi
else:
limit = child.getElementsByTagName('limit')[0]
minval = float(limit.getAttribute('lower'))
maxval = float(limit.getAttribute('upper'))
if minval > 0 or maxval < 0:
zeroval = (maxval + minval)/2
else:
zeroval = 0
joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
joints[name] = joint
return joints
except:
rospy.loginfo('No URDF defined, proceeding with defaults')
return dict()
## @brief Get limits of servo, from YAML, then URDF, then defaults if neither is defined.
def getJointLimits(name, joint_defaults, default_min=-150, default_max=150):
min_angle = radians(default_min)
max_angle = radians(default_max)
try:
min_angle = joint_defaults[name]['min']
except:
pass
try:
min_angle = radians(rospy.get_param("/arbotix/dynamixels/"+name+"/min_angle"))
except:
pass
try:
min_angle = radians(rospy.get_param("/arbotix/joints/"+name+"/min_angle"))
except:
pass
try:
min_angle = rospy.get_param("/arbotix/joints/"+name+"/min_position")
except:
pass
try:
max_angle = joint_defaults[name]['max']
except:
pass
try:
max_angle = radians(rospy.get_param("/arbotix/dynamixels/"+name+"/max_angle"))
except:
pass
try:
max_angle = radians(rospy.get_param("/arbotix/joints/"+name+"/max_angle"))
except:
pass
try:
max_angle = rospy.get_param("/arbotix/joints/"+name+"/max_position")
except:
pass
return (min_angle, max_angle)

285
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/linear_controller.py Normal file
View File

@ -0,0 +1,285 @@
#!/usr/bin/env python
"""
linear_controller.py - controller for a linear actuator with analog feedback
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy, actionlib
from arbotix_python.joints import *
from arbotix_python.controllers import *
from std_msgs.msg import Float64
from diagnostic_msgs.msg import *
from std_srvs.srv import *
from struct import unpack
class LinearJoint(Joint):
def __init__(self, device, name):
Joint.__init__(self, device, name)
self.dirty = False
self.position = 0.0 # current position, as returned by feedback (meters)
self.desired = 0.0 # desired position (meters)
self.velocity = 0.0 # moving speed
self.last = rospy.Time.now()
# TODO: load these from URDF
self.min = rospy.get_param('~joints/'+name+'/min_position',0.0)
self.max = rospy.get_param('~joints/'+name+'/max_position',0.5)
self.max_speed = rospy.get_param('~joints/'+name+'/max_speed',0.0508)
# calibration data {reading: position}
self.cal = { -1: -1, 1: 1 }
self.cal_raw = rospy.get_param('~joints/'+name+'/calibration_data', self.cal)
self.cal = dict()
for key, value in self.cal_raw.items():
self.cal[int(key)] = value
self.keys = sorted(self.cal.keys())
rospy.Subscriber(name+'/command', Float64, self.commandCb)
def interpolate(self, frame):
""" Get new output: 1 = increase position, -1 is decrease position. """
if self.dirty:
cmd = self.desired - self.position
if self.device.fake:
self.position = self.desired
self.dirty = False
return None
if cmd > 0.01:
return 1
elif cmd < -0.01:
return -1
else:
self.dirty = False
return 0
else:
return None
def setCurrentFeedback(self, reading):
if reading >= self.keys[0] and reading <= self.keys[-1]:
last_angle = self.position
self.position = self.readingToPosition(reading)
# update velocity estimate
t = rospy.Time.now()
self.velocity = (self.position - last_angle)/((t - self.last).to_nsec()/1000000000.0)
self.last = t
else:
rospy.logerr(self.name + ": feedback reading out of range")
def setControlOutput(self, position):
""" Set the position that controller is moving to.
Returns output value in raw_data format. """
if position <= self.max and position >= self.min:
self.desired = position
self.dirty = True
else:
rospy.logerr(self.name + ": requested position is out of range: " + str(position))
return None # TODO
def getDiagnostics(self):
""" Get a diagnostics status. """
msg = DiagnosticStatus()
msg.name = self.name
msg.level = DiagnosticStatus.OK
if self.dirty:
msg.message = "Moving"
else:
msg.message = "OK"
msg.values.append(KeyValue("Position", str(self.position)))
return msg
def commandCb(self, req):
""" Float64 style command input. """
if self.device.fake:
self.position = req.data
else:
if req.data <= self.max and req.data >= self.min:
self.desired = req.data
self.dirty = True
else:
rospy.logerr(self.name + ": requested position is out of range: " + str(req))
def readingToPosition(self, reading):
low = 0
while reading > self.keys[low+1]:
low += 1
high = len(self.keys) - 1
while reading < self.keys[high-1]:
high += -1
x = self.keys[high] - self.keys[low]
y = self.cal[self.keys[high]] - self.cal[self.keys[low]]
x1 = reading - self.keys[low]
y1 = y * ( float(x1)/float(x) )
return self.cal[self.keys[low]] + y1
class LinearControllerAbsolute(Controller):
""" A controller for a linear actuator, with absolute positional feedback. """
def __init__(self, device, name):
Controller.__init__(self, device, name)
self.a = rospy.get_param('~controllers/'+name+'/motor_a',29)
self.b = rospy.get_param('~controllers/'+name+'/motor_b',30)
self.p = rospy.get_param('~controllers/'+name+'/motor_pwm',31)
self.analog = rospy.get_param('~controllers/'+name+'/feedback',0)
self.last = 0
self.last_reading = 0
self.delta = rospy.Duration(1.0/rospy.get_param('~controllers/'+name+'/rate', 10.0))
self.next = rospy.Time.now() + self.delta
self.joint = device.joints[rospy.get_param('~controllers/'+name+'/joint')]
rospy.loginfo("Started LinearController ("+self.name+").")
def startup(self):
if not self.fake:
self.joint.setCurrentFeedback(self.device.getAnalog(self.analog))
def update(self):
now = rospy.Time.now()
if now > self.next:
# read current position
if self.joint.dirty:
if not self.fake:
try:
self.last_reading = self.getPosition()
self.joint.setCurrentFeedback(self.last_reading)
except Exception as e:
print("linear error: ", e)
# update movement
output = self.joint.interpolate(1.0/self.delta.to_sec())
if self.last != output and not self.fake:
self.setSpeed(output)
self.last = output
self.next = now + self.delta
def setSpeed(self, speed):
""" Set speed of actuator. """
if speed > 0:
self.device.setDigital(self.a, 0); self.device.setDigital(self.b, 1); # up
self.device.setDigital(self.p, 1)
elif speed < 0:
self.device.setDigital(self.a, 1); self.device.setDigital(self.b, 0); # down
self.device.setDigital(self.p, 1)
else:
self.device.setDigital(self.p, 0)
def getPosition(self):
return self.device.getAnalog(self.analog)
def shutdown(self):
if not self.fake:
self.device.setDigital(self.p, 0)
def getDiagnostics(self):
""" Get a diagnostics status. """
msg = DiagnosticStatus()
msg.name = self.name
msg.level = DiagnosticStatus.OK
msg.message = "OK"
if not self.fake:
msg.values.append(KeyValue("Encoder Reading", str(self.last_reading)))
return msg
class LinearControllerIncremental(LinearControllerAbsolute):
""" A controller for a linear actuator, without absolute encoder. """
POSITION_L = 100
POSITION_H = 101
DIRECTION = 102
def __init__(self, device, name):
Controller.__init__(self, device, name)
self.pause = True
self.a = rospy.get_param('~controllers/'+name+'/motor_a',29)
self.b = rospy.get_param('~controllers/'+name+'/motor_b',30)
self.p = rospy.get_param('~controllers/'+name+'/motor_pwm',31)
self.last = 0
self.last_reading = 0
self.delta = rospy.Duration(1.0/rospy.get_param('~controllers/'+name+'/rate', 10.0))
self.next = rospy.Time.now() + self.delta
self.joint = device.joints[rospy.get_param('~controllers/'+name+'/joint')]
rospy.Service(name+'/zero', Empty, self.zeroCb)
rospy.loginfo("Started LinearControllerIncremental ("+self.name+").")
def startup(self):
if not self.fake:
self.zeroEncoder()
def setSpeed(self, speed):
""" Set speed of actuator. We need to set direction for encoder. """
if speed > 0:
self.device.write(253, self.DIRECTION, [1])
self.device.setDigital(self.a, 0); self.device.setDigital(self.b, 1); # up
self.device.setDigital(self.p, 1)
elif speed < 0:
self.device.write(253, self.DIRECTION, [0])
self.device.setDigital(self.a, 1); self.device.setDigital(self.b, 0); # down
self.device.setDigital(self.p, 1)
else:
self.device.setDigital(self.p, 0)
def getPosition(self):
return unpack('=h', "".join([chr(k) for k in self.device.read(253, self.POSITION_L, 2)]) )[0]
def zeroEncoder(self, timeout=15.0):
rospy.loginfo(self.name + ': zeroing encoder')
self.setSpeed(1)
last_pos = None
for i in range(int(timeout)):
if rospy.is_shutdown():
return
try:
new_pos = self.getPosition()
except:
pass
if last_pos == new_pos:
break
last_pos = new_pos
rospy.sleep(1)
self.setSpeed(0)
self.device.write(253, self.POSITION_L, [0, 0])
self.joint.setCurrentFeedback(0)
def zeroCb(self, msg):
if not self.fake:
self.zeroEncoder(15.0)
return EmptyResponse()
def shutdown(self):
if not self.fake:
self.setSpeed(0)

92
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/publishers.py Normal file
View File

@ -0,0 +1,92 @@
#!/usr/bin/env python
"""
diagnostics.py - diagnostic output code
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
from diagnostic_msgs.msg import DiagnosticArray
from sensor_msgs.msg import JointState
class DiagnosticsPublisher:
""" Class to handle publications of joint_states message. """
def __init__(self):
self.t_delta = rospy.Duration(1.0/rospy.get_param("~diagnostic_rate", 1.0))
self.t_next = rospy.Time.now() + self.t_delta
self.pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=5)
def update(self, joints, controllers):
""" Publish diagnostics. """
now = rospy.Time.now()
if now > self.t_next:
# create message
msg = DiagnosticArray()
msg.header.stamp = now
for controller in controllers:
d = controller.getDiagnostics()
if d:
msg.status.append(d)
for joint in joints:
d = joint.getDiagnostics()
if d:
msg.status.append(d)
# publish and update stats
self.pub.publish(msg)
self.t_next = now + self.t_delta
class JointStatePublisher:
""" Class to handle publications of joint_states message. """
def __init__(self):
# parameters: throttle rate and geometry
self.rate = rospy.get_param("~read_rate", 10.0)
self.t_delta = rospy.Duration(1.0/self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# subscriber
self.pub = rospy.Publisher('joint_states', JointState, queue_size=5)
def update(self, joints, controllers):
""" publish joint states. """
if rospy.Time.now() > self.t_next:
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = list()
msg.position = list()
msg.velocity = list()
for joint in joints:
msg.name.append(joint.name)
msg.position.append(joint.position)
msg.velocity.append(joint.velocity)
for controller in controllers:
msg.name += controller.joint_names
msg.position += controller.joint_positions
msg.velocity += controller.joint_velocities
self.pub.publish(msg)
self.t_next = rospy.Time.now() + self.t_delta

470
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_python/src/arbotix_python/servo_controller.py Normal file
View File

@ -0,0 +1,470 @@
#!/usr/bin/env python
"""
servo_controller.py: classes for servo interaction
Copyright (c) 2011-2013 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
from math import radians
from std_msgs.msg import Float64
from arbotix_msgs.srv import *
from diagnostic_msgs.msg import *
from arbotix_python.ax12 import *
from arbotix_python.joints import *
class DynamixelServo(Joint):
def __init__(self, device, name, ns="~joints"):
Joint.__init__(self, device, name)
n = ns+"/"+name+"/"
self.id = int(rospy.get_param(n+"id"))
self.ticks = rospy.get_param(n+"ticks", 1024)
self.neutral = rospy.get_param(n+"neutral", self.ticks/2)
if self.ticks == 4096:
self.range = 360.0
else:
self.range = 300.0
self.range = rospy.get_param(n+"range", self.range)
self.rad_per_tick = radians(self.range)/self.ticks
# TODO: load these from URDF
self.max_angle = radians(rospy.get_param(n+"max_angle",self.range/2.0))
self.min_angle = radians(rospy.get_param(n+"min_angle",-self.range/2.0))
self.max_speed = radians(rospy.get_param(n+"max_speed",684.0))
# max speed = 114 rpm - 684 deg/s
self.invert = rospy.get_param(n+"invert",False)
self.readable = rospy.get_param(n+"readable",True)
self.status = "OK"
self.level = DiagnosticStatus.OK
self.dirty = False # newly updated position?
self.position = 0.0 # current position, as returned by servo (radians)
self.desired = 0.0 # desired position (radians)
self.last_cmd = 0.0 # last position sent (radians)
self.velocity = 0.0 # moving speed
self.enabled = True # can we take commands?
self.active = False # are we under torque control?
self.last = rospy.Time.now()
self.reads = 0.0 # number of reads
self.errors = 0 # number of failed reads
self.total_reads = 0.0
self.total_errors = [0.0]
self.voltage = 0.0
self.temperature = 0.0
# ROS interfaces
rospy.Subscriber(name+'/command', Float64, self.commandCb)
rospy.Service(name+'/relax', Relax, self.relaxCb)
rospy.Service(name+'/enable', Enable, self.enableCb)
rospy.Service(name+'/set_speed', SetSpeed, self.setSpeedCb)
def interpolate(self, frame):
""" Get the new position to move to, in ticks. """
if self.enabled and self.active and self.dirty:
# compute command, limit velocity
cmd = self.desired - self.last_cmd
if cmd > self.max_speed/frame:
cmd = self.max_speed/frame
elif cmd < -self.max_speed/frame:
cmd = -self.max_speed/frame
# compute angle, apply limits
ticks = self.angleToTicks(self.last_cmd + cmd)
self.last_cmd = self.ticksToAngle(ticks)
self.speed = cmd*frame
# cap movement
if self.last_cmd == self.desired:
self.dirty = False
# when fake, need to set position/velocity here
if self.device.fake:
last_angle = self.position
self.position = self.last_cmd
t = rospy.Time.now()
self.velocity = (self.position - last_angle)/((t - self.last).to_nsec()/1000000000.0)
self.last = t
return None
return int(ticks)
else:
# when fake, need to reset velocity to 0 here.
if self.device.fake:
self.velocity = 0.0
self.last = rospy.Time.now()
return None
def setCurrentFeedback(self, reading):
""" Update angle in radians by reading from servo, or by
using position passed in from a sync read (in ticks). """
if reading > -1 and reading < self.ticks: # check validity
self.reads += 1
self.total_reads += 1
last_angle = self.position
self.position = self.ticksToAngle(reading)
# update velocity estimate
t = rospy.Time.now()
self.velocity = (self.position - last_angle)/((t - self.last).to_nsec()/1000000000.0)
self.last = t
else:
rospy.logdebug("Invalid read of servo: id " + str(self.id) + ", value " + str(reading))
self.errors += 1
self.total_reads += 1
return
if not self.active:
self.last_cmd = self.position
def setControlOutput(self, position):
""" Set the position that controller is moving to.
Returns output value in ticks. """
if self.enabled:
ticks = self.angleToTicks(position)
self.desired = position
self.dirty = True
self.active = True
return int(ticks)
return -1
def getDiagnostics(self):
""" Get a diagnostics status. """
msg = DiagnosticStatus()
msg.name = self.name
if self.temperature > 60: # TODO: read this value from eeprom
self.status = "OVERHEATED, SHUTDOWN"
self.level = DiagnosticStatus.ERROR
elif self.temperature > 50 and self.status != "OVERHEATED, SHUTDOWN":
self.status = "OVERHEATING"
self.level = DiagnosticStatus.WARN
elif self.status != "OVERHEATED, SHUTDOWN":
self.status = "OK"
self.level = DiagnosticStatus.OK
msg.level = self.level
msg.message = self.status
msg.values.append(KeyValue("Position", str(self.position)))
msg.values.append(KeyValue("Temperature", str(self.temperature)))
msg.values.append(KeyValue("Voltage", str(self.voltage)))
if self.reads + self.errors > 100:
self.total_errors.append((self.errors*100.0)/(self.reads+self.errors))
if len(self.total_errors) > 10:
self.total_errors = self.total_errors[-10:]
self.reads = 0
self.errors = 0
msg.values.append(KeyValue("Reads", str(self.total_reads)))
msg.values.append(KeyValue("Error Rate", str(sum(self.total_errors)/len(self.total_errors))+"%" ))
if self.active:
msg.values.append(KeyValue("Torque", "ON"))
else:
msg.values.append(KeyValue("Torque", "OFF"))
return msg
def angleToTicks(self, angle):
""" Convert an angle to ticks, applying limits. """
ticks = self.neutral + (angle/self.rad_per_tick)
if self.invert:
ticks = self.neutral - (angle/self.rad_per_tick)
if ticks >= self.ticks:
return self.ticks-1.0
if ticks < 0:
return 0
return ticks
def ticksToAngle(self, ticks):
""" Convert an ticks to angle, applying limits. """
angle = (ticks - self.neutral) * self.rad_per_tick
if self.invert:
angle = -1.0 * angle
return angle
def speedToTicks(self, rads_per_sec):
""" Convert speed in radians per second to ticks, applying limits. """
ticks = self.ticks * rads_per_sec / self.max_speed
if ticks >= self.ticks:
return self.ticks-1.0
if ticks < 0:
return 0
return ticks
def enableCb(self, req):
""" Turn on/off servo torque, so that it is pose-able. """
if req.enable:
self.enabled = True
else:
if not self.device.fake:
self.device.disableTorque(self.id)
self.dirty = False
self.enabled = False
self.active = False
return EnableResponse(self.enabled)
def relaxCb(self, req):
""" Turn off servo torque, so that it is pose-able. """
if not self.device.fake:
self.device.disableTorque(self.id)
self.dirty = False
self.active = False
return RelaxResponse()
def commandCb(self, req):
""" Float64 style command input. """
if self.enabled:
if self.controller and self.controller.active():
# Under and action control, do not interfere
return
elif self.desired != req.data or not self.active:
self.dirty = True
self.active = True
self.desired = req.data
def setSpeedCb(self, req):
""" Set servo speed. Requested speed is in radians per second.
Don't allow 0 which means "max speed" to a Dynamixel in joint mode. """
if not self.device.fake:
ticks_per_sec = max(1, int(self.speedToTicks(req.speed)))
self.device.setSpeed(self.id, ticks_per_sec)
return SetSpeedResponse()
class HobbyServo(Joint):
def __init__(self, device, name, ns="~joints"):
Joint.__init__(self, device, name)
n = ns+"/"+name+"/"
self.id = int(rospy.get_param(n+"id"))
self.ticks = rospy.get_param(n+"ticks", 2000)
self.neutral = rospy.get_param(n+"neutral", 1500)
self.range = rospy.get_param(n+"range", 180)
self.rad_per_tick = radians(self.range)/self.ticks
# TODO: load these from URDF
self.max_angle = radians(rospy.get_param(n+"max_angle",self.range/2.0))
self.min_angle = radians(rospy.get_param(n+"min_angle",-self.range/2.0))
self.max_speed = radians(rospy.get_param(n+"max_speed",90.0))
self.invert = rospy.get_param(n+"invert",False)
self.dirty = False # newly updated position?
self.position = 0.0 # current position, as returned by servo (radians)
self.desired = 0.0 # desired position (radians)
self.last_cmd = 0.0 # last position sent (radians)
self.velocity = 0.0 # moving speed
self.last = rospy.Time.now()
# ROS interfaces
rospy.Subscriber(name+'/command', Float64, self.commandCb)
def interpolate(self, frame):
""" Get the new position to move to, in ticks. """
if self.dirty:
# compute command, limit velocity
cmd = self.desired - self.last_cmd
if cmd > self.max_speed/frame:
cmd = self.max_speed/frame
elif cmd < -self.max_speed/frame:
cmd = -self.max_speed/frame
# compute angle, apply limits
ticks = self.angleToTicks(self.last_cmd + cmd)
self.last_cmd = self.ticksToAngle(ticks)
self.speed = cmd*frame
# cap movement
if self.last_cmd == self.desired:
self.dirty = False
if self.device.fake:
self.position = self.last_cmd
return None
return int(ticks)
else:
return None
def setCurrentFeedback(self, raw_data):
""" Update angle in radians by reading from servo, or by
using position passed in from a sync read (in ticks). """
return None
def setControlOutput(self, position):
""" Set the position that controller is moving to.
Returns output value in ticks. """
ticks = self.angleToTicks(position)
self.desired = position
self.dirty = True
return int(ticks)
def getDiagnostics(self):
""" Get a diagnostics status. """
msg = DiagnosticStatus()
msg.name = self.name
msg.level = DiagnosticStatus.OK
msg.message = "OK"
msg.values.append(KeyValue("Position", str(self.position)))
return msg
def angleToTicks(self, angle):
""" Convert an angle to ticks, applying limits. """
ticks = self.neutral + (angle/self.rad_per_tick)
if self.invert:
ticks = self.neutral - (angle/self.rad_per_tick)
if ticks >= self.ticks:
return self.ticks-1.0
if ticks < 0:
return 0
return ticks
def ticksToAngle(self, ticks):
""" Convert an ticks to angle, applying limits. """
angle = (ticks - self.neutral) * self.rad_per_tick
if self.invert:
angle = -1.0 * angle
return angle
def commandCb(self, req):
""" Float64 style command input. """
if self.controller and self.controller.active():
# Under and action control, do not interfere
return
else:
self.dirty = True
self.desired = req.data
from arbotix_python.controllers import *
class ServoController(Controller):
def __init__(self, device, name):
Controller.__init__(self, device, name)
self.dynamixels = list()
self.hobbyservos = list()
self.iter = 0
# steal some servos
for joint in device.joints.values():
if isinstance(joint, DynamixelServo):
self.dynamixels.append(joint)
elif isinstance(joint, HobbyServo):
self.hobbyservos.append(joint)
self.w_delta = rospy.Duration(1.0/rospy.get_param("~write_rate", 10.0))
self.w_next = rospy.Time.now() + self.w_delta
self.r_delta = rospy.Duration(1.0/rospy.get_param("~read_rate", 10.0))
self.r_next = rospy.Time.now() + self.r_delta
rospy.Service(name + '/relax_all', Relax, self.relaxCb)
rospy.Service(name + '/enable_all', Enable, self.enableCb)
def update(self):
""" Read servo positions, update them. """
if rospy.Time.now() > self.r_next and not self.fake:
if self.device.use_sync_read:
# arbotix/servostik/wifi board sync_read
synclist = list()
for joint in self.dynamixels:
if joint.readable:
synclist.append(joint.id)
if len(synclist) > 0:
val = self.device.syncRead(synclist, P_PRESENT_POSITION_L, 2)
if val:
for joint in self.dynamixels:
try:
i = synclist.index(joint.id)*2
joint.setCurrentFeedback(val[i]+(val[i+1]<<8))
except:
# not a readable servo
continue
else:
# direct connection, or other hardware with no sync_read capability
for joint in self.dynamixels:
joint.setCurrentFeedback(self.device.getPosition(joint.id))
self.r_next = rospy.Time.now() + self.r_delta
if rospy.Time.now() > self.w_next:
if self.device.use_sync_write and not self.fake:
syncpkt = list()
for joint in self.dynamixels:
v = joint.interpolate(1.0/self.w_delta.to_sec())
if v != None: # if was dirty
syncpkt.append([joint.id,int(v)%256,int(v)>>8])
if len(syncpkt) > 0:
self.device.syncWrite(P_GOAL_POSITION_L,syncpkt)
else:
for joint in self.dynamixels:
v = joint.interpolate(1.0/self.w_delta.to_sec())
if v != None: # if was dirty
self.device.setPosition(joint.id, int(v))
for joint in self.hobbyservos:
v = joint.interpolate(1.0/self.w_delta.to_sec())
if v != None: # if it was dirty
self.device.setServo(joint.id, v)
self.w_next = rospy.Time.now() + self.w_delta
def getDiagnostics(self):
""" Update status of servos (voltages, temperatures). """
if self.iter % 5 == 0 and not self.fake:
if self.device.use_sync_read:
# arbotix/servostik/wifi board sync_read
synclist = list()
for joint in self.dynamixels:
if joint.readable:
synclist.append(joint.id)
if len(synclist) > 0:
val = self.device.syncRead(synclist, P_PRESENT_VOLTAGE, 2)
if val:
for joint in self.dynamixels:
try:
i = synclist.index(joint.id)*2
if val[i] < 250:
joint.voltage = val[i]/10.0
if val[i+1] < 100:
joint.temperature = val[i+1]
except:
# not a readable servo
continue
else:
# direct connection, or other hardware with no sync_read capability
for joint in self.dynamixels:
if joint.readable:
val = self.device.read(joint.id, P_PRESENT_VOLTAGE, 2)
try:
joint.voltage = val[0]
joint.temperature = val[1]
except:
continue
self.iter += 1
return None
def enableCb(self, req):
""" Turn on/off all servos torque, so that they are pose-able. """
for joint in self.dynamixels:
resp = joint.enableCb(req)
return resp
def relaxCb(self, req):
""" Turn off all servos torque, so that they are pose-able. """
for joint in self.dynamixels:
resp = joint.relaxCb(req)
return resp

36
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_sensors/CHANGELOG.rst Normal file
View File

@ -0,0 +1,36 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package arbotix_sensors
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.11.0 (2020-12-29)
-------------------
* Update all python shebangs to python3 + rosdep dependency (`#48 <https://github.com/vanadiumlabs/arbotix_ros/issues/48>`_)
Co-authored-by: Murat Calis <mc@pirate-robotics.net>
* Merge pull request `#22 <https://github.com/vanadiumlabs/arbotix_ros/issues/22>`_ from corot/indigo-devel
roslib.load_manifest should not be used on catkin packages
* roslib.load_manifest should not be used on catkin packages according to http://wiki.ros.org/PyStyleGuide
* Contributors: Jorge Santos, Michael Ferguson, calismurat
0.10.0 (2014-07-14)
-------------------
* Set queue_size=5 on all publishers
* Contributors: Jorge Santos
0.9.2 (2014-02-12)
------------------
0.9.1 (2014-01-28)
------------------
0.9.0 (2013-08-22)
------------------
0.8.2 (2013-03-28)
------------------
0.8.1 (2013-03-09)
------------------
0.8.0 (2013-02-21)
------------------
* import drivers and catkinize

10
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_sensors/CMakeLists.txt Normal file
View File

@ -0,0 +1,10 @@
cmake_minimum_required(VERSION 3.0.2)
project(arbotix_sensors)
find_package(catkin REQUIRED)
catkin_package()
catkin_python_setup()
install(PROGRAMS bin/ir_ranger.py bin/max_sonar.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

82
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_sensors/bin/ir_ranger.py Normal file
View File

@ -0,0 +1,82 @@
#!/usr/bin/env python
"""
ir_ranger.py - convert analog stream into range measurements
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
from sensor_msgs.msg import Range
from arbotix_msgs.msg import Analog
from arbotix_msgs.srv import SetupChannel, SetupChannelRequest
from arbotix_python.sensors import *
class ir_ranger:
def __init__(self):
rospy.init_node("ir_ranger")
# sensor type: choices are A710YK (40-216"), A02YK (8-60"), A21YK (4-30")
self.sensor_t = rospy.get_param("~type","GP2D12")
if self.sensor_t == "A710YK" or self.sensor_t == "ultralong":
self.sensor = gpA710YK()
elif self.sensor_t == "A02YK" or self.sensor_t == "long":
self.sensor = gpA02YK()
else:
self.sensor = gp2d12()
# start channel broadcast using SetupAnalogIn
rospy.wait_for_service('arbotix/SetupAnalogIn')
analog_srv = rospy.ServiceProxy('arbotix/SetupAnalogIn', SetupChannel)
req = SetupChannelRequest()
req.topic_name = rospy.get_param("~name")
req.pin = rospy.get_param("~pin")
req.rate = int(rospy.get_param("~rate",10))
analog_srv(req)
# setup a range message to use
self.msg = Range()
self.msg.radiation_type = self.sensor.radiation_type
self.msg.field_of_view = self.sensor.field_of_view
self.msg.min_range = self.sensor.min_range
self.msg.max_range = self.sensor.max_range
# publish/subscribe
self.pub = rospy.Publisher("ir_range", Range, queue_size=5)
rospy.Subscriber("arbotix/"+req.topic_name, Analog, self.readingCb)
rospy.spin()
def readingCb(self, msg):
# convert msg.value into range.range
self.msg.header.stamp = rospy.Time.now()
self.msg.range = self.sensor.convert(msg.value<<2)
self.pub.publish(self.msg)
if __name__=="__main__":
ir_ranger()

75
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_sensors/bin/max_sonar.py Normal file
View File

@ -0,0 +1,75 @@
#!/usr/bin/env python
"""
max_sonar.py - convert analog stream into range measurements
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
import rospy
from sensor_msgs.msg import Range
from arbotix_msgs.msg import Analog
from arbotix_msgs.srv import SetupChannel, SetupChannelRequest
from arbotix_python.sensors import *
class max_sonar:
def __init__(self):
rospy.init_node("max_sonar")
self.sensor = maxSonar()
# start channel broadcast using SetupAnalogIn
rospy.wait_for_service('arbotix/SetupAnalogIn')
analog_srv = rospy.ServiceProxy('arbotix/SetupAnalogIn', SetupChannel)
req = SetupChannelRequest()
req.topic_name = rospy.get_param("~name")
req.pin = rospy.get_param("~pin")
req.rate = int(rospy.get_param("~rate",10))
analog_srv(req)
# setup a range message to use
self.msg = Range()
self.msg.radiation_type = self.sensor.radiation_type
self.msg.field_of_view = self.sensor.field_of_view
self.msg.min_range = self.sensor.min_range
self.msg.max_range = self.sensor.max_range
# publish/subscribe
self.pub = rospy.Publisher("sonar_range", Range, queue_size=5)
rospy.Subscriber("arbotix/"+req.topic_name, Analog, self.readingCb)
rospy.spin()
def readingCb(self, msg):
# convert msg.value into range.range
self.msg.header.stamp = rospy.Time.now()
self.msg.range = self.sensor.convert(msg.value<<2)
self.pub.publish(self.msg)
if __name__=="__main__":
max_sonar()

15
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_sensors/package.xml Normal file
View File

@ -0,0 +1,15 @@
<package>
<name>arbotix_sensors</name>
<version>0.11.0</version>
<description>
Extends the arbotix_node package with a number of more sophisticated ROS wrappers for common devices.
</description>
<author>Michael Ferguson</author>
<maintainer email="mike@vanadiumlabs.com">Michael Ferguson</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/arbotix_sensors</url>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>arbotix_python</run_depend>
</package>

11
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_sensors/setup.py Normal file
View File

@ -0,0 +1,11 @@
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages=['arbotix_sensors'],
package_dir={'': 'src'},
)
setup(**d)

0
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_sensors/src/arbotix_sensors/__init__.py Normal file
View File

86
Code/RK3588/PIBot_ROS/ros_ws/src/arbotix_ros/arbotix_sensors/src/arbotix_sensors/sensors.py Normal file
View File

@ -0,0 +1,86 @@
#!/usr/bin/env python
"""
sensors.py - various conversions from raw analog to sensor range
Copyright (c) 2011 Vanadium Labs LLC. All right reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of Vanadium Labs LLC nor the names of its
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""
from sensor_msgs.msg import Range
class SharpIR:
radiation_type = Range.INFRARED
field_of_view = 0.001
min_range = 0.0
max_range = 1.0
def convert(self, raw):
""" Convert raw analog (10-bit) to distance. """
return raw
class gpA710YK(SharpIR):
""" Ultra long-range Sharp IR sensor. """
min_range = 0.75
max_range = 5.50
def convert(self, raw):
""" Convert raw analog (10-bit) to distance. """
if raw > 100:
return (497.0/(raw-56))
else:
return self.max_range+0.1
class gpA02YK(SharpIR):
min_range = 0.20
max_range = 1.50
def convert(self, raw):
""" Convert raw analog (10-bit) to distance. """
if raw > 80:
return (115.0/(raw-12))
else:
return self.max_range+0.1
class gp2d12(SharpIR):
""" The typical GP2D12 IR ranger. """
min_range = 0.10
max_range = 0.80
def convert(self, raw):
""" Convert raw analog (10-bit) to distance. """
if raw > 40:
return (52.0/(raw-12))
else:
return self.max_range+0.1
class maxSonar():
radiation_type = Range.ULTRASOUND
field_of_view = 0.785398163
min_range = 0.0
max_range = 6.4516
def convert(self, raw):
""" Convert raw analog (10-bit) to distance. """
return 12.7 * (raw/1024.0)

66
Code/RK3588/PIBot_ROS/ros_ws/src/common_utils/CMakeLists.txt Normal file
View File

@ -0,0 +1,66 @@
cmake_minimum_required(VERSION 3.0.2)
project(common_utils)
set(ROS_DISTRO $ENV{ROS_DISTRO})
if (${ROS_DISTRO} STREQUAL "kinetic")
add_compile_options(-std=c++11)
endif()
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
tf
)
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system thread)
find_package(OpenCV)
catkin_package(
INCLUDE_DIRS include
# LIBRARIES common_utils
# CATKIN_DEPENDS roscpp
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRES}
${Boost_INCLUDE_DIRES}
)
# Declare a C++ library
add_library(${PROJECT_NAME}
src/common_utils.cpp
)
# Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${Boost_LIBRARIES}
)
#############
## Install ##
#############
# Mark libraries for installation
# See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

78
Code/RK3588/PIBot_ROS/ros_ws/src/common_utils/include/common_utils/common_utils.h Normal file
View File

@ -0,0 +1,78 @@
#ifndef _COMMON_UTILS_H_
#define _COMMON_UTILS_H_
// ros//
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
// std//
#include <algorithm>
#include <array>
#include <deque>
#include <iostream>
#include <map>
#include <numeric>
#include <string>
#include <vector>
// opencv//
#include <opencv2/opencv.hpp>
#include <sys/stat.h>
#include <unistd.h>
using namespace ros;
using namespace std;
using namespace cv;
#define DEGREE_TO_RADIAN(X) X* CV_PI / 180.0f
#define RADIAN_TO_DEGREE(X) X * 180.0f / CV_PI
#define ANGLE_THRESHOLD 180.0f
#define SPARSE_PATH 1
enum LDSDirection { LDS_FRONT = 0,
LDS_LEFT,
LDS_BACK,
LDS_RIGHT };
enum RotateDirection {
CLOCKWISE, //角度减小//
ANTICLOCKWISE
};
// Regions//
struct CleanRegion {
CleanRegion(string name) {
regionName = name;
floorType = 0;
sweepType = 0;
regionPoints.clear();
}
string regionName;
int floorType;
int sweepType;
vector<Point> regionPoints;
};
// utils methods//
float calcDistance(const Point& p1, const Point& p2);
float calcDistance(const Point2f& p1, const Point2f& p2);
//两个点位之间角度//
float calcDegreeAngle(const Point& startPoint, const Point& endPoint);
float calcRadianAngle(const Point& startPoint, const Point& endPoint);
float calcDegreeAngle(const Point2f& startPoint, const Point2f& endPoint);
//计算两个角度差//
float calcAngleDiff(float angle1, float angle2);
//沿着路径的点，计算角度//
float calcPathAngle(const vector<Point>& pathPoints, int index);
RotateDirection calcRotateDirection(float robot_angle, float path_angle);
#endif

Some files were not shown because too many files have changed in this diff Show More