forked from ray/RobotKernal-UESTC
上传PIBot ROS相关代码
commit
29321792b8
|
|
@ -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
|
||||
Binary file not shown.
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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"
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
#!/bin/bash
|
||||
|
||||
sudo systemctl disable pibot
|
||||
sudo systemctl is-enabled pibot
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
#!/bin/bash
|
||||
/usr/bin/pibot-stop
|
||||
sleep 2
|
||||
/usr/bin/pibot-start
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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"
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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().
|
||||
"""
|
||||
|
|
@ -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")
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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(),
|
||||
}
|
||||
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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")
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1 @@
|
|||
# This file currently only serves to mark the location of a catkin workspace for tool integration
|
||||
|
|
@ -0,0 +1 @@
|
|||
ros_package
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
*.pyc
|
||||
.project
|
||||
.cproject
|
||||
.pydevproject
|
||||
.vscode
|
||||
|
|
@ -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.
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
## Arbotix Drivers
|
||||
|
||||
This repository contains the Arbotix ROS drivers, catkinized, and ready for ROS Noetic.
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(arbotix)
|
||||
find_package(catkin REQUIRED)
|
||||
catkin_metapackage()
|
||||
|
|
@ -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>
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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}
|
||||
)
|
||||
|
|
@ -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...')
|
||||
|
||||
|
|
@ -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...')
|
||||
|
||||
|
|
@ -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...")
|
||||
|
||||
|
|
@ -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...')
|
||||
|
||||
|
|
@ -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...")
|
||||
|
||||
|
|
@ -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...")
|
||||
|
||||
|
|
@ -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>
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(arbotix_firmware)
|
||||
|
||||
find_package(catkin REQUIRED)
|
||||
catkin_package()
|
||||
|
|
@ -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>
|
||||
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
# Reading from a single analog IO pin.
|
||||
Header header
|
||||
uint16 value
|
||||
|
|
@ -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
|
||||
|
|
@ -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>
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
bool enable
|
||||
---
|
||||
bool state
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
|
||||
---
|
||||
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
float64 speed
|
||||
---
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
# message to setup an IO channel
|
||||
string topic_name
|
||||
uint8 pin
|
||||
uint8 value
|
||||
uint8 rate
|
||||
---
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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}
|
||||
)
|
||||
|
|
@ -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()
|
||||
|
||||
|
|
@ -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()
|
||||
|
||||
|
|
@ -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...")
|
||||
|
||||
|
||||
|
|
@ -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'
|
||||
|
|
@ -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
|
||||
|
||||
*/
|
||||
|
|
@ -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>
|
||||
|
|
@ -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,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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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)
|
||||
|
||||
|
|
@ -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)
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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}
|
||||
)
|
||||
|
|
@ -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()
|
||||
|
||||
|
|
@ -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()
|
||||
|
||||
|
|
@ -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>
|
||||
|
|
@ -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,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)
|
||||
|
||||
|
|
@ -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}
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
|
@ -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
Loading…
Reference in New Issue