Compare commits
No commits in common. "6fbae252f9da36f718b8b395bb66dfc35d1e19a5" and "2c928adfe1935928144f916095765089e8610659" have entirely different histories.
6fbae252f9
...
2c928adfe1
|
|
@ -24,7 +24,3 @@ install
|
||||||
.tmp
|
.tmp
|
||||||
/spark_tutorials/spark_iflytek/msc/res/asr/GrmBuilld/
|
/spark_tutorials/spark_iflytek/msc/res/asr/GrmBuilld/
|
||||||
tensorflow
|
tensorflow
|
||||||
|
|
||||||
# workflow
|
|
||||||
Another team profiles/
|
|
||||||
20220904晚备份/
|
|
||||||
306
onekey.sh
306
onekey.sh
|
|
@ -9,8 +9,7 @@ export PATH
|
||||||
# SPARK技术讨论与反馈QQ群：6646169 8346256
|
# SPARK技术讨论与反馈QQ群：6646169 8346256
|
||||||
#=================================================
|
#=================================================
|
||||||
|
|
||||||
GAME_ENABLE="no"
|
GAME_ENABLE="yes"
|
||||||
ADV_GAME_ENABLE="no"
|
|
||||||
sh_ver="2.0"
|
sh_ver="2.0"
|
||||||
filepath=$(cd "$(dirname "$0")"; pwd)
|
filepath=$(cd "$(dirname "$0")"; pwd)
|
||||||
Green_font_prefix="\033[32m" && Red_font_prefix="\033[31m" && Green_background_prefix="\033[42;37m" && Red_background_prefix="\033[41;37m" && Yellow_background_prefix="\033[43;37m" && Font_color_suffix="\033[0m" && Yellow_font_prefix="\e[1;33m" && Blue_font_prefix="\e[0;34m"
|
Green_font_prefix="\033[32m" && Red_font_prefix="\033[31m" && Green_background_prefix="\033[42;37m" && Red_background_prefix="\033[41;37m" && Yellow_background_prefix="\033[43;37m" && Font_color_suffix="\033[0m" && Yellow_font_prefix="\e[1;33m" && Blue_font_prefix="\e[0;34m"
|
||||||
|
|
@ -99,21 +98,6 @@ check_lidar(){
|
||||||
echo -e "${Error} 没有找到雷达，请确认雷达已正确连接！！"
|
echo -e "${Error} 没有找到雷达，请确认雷达已正确连接！！"
|
||||||
fi
|
fi
|
||||||
}
|
}
|
||||||
|
|
||||||
check_camera_no_print(){
|
|
||||||
#检查使用哪种设备
|
|
||||||
if [ -n "$(lsusb -d 2bc5:0403)" ]; then
|
|
||||||
CAMERATYPE="astrapro"
|
|
||||||
fi
|
|
||||||
if [ -n "$(lsusb -d 2bc5:0401)" ]; then
|
|
||||||
CAMERATYPE="astra"
|
|
||||||
fi
|
|
||||||
if [ -n "$(lsusb -d 8086:0b07)" ]; then
|
|
||||||
CAMERATYPE="d435"
|
|
||||||
fi
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
#检查摄像头设备
|
#检查摄像头设备
|
||||||
check_camera(){
|
check_camera(){
|
||||||
|
|
||||||
|
|
@ -315,18 +299,7 @@ install_spark(){
|
||||||
#catkin_make install
|
#catkin_make install
|
||||||
}
|
}
|
||||||
|
|
||||||
#安装sagittarius机械臂的提示信息
|
|
||||||
install_sgr_msg(){
|
|
||||||
echo -e "${Info}没有找到sagittarius_moveit, sagittarius_arm_ros未安装?"
|
|
||||||
echo -e "${Info}"
|
|
||||||
echo -e "${Info}执行下列指令安装sagittarius_arm_ros, 假设Spark的工作空间路径为~/spark_noetic"
|
|
||||||
echo -e "${Info}"
|
|
||||||
echo -e "${Info}cd ~/spark_noetic/src/spark_driver/arm"
|
|
||||||
echo -e "${Info}git clone https://github.com/NXROBO/sagittarius_ws.git"
|
|
||||||
echo -e "${Info}rm ./sagittarius_ws/src/CMakeLists.txt"
|
|
||||||
echo -e "${Info}cd ~/spark_noetic && catkin_make"
|
|
||||||
echo -e "${Info}"
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
#远程设置
|
#远程设置
|
||||||
|
|
@ -464,11 +437,14 @@ voice_nav(){
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#机械臂与摄像头匹对标定uArm
|
#机械臂与摄像头匹对标定
|
||||||
cal_camera_arm_uarm(){
|
cal_camera_arm(){
|
||||||
|
echo -e "${Info}"
|
||||||
|
echo -e "${Info}机械臂与摄像头匹对标定"
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
ROSVER=`/usr/bin/rosversion -d`
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
PROJECTPATH=$(cd `dirname $0`; pwd)
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
source ${PROJECTPATH}/devel/setup.bash
|
||||||
|
|
||||||
echo -e "${Info}"
|
echo -e "${Info}"
|
||||||
echo -e "${Info}请确定："
|
echo -e "${Info}请确定："
|
||||||
echo -e "${Info} A.摄像头已反向向下安装好。机械臂正常上电。"
|
echo -e "${Info} A.摄像头已反向向下安装好。机械臂正常上电。"
|
||||||
|
|
@ -480,88 +456,21 @@ cal_camera_arm_uarm(){
|
||||||
if [ $ROSVER = "kinetic" ]; then
|
if [ $ROSVER = "kinetic" ]; then
|
||||||
echo -e "${Info}It is kinetic."
|
echo -e "${Info}It is kinetic."
|
||||||
print_command "roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
print_command "roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
||||||
roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
||||||
elif [ $ROSVER = "indigo" ]; then
|
elif [ $ROSVER = "indigo" ]; then
|
||||||
echo -e "${Info}It is indigo."
|
echo -e "${Info}It is indigo."
|
||||||
print_command "roslaunch spark_carry_object spark_carry_cal_cv2.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
print_command "roslaunch spark_carry_object spark_carry_cal_cv2.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
||||||
roslaunch spark_carry_object spark_carry_cal_cv2.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
roslaunch spark_carry_object spark_carry_cal_cv2.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
||||||
elif [ $ROSVER = "melodic" ]; then
|
elif [ $ROSVER = "melodic" ]; then
|
||||||
echo -e "${Info}It is melodic."
|
echo -e "${Info}It is melodic."
|
||||||
print_command "roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
print_command "roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
||||||
roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
||||||
elif [ $ROSVER = "noetic" ]; then
|
elif [ $ROSVER = "noetic" ]; then
|
||||||
echo -e "${Info}It is melodic."
|
echo -e "${Info}It is melodic."
|
||||||
print_command "roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
print_command "roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
||||||
roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
||||||
fi
|
fi
|
||||||
}
|
|
||||||
|
|
||||||
#机械臂与摄像头匹对标定 sagittarius
|
|
||||||
cal_camera_arm_sgr(){
|
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
|
||||||
|
|
||||||
if [ -n "$(roscd sagittarius_moveit)" ]; then
|
|
||||||
install_sgr_msg
|
|
||||||
else
|
|
||||||
echo -e "${Info} "
|
|
||||||
echo -e "${Info} 请确定："
|
|
||||||
echo -e "${Info} A.摄像头已反向向下安装好。"
|
|
||||||
echo -e "${Info} B.机械臂正常上电。"
|
|
||||||
echo -e "${Info} 退出请输入：Ctrl + c "
|
|
||||||
echo -e "${Info} "
|
|
||||||
echo -e "${Info} 请选择标定模式："
|
|
||||||
echo -e "${Info} 1.自动模式(简易版)"
|
|
||||||
echo -e "${Info} 2.自动模式"
|
|
||||||
echo -e "${Info} 3.手动模式"
|
|
||||||
echo && stty erase ^? && read -p "请输入数字 [1-3]：" calinum
|
|
||||||
case "$calinum" in
|
|
||||||
1)
|
|
||||||
print_command "roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE}"
|
|
||||||
roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE}
|
|
||||||
;;
|
|
||||||
2)
|
|
||||||
print_command "roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE} fast_mode:=false"
|
|
||||||
roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE} fast_mode:=false
|
|
||||||
;;
|
|
||||||
3)print_command "roslaunch spark_sagittarius_carry cal_cv3.launch camera_type_tel:=${CAMERATYPE}"
|
|
||||||
roslaunch spark_sagittarius_carry cal_cv3.launch camera_type_tel:=${CAMERATYPE}
|
|
||||||
;;
|
|
||||||
*)
|
|
||||||
echo -e "${Error} 错误，默认使用自动模式"
|
|
||||||
print_command "roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE}"
|
|
||||||
roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE}
|
|
||||||
;;
|
|
||||||
esac
|
|
||||||
fi
|
|
||||||
}
|
|
||||||
|
|
||||||
#机械臂与摄像头匹对标定
|
|
||||||
cal_camera_arm(){
|
|
||||||
echo -e "${Info} "
|
|
||||||
echo -e "${Info} 机械臂与摄像头匹对标定"
|
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
|
||||||
|
|
||||||
echo -e "${Info} "
|
|
||||||
echo -e "${Info} 请选择机械臂："
|
|
||||||
echo -e "${Info} 1.Sagittarius"
|
|
||||||
echo -e "${Info} 2.uArm"
|
|
||||||
echo && stty erase ^? && read -p "请输入数字 [1-2]：" armnum
|
|
||||||
|
|
||||||
case "$armnum" in
|
|
||||||
1)
|
|
||||||
cal_camera_arm_sgr
|
|
||||||
;;
|
|
||||||
2)
|
|
||||||
cal_camera_arm_uarm
|
|
||||||
;;
|
|
||||||
*)
|
|
||||||
echo -e "${Error} 错误，请选择正确的机械臂"
|
|
||||||
;;
|
|
||||||
esac
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -724,9 +633,10 @@ spark_intel_movidius(){
|
||||||
exit
|
exit
|
||||||
fi
|
fi
|
||||||
}
|
}
|
||||||
|
#让SPARK通过机械臂进行视觉抓取
|
||||||
#让SPARK通过机械臂进行视觉抓取 uarm
|
spark_carry_obj(){
|
||||||
spark_carry_obj_uarm(){
|
echo -e "${Info}"
|
||||||
|
echo -e "${Info}让SPARK通过机械臂进行视觉抓取"
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
ROSVER=`/usr/bin/rosversion -d`
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
PROJECTPATH=$(cd `dirname $0`; pwd)
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
source ${PROJECTPATH}/devel/setup.bash
|
||||||
|
|
@ -772,66 +682,7 @@ spark_carry_obj_uarm(){
|
||||||
print_command "roslaunch spark_carry_object spark_carry_object_only_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
print_command "roslaunch spark_carry_object spark_carry_object_only_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
||||||
roslaunch spark_carry_object spark_carry_object_only_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
roslaunch spark_carry_object spark_carry_object_only_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
||||||
fi
|
fi
|
||||||
}
|
|
||||||
|
|
||||||
#让SPARK通过机械臂进行视觉抓取 uarm
|
|
||||||
spark_carry_obj_sgr(){
|
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
|
||||||
echo -e "${Info}请选择移动的方式：
|
|
||||||
${Green_font_prefix}1.${Font_color_suffix} 固定位置移动
|
|
||||||
${Green_font_prefix}2.${Font_color_suffix} 手动地图指定位置导航(未完成。。)
|
|
||||||
${Green_font_prefix}3.${Font_color_suffix} 退出请输入：Ctrl + c"
|
|
||||||
echo && stty erase ^? && read -p "请输入数字 [1-2]：" armnum
|
|
||||||
case "$armnum" in
|
|
||||||
1)
|
|
||||||
MOVETYPE="fix"
|
|
||||||
;;
|
|
||||||
2)
|
|
||||||
MOVETYPE="slam"
|
|
||||||
;;
|
|
||||||
*)
|
|
||||||
echo -e "${Error} 错误，默认使用固定位置移动"
|
|
||||||
MOVETYPE="fix"
|
|
||||||
;;
|
|
||||||
esac
|
|
||||||
echo -e "${Info}"
|
|
||||||
echo -e "${Info}请确定："
|
|
||||||
echo -e "${Info} A.机械臂正常上电。"
|
|
||||||
echo -e "${Info} B.准备好可与标定方块同样颜色的物品。"
|
|
||||||
echo -e "${Info}退出请输入：Ctrl + c "
|
|
||||||
echo -e "${Info}"
|
|
||||||
echo && stty erase ^? && read -p "按回车键（Enter）开始："
|
|
||||||
print_command "roslaunch spark_sagittarius_carry carry_object_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}"
|
|
||||||
roslaunch spark_sagittarius_carry carry_object_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}
|
|
||||||
}
|
|
||||||
|
|
||||||
#让SPARK通过机械臂进行视觉抓取
|
|
||||||
spark_carry_obj(){
|
|
||||||
echo -e "${Info}"
|
|
||||||
echo -e "${Info}让SPARK通过机械臂进行视觉抓取"
|
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
|
||||||
|
|
||||||
echo -e "${Info} "
|
|
||||||
echo -e "${Info} 请选择机械臂："
|
|
||||||
echo -e "${Info} 1.Sagittarius"
|
|
||||||
echo -e "${Info} 2.uArm"
|
|
||||||
echo && stty erase ^? && read -p "请输入数字 [1-2]：" armnum
|
|
||||||
|
|
||||||
case "$armnum" in
|
|
||||||
1)
|
|
||||||
spark_carry_obj_sgr
|
|
||||||
;;
|
|
||||||
2)
|
|
||||||
spark_carry_obj_uarm
|
|
||||||
;;
|
|
||||||
*)
|
|
||||||
echo -e "${Error} 错误，请选择正确的机械臂"
|
|
||||||
;;
|
|
||||||
esac
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#让SPARK使用激光雷达绘制地图(gmapping)
|
#让SPARK使用激光雷达绘制地图(gmapping)
|
||||||
|
|
@ -901,49 +752,14 @@ spark_dock(){
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
#hsv范围值查找
|
#让SPARK夺宝奇兵比赛用示例程序
|
||||||
spark_hsv_detection(){
|
|
||||||
echo -e "${Info}"
|
|
||||||
echo -e "${Info}hsv值自动查找"
|
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
|
||||||
|
|
||||||
echo -e "${Info}"
|
|
||||||
echo -e "${Info}请确定："
|
|
||||||
echo -e "${Info} A.摄像头已反向向下安装好。机械臂正常上电。"
|
|
||||||
echo -e "${Info} B.${Red_font_prefix}红色${Font_color_suffix}标定物已贴好在吸盘固定头正上方。"
|
|
||||||
echo -e "${Info} C.机械臂正常上电。"
|
|
||||||
echo -e "${Info}退出请输入：Ctrl + c "
|
|
||||||
echo -e "${Info}"
|
|
||||||
|
|
||||||
echo -e "${Info}请选择比赛方式：
|
|
||||||
${Green_font_prefix}1.${Font_color_suffix} 获取吸盘上方hsv值
|
|
||||||
${Green_font_prefix}2.${Font_color_suffix} 获取颜色块hsv值,需要将颜色块放在图像的矩形框中
|
|
||||||
${Green_font_prefix}3.${Font_color_suffix} 退出请输入：Ctrl + c"
|
|
||||||
echo && stty erase ^? && read -p "请输入数字 [1-2]：" armnum
|
|
||||||
case "$armnum" in
|
|
||||||
1)
|
|
||||||
roslaunch spark_carry_object hsv_detection.launch camera_type_tel:=${CAMERATYPE} color:=${calibration}
|
|
||||||
;;
|
|
||||||
2)
|
|
||||||
roslaunch spark_carry_object hsv_detection.launch camera_type_tel:=${CAMERATYPE} color:=${color_block}
|
|
||||||
;;
|
|
||||||
*)
|
|
||||||
echo -e "${Error} 错误，请填入正确的数字"
|
|
||||||
;;
|
|
||||||
esac
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
#方块抓取示例程序
|
|
||||||
spark_carry_game(){
|
spark_carry_game(){
|
||||||
echo -e "${Info}"
|
echo -e "${Info}"
|
||||||
echo -e "${Info}夺宝奇兵比赛用示例程序"
|
echo -e "${Info}夺宝奇兵比赛用示例程序"
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
ROSVER=`/usr/bin/rosversion -d`
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
PROJECTPATH=$(cd `dirname $0`; pwd)
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
source ${PROJECTPATH}/devel/setup.bash
|
||||||
echo -e "${Info}请选择方式：
|
echo -e "${Info}请选择比赛方式：
|
||||||
${Green_font_prefix}1.${Font_color_suffix} 手动模式
|
${Green_font_prefix}1.${Font_color_suffix} 手动模式
|
||||||
${Green_font_prefix}2.${Font_color_suffix} 自动模式
|
${Green_font_prefix}2.${Font_color_suffix} 自动模式
|
||||||
${Green_font_prefix}3.${Font_color_suffix} 退出请输入：Ctrl + c"
|
${Green_font_prefix}3.${Font_color_suffix} 退出请输入：Ctrl + c"
|
||||||
|
|
@ -961,54 +777,6 @@ spark_carry_game(){
|
||||||
esac
|
esac
|
||||||
}
|
}
|
||||||
|
|
||||||
#进阶赛示例程序
|
|
||||||
spark_sgr_game(){
|
|
||||||
|
|
||||||
ROSVER=`/usr/bin/rosversion -d`
|
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
|
||||||
source ${PROJECTPATH}/devel/setup.bash
|
|
||||||
if [ -n "$(roscd spark_match)" ]; then
|
|
||||||
echo -e "${Info}没有找到进阶赛的示例程序"
|
|
||||||
elif [ -n "$(roscd sagittarius_moveit)" ]; then
|
|
||||||
install_sgr_msg
|
|
||||||
else
|
|
||||||
echo -e "${Info}"
|
|
||||||
echo -e "${Info}进阶赛比赛示例程序"
|
|
||||||
echo -e "${Info}请选择方式：
|
|
||||||
${Green_font_prefix}1.${Font_color_suffix} 建图
|
|
||||||
${Green_font_prefix}2.${Font_color_suffix} 控制Spark记录地点
|
|
||||||
${Green_font_prefix}3.${Font_color_suffix} USB摄像头标定(只需要标定一次)
|
|
||||||
${Green_font_prefix}4.${Font_color_suffix} 运行拾取示例程序
|
|
||||||
${Green_font_prefix}5.${Font_color_suffix} 运行进阶赛示例程序
|
|
||||||
${Green_font_prefix}6.${Font_color_suffix} 退出请输入：Ctrl + c"
|
|
||||||
echo && stty erase ^? && read -p "请输入数字 [1-5]：" armnum
|
|
||||||
case "$armnum" in
|
|
||||||
1)
|
|
||||||
roslaunch spark_match spark_match_map.launch
|
|
||||||
;;
|
|
||||||
2)
|
|
||||||
gnome-terminal -x bash -c "rosrun spark_match semantic_map.sh"
|
|
||||||
roslaunch spark_match spark_match_nav.launch enable_arm_tel:="no"
|
|
||||||
;;
|
|
||||||
3)
|
|
||||||
gnome-terminal -x bash -c "rosrun spark_match usb_cam_cali.sh"
|
|
||||||
roslaunch spark_match usb_cam.launch
|
|
||||||
;;
|
|
||||||
4)
|
|
||||||
gnome-terminal -x bash -c "rosrun spark_match pickup_demo.sh"
|
|
||||||
roslaunch spark_match spark_match_nav.launch
|
|
||||||
;;
|
|
||||||
5)
|
|
||||||
gnome-terminal -x bash -c "rosrun spark_match run_demo.sh"
|
|
||||||
roslaunch spark_match spark_match_nav.launch
|
|
||||||
;;
|
|
||||||
*)
|
|
||||||
echo -e "${Error} 错误，请填入正确的数字"
|
|
||||||
;;
|
|
||||||
esac
|
|
||||||
fi
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
spark_test_mode(){
|
spark_test_mode(){
|
||||||
PROJECTPATH=$(cd `dirname $0`; pwd)
|
PROJECTPATH=$(cd `dirname $0`; pwd)
|
||||||
|
|
@ -1224,31 +992,6 @@ coming_soon(){
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
check_camera_game(){
|
|
||||||
if [[ "${GAME_ENABLE}" == "yes" ]]; then
|
|
||||||
check_camera_no_print
|
|
||||||
if [[ "${CAMERATYPE}" == "d435" ]]; then
|
|
||||||
aa='a'
|
|
||||||
else
|
|
||||||
echo -e "
|
|
||||||
${Green_font_prefix} 13.${Font_color_suffix} hsv值自动查找"
|
|
||||||
fi
|
|
||||||
echo -e "
|
|
||||||
${Green_font_prefix} 20.${Font_color_suffix} 竞赛示例程序"
|
|
||||||
fi
|
|
||||||
|
|
||||||
if [[ "${ADV_GAME_ENABLE}" == "yes" ]]; then
|
|
||||||
echo -e "
|
|
||||||
${Green_font_prefix} 21.${Font_color_suffix} 进阶赛示例程序"
|
|
||||||
fi
|
|
||||||
|
|
||||||
if [[ "${GAME_ENABLE}" == "yes" ]] || [[ "${ADV_GAME_ENABLE}" == "yes" ]]; then
|
|
||||||
echo -e "
|
|
||||||
————————————"
|
|
||||||
fi
|
|
||||||
}
|
|
||||||
|
|
||||||
#printf
|
#printf
|
||||||
menu_status(){
|
menu_status(){
|
||||||
echo -e "${Tip} 当前系统版本 ${OSDescription} !"
|
echo -e "${Tip} 当前系统版本 ${OSDescription} !"
|
||||||
|
|
@ -1300,9 +1043,10 @@ echo -e "
|
||||||
${Green_font_prefix} 11.${Font_color_suffix} 语音控制SPARK移动
|
${Green_font_prefix} 11.${Font_color_suffix} 语音控制SPARK移动
|
||||||
${Green_font_prefix} 12.${Font_color_suffix} 给摄像头做标定
|
${Green_font_prefix} 12.${Font_color_suffix} 给摄像头做标定
|
||||||
|
|
||||||
————————————"
|
${Green_font_prefix} 20.${Font_color_suffix} 比赛程序
|
||||||
check_camera_game
|
|
||||||
echo -e "
|
————————————
|
||||||
|
|
||||||
${Green_font_prefix}100.${Font_color_suffix} 问题反馈
|
${Green_font_prefix}100.${Font_color_suffix} 问题反馈
|
||||||
${Green_font_prefix}104.${Font_color_suffix} 文件传输
|
${Green_font_prefix}104.${Font_color_suffix} 文件传输
|
||||||
"
|
"
|
||||||
|
|
@ -1349,15 +1093,9 @@ case "$num" in
|
||||||
12)
|
12)
|
||||||
calibrate_camera
|
calibrate_camera
|
||||||
;;
|
;;
|
||||||
13)
|
|
||||||
spark_hsv_detection
|
|
||||||
;;
|
|
||||||
20)
|
20)
|
||||||
spark_carry_game
|
spark_carry_game
|
||||||
;;
|
;;
|
||||||
21)
|
|
||||||
spark_sgr_game
|
|
||||||
;;
|
|
||||||
100)
|
100)
|
||||||
tell_us
|
tell_us
|
||||||
;;
|
;;
|
||||||
|
|
|
||||||
|
|
@ -51,7 +51,7 @@
|
||||||
</node>
|
</node>
|
||||||
</group>
|
</group>
|
||||||
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
||||||
<node pkg="move2grasp" type="grasp_pro_new.py" name="grasp" />
|
<node pkg="move2grasp" type="grasp_pro.py" name="grasp" />
|
||||||
</group>
|
</group>
|
||||||
|
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -15,7 +15,6 @@
|
||||||
|
|
||||||
<arg name="camera_type_tel" value="$(arg camera_type_tel)"/>
|
<arg name="camera_type_tel" value="$(arg camera_type_tel)"/>
|
||||||
<arg name="lidar_type_tel" value="$(arg lidar_type_tel)"/>
|
<arg name="lidar_type_tel" value="$(arg lidar_type_tel)"/>
|
||||||
<arg name="start_camera" value="true"/>
|
|
||||||
</include>
|
</include>
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -54,7 +53,7 @@
|
||||||
</node>
|
</node>
|
||||||
</group>
|
</group>
|
||||||
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
<group if="$(eval arg('camera_type_tel')=='astrapro')">
|
||||||
<node pkg="move2grasp" type="grasp_pro_new.py" name="grasp" output="screen"/>
|
<node pkg="move2grasp" type="grasp_pro.py" name="grasp" />
|
||||||
</group>
|
</group>
|
||||||
|
|
||||||
</launch>
|
</launch>
|
||||||
|
|
|
||||||
|
|
@ -6,8 +6,8 @@ Panels:
|
||||||
Expanded:
|
Expanded:
|
||||||
- /TF1/Frames1
|
- /TF1/Frames1
|
||||||
- /PointStamped1
|
- /PointStamped1
|
||||||
Splitter Ratio: 0.594406008720398
|
Splitter Ratio: 0.594406009
|
||||||
Tree Height: 210
|
Tree Height: 112
|
||||||
- Class: rviz/Selection
|
- Class: rviz/Selection
|
||||||
Name: Selection
|
Name: Selection
|
||||||
- Class: rviz/Tool Properties
|
- Class: rviz/Tool Properties
|
||||||
|
|
@ -16,7 +16,7 @@ Panels:
|
||||||
- /2D Nav Goal1
|
- /2D Nav Goal1
|
||||||
- /Publish Point1
|
- /Publish Point1
|
||||||
Name: Tool Properties
|
Name: Tool Properties
|
||||||
Splitter Ratio: 0.5886790156364441
|
Splitter Ratio: 0.588679016
|
||||||
- Class: rviz/Views
|
- Class: rviz/Views
|
||||||
Expanded:
|
Expanded:
|
||||||
- /Current View1
|
- /Current View1
|
||||||
|
|
@ -27,8 +27,6 @@ Panels:
|
||||||
Name: Time
|
Name: Time
|
||||||
SyncMode: 0
|
SyncMode: 0
|
||||||
SyncSource: LaserScan
|
SyncSource: LaserScan
|
||||||
Preferences:
|
|
||||||
PromptSaveOnExit: true
|
|
||||||
Toolbars:
|
Toolbars:
|
||||||
toolButtonStyle: 2
|
toolButtonStyle: 2
|
||||||
Visualization Manager:
|
Visualization Manager:
|
||||||
|
|
@ -40,7 +38,7 @@ Visualization Manager:
|
||||||
Color: 160; 160; 164
|
Color: 160; 160; 164
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Line Style:
|
Line Style:
|
||||||
Line Width: 0.029999999329447746
|
Line Width: 0.0299999993
|
||||||
Value: Lines
|
Value: Lines
|
||||||
Name: Grid
|
Name: Grid
|
||||||
Normal Cell Count: 0
|
Normal Cell Count: 0
|
||||||
|
|
@ -123,8 +121,6 @@ Visualization Manager:
|
||||||
Value: false
|
Value: false
|
||||||
ir_bumper_right_link:
|
ir_bumper_right_link:
|
||||||
Value: false
|
Value: false
|
||||||
laser_frame:
|
|
||||||
Value: true
|
|
||||||
left_wheel_link:
|
left_wheel_link:
|
||||||
Value: false
|
Value: false
|
||||||
lidar_link:
|
lidar_link:
|
||||||
|
|
@ -139,7 +135,6 @@ Visualization Manager:
|
||||||
Value: false
|
Value: false
|
||||||
spark_stack:
|
spark_stack:
|
||||||
Value: true
|
Value: true
|
||||||
Marker Alpha: 1
|
|
||||||
Marker Scale: 1
|
Marker Scale: 1
|
||||||
Name: TF
|
Name: TF
|
||||||
Show Arrows: true
|
Show Arrows: true
|
||||||
|
|
@ -208,8 +203,7 @@ Visualization Manager:
|
||||||
left_wheel_link:
|
left_wheel_link:
|
||||||
{}
|
{}
|
||||||
lidar_link:
|
lidar_link:
|
||||||
laser_frame:
|
{}
|
||||||
{}
|
|
||||||
right_wheel_link:
|
right_wheel_link:
|
||||||
{}
|
{}
|
||||||
room_base_link:
|
room_base_link:
|
||||||
|
|
@ -233,13 +227,15 @@ Visualization Manager:
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Invert Rainbow: false
|
Invert Rainbow: false
|
||||||
Max Color: 255; 255; 255
|
Max Color: 255; 255; 255
|
||||||
|
Max Intensity: 11799
|
||||||
Min Color: 0; 0; 0
|
Min Color: 0; 0; 0
|
||||||
|
Min Intensity: 0
|
||||||
Name: LaserScan
|
Name: LaserScan
|
||||||
Position Transformer: XYZ
|
Position Transformer: XYZ
|
||||||
Queue Size: 10
|
Queue Size: 10
|
||||||
Selectable: true
|
Selectable: true
|
||||||
Size (Pixels): 3
|
Size (Pixels): 3
|
||||||
Size (m): 0.029999999329447746
|
Size (m): 0.0299999993
|
||||||
Style: Flat Squares
|
Style: Flat Squares
|
||||||
Topic: /scan
|
Topic: /scan
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
|
|
@ -258,7 +254,7 @@ Visualization Manager:
|
||||||
Transport Hint: compressed
|
Transport Hint: compressed
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
Value: true
|
Value: true
|
||||||
- Alpha: 0.699999988079071
|
- Alpha: 0.699999988
|
||||||
Class: rviz/Map
|
Class: rviz/Map
|
||||||
Color Scheme: map
|
Color Scheme: map
|
||||||
Draw Behind: false
|
Draw Behind: false
|
||||||
|
|
@ -391,10 +387,6 @@ Visualization Manager:
|
||||||
Alpha: 1
|
Alpha: 1
|
||||||
Show Axes: false
|
Show Axes: false
|
||||||
Show Trail: false
|
Show Trail: false
|
||||||
laser_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
left_wheel_link:
|
left_wheel_link:
|
||||||
Alpha: 1
|
Alpha: 1
|
||||||
Show Axes: false
|
Show Axes: false
|
||||||
|
|
@ -404,7 +396,6 @@ Visualization Manager:
|
||||||
Alpha: 1
|
Alpha: 1
|
||||||
Show Axes: false
|
Show Axes: false
|
||||||
Show Trail: false
|
Show Trail: false
|
||||||
Value: true
|
|
||||||
right_wheel_link:
|
right_wheel_link:
|
||||||
Alpha: 1
|
Alpha: 1
|
||||||
Show Axes: false
|
Show Axes: false
|
||||||
|
|
@ -429,8 +420,8 @@ Visualization Manager:
|
||||||
- Alpha: 1
|
- Alpha: 1
|
||||||
Autocompute Intensity Bounds: true
|
Autocompute Intensity Bounds: true
|
||||||
Autocompute Value Bounds:
|
Autocompute Value Bounds:
|
||||||
Max Value: 1.3373792171478271
|
Max Value: 1.33737922
|
||||||
Min Value: 0.17965421080589294
|
Min Value: 0.179654211
|
||||||
Value: true
|
Value: true
|
||||||
Axis: Z
|
Axis: Z
|
||||||
Channel Name: intensity
|
Channel Name: intensity
|
||||||
|
|
@ -441,13 +432,15 @@ Visualization Manager:
|
||||||
Enabled: false
|
Enabled: false
|
||||||
Invert Rainbow: false
|
Invert Rainbow: false
|
||||||
Max Color: 255; 255; 255
|
Max Color: 255; 255; 255
|
||||||
|
Max Intensity: 4096
|
||||||
Min Color: 0; 0; 0
|
Min Color: 0; 0; 0
|
||||||
|
Min Intensity: 0
|
||||||
Name: PointCloud2
|
Name: PointCloud2
|
||||||
Position Transformer: XYZ
|
Position Transformer: XYZ
|
||||||
Queue Size: 10
|
Queue Size: 10
|
||||||
Selectable: true
|
Selectable: true
|
||||||
Size (Pixels): 3
|
Size (Pixels): 3
|
||||||
Size (m): 0.009999999776482582
|
Size (m): 0.00999999978
|
||||||
Style: Flat Squares
|
Style: Flat Squares
|
||||||
Topic: /camera/depth/points
|
Topic: /camera/depth/points
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
|
|
@ -460,8 +453,7 @@ Visualization Manager:
|
||||||
Enabled: true
|
Enabled: true
|
||||||
History Length: 1
|
History Length: 1
|
||||||
Name: PointStamped
|
Name: PointStamped
|
||||||
Queue Size: 10
|
Radius: 0.200000003
|
||||||
Radius: 0.20000000298023224
|
|
||||||
Topic: /clicked_point
|
Topic: /clicked_point
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
Value: true
|
Value: true
|
||||||
|
|
@ -480,10 +472,7 @@ Visualization Manager:
|
||||||
- Class: rviz/FocusCamera
|
- Class: rviz/FocusCamera
|
||||||
- Class: rviz/Measure
|
- Class: rviz/Measure
|
||||||
- Class: rviz/SetInitialPose
|
- Class: rviz/SetInitialPose
|
||||||
Theta std deviation: 0.2617993950843811
|
|
||||||
Topic: /initialpose
|
Topic: /initialpose
|
||||||
X std deviation: 0.5
|
|
||||||
Y std deviation: 0.5
|
|
||||||
- Class: rviz/SetGoal
|
- Class: rviz/SetGoal
|
||||||
Topic: /move_base_simple/goal
|
Topic: /move_base_simple/goal
|
||||||
- Class: rviz/PublishPoint
|
- Class: rviz/PublishPoint
|
||||||
|
|
@ -493,35 +482,35 @@ Visualization Manager:
|
||||||
Views:
|
Views:
|
||||||
Current:
|
Current:
|
||||||
Class: rviz/ThirdPersonFollower
|
Class: rviz/ThirdPersonFollower
|
||||||
Distance: 11.24269962310791
|
Distance: 11.2426996
|
||||||
Enable Stereo Rendering:
|
Enable Stereo Rendering:
|
||||||
Stereo Eye Separation: 0.05999999865889549
|
Stereo Eye Separation: 0.0599999987
|
||||||
Stereo Focal Distance: 1
|
Stereo Focal Distance: 1
|
||||||
Swap Stereo Eyes: false
|
Swap Stereo Eyes: false
|
||||||
Value: false
|
Value: false
|
||||||
Field of View: 0.7853981852531433
|
|
||||||
Focal Point:
|
Focal Point:
|
||||||
X: 9.536739753457368e-07
|
X: 9.53673975e-07
|
||||||
Y: 4.768369876728684e-07
|
Y: 4.76836988e-07
|
||||||
Z: -4.768369876728684e-07
|
Z: -4.76836988e-07
|
||||||
Focal Shape Fixed Size: true
|
Focal Shape Fixed Size: true
|
||||||
Focal Shape Size: 0.05000000074505806
|
Focal Shape Size: 0.0500000007
|
||||||
Invert Z Axis: false
|
Invert Z Axis: false
|
||||||
Name: Current View
|
Name: Current View
|
||||||
Near Clip Distance: 0.009999999776482582
|
Near Clip Distance: 0.00999999978
|
||||||
Pitch: 0.4803990423679352
|
Pitch: 0.480399042
|
||||||
Target Frame: map
|
Target Frame: map
|
||||||
Yaw: 1.8854000568389893
|
Value: ThirdPersonFollower (rviz)
|
||||||
|
Yaw: 1.88540006
|
||||||
Saved: ~
|
Saved: ~
|
||||||
Window Geometry:
|
Window Geometry:
|
||||||
Displays:
|
Displays:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Height: 536
|
Height: 576
|
||||||
Hide Left Dock: false
|
Hide Left Dock: false
|
||||||
Hide Right Dock: false
|
Hide Right Dock: false
|
||||||
Image:
|
Image:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000017afc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d0000017a000000e60100001cfa000000000100000002fb0000000a0049006d0061006700650100000000ffffffff0000005e00fffffffb000000100044006900730070006c00610079007301000000000000016a0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003b80000003efc0100000002fb0000000800540069006d00650100000000000003b8000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002480000017a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
QMainWindow State: 000000ff00000000fd00000004000000000000016a000001b6fc020000000afb0000001200530065006c0065006300740069006f006e00000000280000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000ff000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000012d000000b10000001600fffffffb0000000a0049006d00610067006501000001c4000000b30000000000000000000000010000010f00000242fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000242000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003bf0000003efc0100000002fb0000000800540069006d00650100000000000003bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000024f000001b600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
Selection:
|
Selection:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Time:
|
Time:
|
||||||
|
|
@ -530,6 +519,6 @@ Window Geometry:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Views:
|
Views:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Width: 952
|
Width: 959
|
||||||
X: 72
|
X: 65
|
||||||
Y: 27
|
Y: 24
|
||||||
|
|
|
||||||
|
|
@ -135,7 +135,7 @@ class GraspObject():
|
||||||
self.pub1.publish(pos)
|
self.pub1.publish(pos)
|
||||||
r2.sleep()
|
r2.sleep()
|
||||||
# go down -100
|
# go down -100
|
||||||
pos.z = -70
|
pos.z = -50
|
||||||
self.pub1.publish(pos)
|
self.pub1.publish(pos)
|
||||||
print("z = -83\n")
|
print("z = -83\n")
|
||||||
r2.sleep()
|
r2.sleep()
|
||||||
|
|
@ -163,10 +163,8 @@ class GraspObject():
|
||||||
cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
|
cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
|
||||||
|
|
||||||
# 蓝色物体颜色检测范围
|
# 蓝色物体颜色检测范围
|
||||||
LowerBlue = np.array([95, 90, 100])
|
LowerBlue = np.array([95, 90, 80])
|
||||||
UpperBlue = np.array([130, 210, 255])
|
UpperBlue = np.array([130, 255, 255])
|
||||||
# LowerBlue = np.array([4, 103, 162])
|
|
||||||
# UpperBlue = np.array([85, 136, 181])
|
|
||||||
mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
|
mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
|
||||||
cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
|
cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
|
||||||
|
|
||||||
|
|
@ -235,7 +233,7 @@ class GraspObject():
|
||||||
|
|
||||||
# stop pump
|
# stop pump
|
||||||
self.pub2.publish(0)
|
self.pub2.publish(0)
|
||||||
# r2.sleep()
|
r2.sleep()
|
||||||
#r1.sleep()
|
#r1.sleep()
|
||||||
pos.x = 120
|
pos.x = 120
|
||||||
pos.y = 0
|
pos.y = 0
|
||||||
|
|
|
||||||
|
|
@ -1,263 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
# -*- coding: utf-8 -*-
|
|
||||||
|
|
||||||
import os
|
|
||||||
import time
|
|
||||||
import random
|
|
||||||
import ctypes
|
|
||||||
import roslib
|
|
||||||
import rospy
|
|
||||||
import smach
|
|
||||||
import smach_ros
|
|
||||||
import threading
|
|
||||||
import string
|
|
||||||
import math
|
|
||||||
import cv2
|
|
||||||
import numpy as np
|
|
||||||
from geometry_msgs.msg import Twist
|
|
||||||
from std_msgs.msg import String
|
|
||||||
from geometry_msgs.msg import Pose, Point, Quaternion
|
|
||||||
from sensor_msgs.msg import Image
|
|
||||||
from cv_bridge import CvBridge, CvBridgeError
|
|
||||||
from spark_carry_object.msg import *
|
|
||||||
|
|
||||||
|
|
||||||
class GraspObject():
|
|
||||||
'''
|
|
||||||
监听主控，用于物品抓取功能
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
|
|
||||||
global xc, yc, xc_prev, yc_prev, found_count
|
|
||||||
xc = 0
|
|
||||||
yc = 0
|
|
||||||
xc_prev = xc
|
|
||||||
yc_prev = yc
|
|
||||||
found_count = 0
|
|
||||||
self.is_found_object = False
|
|
||||||
# self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
|
|
||||||
# 订阅机械臂抓取指令
|
|
||||||
self.sub2 = rospy.Subscriber(
|
|
||||||
'/grasp', String, self.grasp_cp, queue_size=1)
|
|
||||||
# 发布机械臂位姿
|
|
||||||
self.pub1 = rospy.Publisher(
|
|
||||||
'position_write_topic', position, queue_size=10)
|
|
||||||
# 发布机械臂吸盘
|
|
||||||
self.pub2 = rospy.Publisher('pump_topic', status, queue_size=1)
|
|
||||||
# 发布机械臂状态
|
|
||||||
self.grasp_status_pub = rospy.Publisher(
|
|
||||||
'grasp_status', String, queue_size=1)
|
|
||||||
# 发布TWist消息控制机器人底盘
|
|
||||||
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
|
|
||||||
pos = position()
|
|
||||||
pos.x = 120
|
|
||||||
pos.y = 0
|
|
||||||
pos.z = 35
|
|
||||||
self.pub1.publish(pos)
|
|
||||||
|
|
||||||
def grasp_cp(self, msg):
|
|
||||||
if msg.data == '1':
|
|
||||||
# 订阅摄像头话题,对图像信息进行处理
|
|
||||||
self.sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
|
|
||||||
self.is_found_object = False
|
|
||||||
rate = rospy.Rate(10)
|
|
||||||
times=0
|
|
||||||
steps=0
|
|
||||||
while not self.is_found_object:
|
|
||||||
rate.sleep()
|
|
||||||
times+=1
|
|
||||||
# 转一圈没有发现可抓取物体,退出抓取
|
|
||||||
if steps>=5:
|
|
||||||
self.sub.unregister()
|
|
||||||
print("stop grasp\n")
|
|
||||||
status=String()
|
|
||||||
status.data='-1'
|
|
||||||
self.grasp_status_pub.publish(status)
|
|
||||||
return
|
|
||||||
# 旋转一定角度扫描是否有可供抓取的物体
|
|
||||||
if times>=30:
|
|
||||||
times=0
|
|
||||||
steps+=1
|
|
||||||
self.turn_body()
|
|
||||||
print("not found\n")
|
|
||||||
print("unregisting sub\n")
|
|
||||||
self.sub.unregister()
|
|
||||||
print("unregisted sub\n")
|
|
||||||
# 抓取检测到的物体
|
|
||||||
self.grasp()
|
|
||||||
status=String()
|
|
||||||
status.data='1'
|
|
||||||
self.grasp_status_pub.publish(status)
|
|
||||||
if msg.data=='0':
|
|
||||||
# 放下物体
|
|
||||||
self.is_found_object = False
|
|
||||||
self.release_object()
|
|
||||||
status=String()
|
|
||||||
status.data='0'
|
|
||||||
self.grasp_status_pub.publish(status)
|
|
||||||
|
|
||||||
# 执行抓取
|
|
||||||
def grasp(self):
|
|
||||||
print("start to grasp\n")
|
|
||||||
global xc, yc, found_count
|
|
||||||
# stop function
|
|
||||||
|
|
||||||
filename = os.environ['HOME'] + "/thefile.txt"
|
|
||||||
file_pix = open(filename, 'r')
|
|
||||||
s = file_pix.read()
|
|
||||||
file_pix.close()
|
|
||||||
print(s)
|
|
||||||
arr=s.split()
|
|
||||||
a1=arr[0]
|
|
||||||
a2=arr[1]
|
|
||||||
a3=arr[2]
|
|
||||||
a4=arr[3]
|
|
||||||
a = [0]*2
|
|
||||||
b = [0]*2
|
|
||||||
a[0]=float(a1)
|
|
||||||
a[1]=float(a2)
|
|
||||||
b[0]=float(a3)
|
|
||||||
b[1]=float(a4)
|
|
||||||
print('k and b value:',a[0],a[1],b[0],b[1])
|
|
||||||
r1 = rospy.Rate(0.095)
|
|
||||||
r2 = rospy.Rate(10)
|
|
||||||
pos = position()
|
|
||||||
# 物体所在坐标+标定误差
|
|
||||||
pos.x = a[0] * yc + a[1]
|
|
||||||
pos.y = b[0] * xc + b[1]
|
|
||||||
pos.z = 20
|
|
||||||
# pos.z = 20
|
|
||||||
print("z = 20\n")
|
|
||||||
self.pub1.publish(pos)
|
|
||||||
r2.sleep()
|
|
||||||
# go down -100
|
|
||||||
pos.z = -50
|
|
||||||
self.pub1.publish(pos)
|
|
||||||
print("z = -83\n")
|
|
||||||
r2.sleep()
|
|
||||||
|
|
||||||
# 开始吸取物体
|
|
||||||
self.pub2.publish(1)
|
|
||||||
r2.sleep()
|
|
||||||
|
|
||||||
# 提起物体
|
|
||||||
pos.x = 250 #160
|
|
||||||
pos.y = 0
|
|
||||||
pos.z = 150 #55
|
|
||||||
self.pub1.publish(pos)
|
|
||||||
r1.sleep()
|
|
||||||
# 使用CV检测物体
|
|
||||||
def image_cb(self, data):
|
|
||||||
global xc, yc, xc_prev, yc_prev, found_count
|
|
||||||
# change to opencv
|
|
||||||
try:
|
|
||||||
cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
|
|
||||||
except CvBridgeError as e:
|
|
||||||
print('error')
|
|
||||||
|
|
||||||
# change rgb to hsv
|
|
||||||
cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
|
|
||||||
|
|
||||||
# 蓝色物体颜色检测范围
|
|
||||||
LowerBlue = np.array([95, 70, 60])
|
|
||||||
UpperBlue = np.array([140, 255, 255])
|
|
||||||
mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
|
|
||||||
cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
|
|
||||||
|
|
||||||
# gray process
|
|
||||||
cv_image4 = cv_image3[:, :, 0]
|
|
||||||
|
|
||||||
# smooth and clean noise
|
|
||||||
blurred = cv2.blur(cv_image4, (9, 9))
|
|
||||||
(_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
|
|
||||||
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
|
|
||||||
cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
|
|
||||||
cv_image5 = cv2.erode(cv_image5, None, iterations=4)
|
|
||||||
cv_image5 = cv2.dilate(cv_image5, None, iterations=4)
|
|
||||||
|
|
||||||
# detect contour
|
|
||||||
# cv2.imshow("win1", cv_image3)
|
|
||||||
# cv2.imshow("win2", cv_image5)
|
|
||||||
cv2.waitKey(1)
|
|
||||||
contours, hier = cv2.findContours(cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
|
|
||||||
|
|
||||||
# if find contours, pick the biggest box
|
|
||||||
if len(contours) > 0:
|
|
||||||
size = []
|
|
||||||
size_max = 0
|
|
||||||
for i, c in enumerate(contours):
|
|
||||||
rect = cv2.minAreaRect(c)
|
|
||||||
box = cv2.boxPoints(rect)
|
|
||||||
box = np.int0(box)
|
|
||||||
x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
|
|
||||||
y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
|
|
||||||
w = math.sqrt((box[0][0] - box[1][0]) ** 2 + (box[0][1] - box[1][1]) ** 2)
|
|
||||||
h = math.sqrt((box[0][0] - box[3][0]) ** 2 + (box[0][1] - box[3][1]) ** 2)
|
|
||||||
size.append(w * h)
|
|
||||||
if size[i] > size_max:
|
|
||||||
size_max = size[i]
|
|
||||||
index = i
|
|
||||||
xc = x_mid
|
|
||||||
yc = y_mid
|
|
||||||
# if box is not moving for 20 times
|
|
||||||
# print found_count
|
|
||||||
if found_count >= 30:
|
|
||||||
self.is_found_object = True
|
|
||||||
cmd_vel = Twist()
|
|
||||||
self.cmd_vel_pub.publish(cmd_vel)
|
|
||||||
else:
|
|
||||||
# if box is not moving
|
|
||||||
if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2:
|
|
||||||
found_count = found_count + 1
|
|
||||||
else:
|
|
||||||
found_count = 0
|
|
||||||
else:
|
|
||||||
found_count = 0
|
|
||||||
xc_prev = xc
|
|
||||||
yc_prev = yc
|
|
||||||
# 释放物体
|
|
||||||
def release_object(self):
|
|
||||||
r1 = rospy.Rate(0.15) # 5s
|
|
||||||
r2 = rospy.Rate(1) # 1s
|
|
||||||
pos = position()
|
|
||||||
# go forward
|
|
||||||
pos.x = 200
|
|
||||||
pos.y = 0
|
|
||||||
pos.z = -40 #-80
|
|
||||||
self.pub1.publish(pos)
|
|
||||||
r1.sleep()
|
|
||||||
|
|
||||||
# stop pump
|
|
||||||
self.pub2.publish(0)
|
|
||||||
r2.sleep()
|
|
||||||
#r1.sleep()
|
|
||||||
pos.x = 120
|
|
||||||
pos.y = 0
|
|
||||||
pos.z = 35
|
|
||||||
self.pub1.publish(pos)
|
|
||||||
r1.sleep()
|
|
||||||
return 'succeeded'
|
|
||||||
# 转动机器人到一定角度
|
|
||||||
def turn_body(self):
|
|
||||||
cmd_vel = Twist()
|
|
||||||
cmd_vel.angular.z = 0.25
|
|
||||||
rate = rospy.Rate(10)
|
|
||||||
for i in range(40):
|
|
||||||
self.cmd_vel_pub.publish(cmd_vel)
|
|
||||||
rate.sleep()
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
try:
|
|
||||||
rospy.init_node('GraspObject', anonymous=False)
|
|
||||||
rospy.loginfo("Init GraspObject main")
|
|
||||||
GraspObject()
|
|
||||||
rospy.spin()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
rospy.loginfo("End spark GraspObject main")
|
|
||||||
|
|
||||||
|
|
@ -33,11 +33,10 @@ def keyboardLoop():
|
||||||
|
|
||||||
#速度变量
|
#速度变量
|
||||||
# 慢速
|
# 慢速
|
||||||
slow_walk_vel_ = rospy.get_param('walk_vel',0.05)
|
walk_vel_ = rospy.get_param('walk_vel', 0.3)
|
||||||
walk_vel_ = rospy.get_param('walk_vel', 0.1)
|
|
||||||
# 快速
|
# 快速
|
||||||
run_vel_ = rospy.get_param('run_vel', 0.5)
|
run_vel_ = rospy.get_param('run_vel', 1.0)
|
||||||
yaw_rate_ = rospy.get_param('yaw_rate', 0.9)
|
yaw_rate_ = rospy.get_param('yaw_rate', 1.0)
|
||||||
yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)
|
yaw_rate_run_ = rospy.get_param('yaw_rate_run', 1.5)
|
||||||
# walk_vel_前后速度
|
# walk_vel_前后速度
|
||||||
max_tv = walk_vel_
|
max_tv = walk_vel_
|
||||||
|
|
@ -52,7 +51,6 @@ def keyboardLoop():
|
||||||
print ("使用[WASD]控制机器人")
|
print ("使用[WASD]控制机器人")
|
||||||
print ("按[G]抓取物体，按[H]放下物体")
|
print ("按[G]抓取物体，按[H]放下物体")
|
||||||
print ("按[Q]退出" )
|
print ("按[Q]退出" )
|
||||||
print ("B紧急停止")
|
|
||||||
|
|
||||||
#读取按键循环
|
#读取按键循环
|
||||||
while not rospy.is_shutdown():
|
while not rospy.is_shutdown():
|
||||||
|
|
@ -74,40 +72,28 @@ def keyboardLoop():
|
||||||
msg.data='1'
|
msg.data='1'
|
||||||
grasp_pub.publish(msg)
|
grasp_pub.publish(msg)
|
||||||
can_grasp=False
|
can_grasp=False
|
||||||
speed = 0
|
|
||||||
turn = 0
|
|
||||||
elif ch == 'h':
|
elif ch == 'h':
|
||||||
if can_release:
|
if can_release:
|
||||||
msg=String()
|
msg=String()
|
||||||
msg.data='0'
|
msg.data='0'
|
||||||
grasp_pub.publish(msg)
|
grasp_pub.publish(msg)
|
||||||
can_release=False
|
can_release=False
|
||||||
speed = 0
|
|
||||||
turn = 0
|
|
||||||
elif ch == 'z':
|
|
||||||
if can_release:
|
|
||||||
msg=String()
|
|
||||||
msg.data='2'
|
|
||||||
grasp_pub.publish(msg)
|
|
||||||
can_release=False
|
|
||||||
speed = 0
|
|
||||||
turn = 0
|
|
||||||
elif ch == 'w':
|
elif ch == 'w':
|
||||||
max_tv = walk_vel_
|
max_tv = walk_vel_
|
||||||
speed = 1
|
speed = 1
|
||||||
turn = 0
|
turn = 0
|
||||||
elif ch == 's':
|
elif ch == 's':
|
||||||
max_tv = walk_vel_
|
max_tv = walk_vel_
|
||||||
speed = -1.2
|
speed = -1
|
||||||
turn = 0
|
turn = 0
|
||||||
elif ch == 'a':
|
elif ch == 'a':
|
||||||
max_rv = yaw_rate_
|
max_rv = yaw_rate_
|
||||||
speed = 0
|
speed = 0
|
||||||
turn = 0.15
|
turn = 1
|
||||||
elif ch == 'd':
|
elif ch == 'd':
|
||||||
max_rv = yaw_rate_
|
max_rv = yaw_rate_
|
||||||
speed = 0
|
speed = 0
|
||||||
turn = -0.15
|
turn = -1
|
||||||
elif ch == 'W':
|
elif ch == 'W':
|
||||||
max_tv = run_vel_
|
max_tv = run_vel_
|
||||||
speed = 1
|
speed = 1
|
||||||
|
|
@ -124,26 +110,6 @@ def keyboardLoop():
|
||||||
max_rv = yaw_rate_run_
|
max_rv = yaw_rate_run_
|
||||||
speed = 0
|
speed = 0
|
||||||
turn = -1
|
turn = -1
|
||||||
elif ch == 'i':
|
|
||||||
max_tv = slow_walk_vel_
|
|
||||||
speed = 1
|
|
||||||
turn = 0
|
|
||||||
elif ch == 'k':
|
|
||||||
max_tv = slow_walk_vel_
|
|
||||||
speed = -1
|
|
||||||
turn = 0
|
|
||||||
elif ch == 'j':
|
|
||||||
max_rv = yaw_rate_
|
|
||||||
speed = 0
|
|
||||||
turn = 0.075
|
|
||||||
elif ch == 'l':
|
|
||||||
max_rv = yaw_rate_
|
|
||||||
speed = 0
|
|
||||||
turn = -0.075
|
|
||||||
elif ch == 'b' or ch == 'B':
|
|
||||||
max_rv = yaw_rate_
|
|
||||||
speed = 0
|
|
||||||
turn = 0
|
|
||||||
elif ch == 'q':
|
elif ch == 'q':
|
||||||
exit()
|
exit()
|
||||||
else:
|
else:
|
||||||
|
|
@ -158,7 +124,7 @@ def keyboardLoop():
|
||||||
pub.publish(cmd)
|
pub.publish(cmd)
|
||||||
rate.sleep()
|
rate.sleep()
|
||||||
#停止机器人
|
#停止机器人
|
||||||
#stop_robot()
|
stop_robot()
|
||||||
|
|
||||||
def stop_robot():
|
def stop_robot():
|
||||||
cmd.linear.x = 0.0
|
cmd.linear.x = 0.0
|
||||||
|
|
|
||||||
|
|
@ -45,7 +45,7 @@
|
||||||
<xacro:include filename="$(find spark_description)/urdf/sensors/spark_d435.urdf.xacro"/>
|
<xacro:include filename="$(find spark_description)/urdf/sensors/spark_d435.urdf.xacro"/>
|
||||||
</xacro:if>
|
</xacro:if>
|
||||||
<xacro:if value="${robot_arm_type == 'sagittarius_arm' and enable_arm == 'yes'}">
|
<xacro:if value="${robot_arm_type == 'sagittarius_arm' and enable_arm == 'yes'}">
|
||||||
<xacro:include filename="$(find spark_description)/urdf/spark_sagittarius_descriptions.urdf"/>
|
<xacro:include filename="$(find sagittarius_descriptions)/urdf/spark_sagittarius_descriptions.urdf"/>
|
||||||
</xacro:if>
|
</xacro:if>
|
||||||
<xacro:if value="${robot_arm_type == 'uarm' and enable_arm == 'yes'}">
|
<xacro:if value="${robot_arm_type == 'uarm' and enable_arm == 'yes'}">
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,599 @@
|
||||||
<?xml version="1.0"?>
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||||
<robot xmlns:xacro="http://ros.org/wiki/xacro">
|
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||||
|
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||||
</robot>
|
<robot
|
||||||
|
name="sagittarius_arm">
|
||||||
|
<!-- <joint name="sagittarius_base_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="sagittarius_base_frame" />
|
||||||
|
</joint>
|
||||||
|
<link name="sagittarius_base_frame"/>
|
||||||
|
-->
|
||||||
|
<joint name="sagittarius_base_link_joint" type="fixed">
|
||||||
|
<origin xyz="0.044 0 0.138" rpy="0 0 0"/>
|
||||||
|
<parent link="base_footprint"/>
|
||||||
|
<child link="sagittarius_base_link" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="sagittarius_base_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.00793179544277737 2.48115460577673E-19 0.0312047945759253"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.940146705073746" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.000950195835062344"
|
||||||
|
ixy="3.14825974312405E-20"
|
||||||
|
ixz="-0.000200082473207336"
|
||||||
|
iyy="0.00267650790326047"
|
||||||
|
iyz="-1.01412991222915E-19"
|
||||||
|
izz="0.002902663900239" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/sagittarius_base_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="0.1 0.1 0.1 0.9" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/sagittarius_base_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link
|
||||||
|
name="link1">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-7.74579608786574E-10 3.23354786979222E-12 0.0344870497564283"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.123745079127031" />
|
||||||
|
<inertia
|
||||||
|
ixx="8.15779591743416E-05"
|
||||||
|
ixy="-4.25325094802094E-15"
|
||||||
|
ixz="-2.22201794150093E-12"
|
||||||
|
iyy="6.7607001964135E-05"
|
||||||
|
iyz="-1.59127137655435E-14"
|
||||||
|
izz="4.41297902395382E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link1.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="0.1 0.1 0.1 0.9" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link1.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint1"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0.045 0 0.07"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="sagittarius_base_link" />
|
||||||
|
<child
|
||||||
|
link="link1" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 -1" />
|
||||||
|
<limit
|
||||||
|
lower="-2"
|
||||||
|
upper="2"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link2">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.0119015312584793 -1.03639845764692E-08 0.150755903867182"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.156160822791157" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.000429266751802994"
|
||||||
|
ixy="-2.27426165787001E-11"
|
||||||
|
ixz="-5.58862834004722E-05"
|
||||||
|
iyy="0.000444676097748043"
|
||||||
|
iyz="-2.41993274420629E-10"
|
||||||
|
izz="7.916162292894E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link2.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="0.1 0.1 0.1 0.9" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link2.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint2"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0 -0.0001 0.055"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link1" />
|
||||||
|
<child
|
||||||
|
link="link2" />
|
||||||
|
<axis
|
||||||
|
xyz="0 -1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-1.57"
|
||||||
|
upper="1.4"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link3">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.054357321065004 -2.67256405253992E-08 0.00064594073830132"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0366487313200473" />
|
||||||
|
<inertia
|
||||||
|
ixx="9.28701176876329E-06"
|
||||||
|
ixy="-2.23878504272996E-11"
|
||||||
|
ixz="6.34310578103811E-07"
|
||||||
|
iyy="2.0727596617309E-05"
|
||||||
|
iyz="-1.71647322080315E-11"
|
||||||
|
izz="2.42416271089979E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link3.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="0.1 0.1 0.1 0.9" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link3.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint3"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0.033 -0.0001 0.179"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link2" />
|
||||||
|
<child
|
||||||
|
link="link3" />
|
||||||
|
<axis
|
||||||
|
xyz="0 -1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-1.48"
|
||||||
|
upper="1.8"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link4">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.0444029485674779 -0.00527865310158246 -1.43738021485262E-10"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.124950088011432" />
|
||||||
|
<inertia
|
||||||
|
ixx="3.52699895078887E-05"
|
||||||
|
ixy="-1.14870103744551E-05"
|
||||||
|
ixz="2.45948329713632E-14"
|
||||||
|
iyy="0.00010928557067047"
|
||||||
|
iyz="-3.42525315676607E-12"
|
||||||
|
izz="0.000121451732690361" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link4.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="0.1 0.1 0.1 0.9" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link4.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint4"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0.0845 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link3" />
|
||||||
|
<child
|
||||||
|
link="link4" />
|
||||||
|
<axis
|
||||||
|
xyz="-1 0 0" />
|
||||||
|
<limit
|
||||||
|
lower="-2.90"
|
||||||
|
upper="2.90"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link5">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.0391172635125616 1.22102848665318E-15 0.011359922796391"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0564710099602834" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.77667699918498E-05"
|
||||||
|
ixy="3.77572698012408E-19"
|
||||||
|
ixz="-6.79618041712231E-07"
|
||||||
|
iyy="1.89643063051294E-05"
|
||||||
|
iyz="1.78381670331667E-19"
|
||||||
|
izz="1.66130972969903E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link5.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="0.1 0.1 0.1 0.9" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link5.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint5"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0.08435 0.00065 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link4" />
|
||||||
|
<child
|
||||||
|
link="link5" />
|
||||||
|
<axis
|
||||||
|
xyz="0 -1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-1.8"
|
||||||
|
upper="1.60"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link6">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.0330748503556987 -1.47583477394259E-10 0.00874151659437283"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0790875358325134" />
|
||||||
|
<inertia
|
||||||
|
ixx="4.00727396068743E-05"
|
||||||
|
ixy="1.5026227935429E-13"
|
||||||
|
ixz="6.77337873337555E-06"
|
||||||
|
iyy="4.37063090227749E-05"
|
||||||
|
iyz="-1.73263828329941E-13"
|
||||||
|
izz="5.69755186998711E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link6.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="0.1 0.1 0.1 0.9" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link6.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint6"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="0.061387 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link5" />
|
||||||
|
<child
|
||||||
|
link="link6" />
|
||||||
|
<axis
|
||||||
|
xyz="-1 0 0" />
|
||||||
|
<limit
|
||||||
|
lower="-3.1"
|
||||||
|
upper="3.1"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="usb_cam_link">
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_usb_cam_frame"
|
||||||
|
type="fixed">
|
||||||
|
<origin
|
||||||
|
xyz="0.05 0 0.05825"
|
||||||
|
rpy="-1.57 0 -1.57" />
|
||||||
|
<parent
|
||||||
|
link="link6" />
|
||||||
|
<child
|
||||||
|
link="usb_cam_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 0" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="ar_tag_link">
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_ar_tag_frame"
|
||||||
|
type="fixed">
|
||||||
|
<origin
|
||||||
|
xyz="0.025 0 0.036"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link6" />
|
||||||
|
<child
|
||||||
|
link="ar_tag_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 0" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link_grasping_frame">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-0.29292863789122 -0.000855248417215765 -0.182862893125548"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="1.55422651132493" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.0246807389934657"
|
||||||
|
ixy="6.12977506319177E-05"
|
||||||
|
ixz="-0.0174568851774044"
|
||||||
|
iyy="0.0450160124726729"
|
||||||
|
iyz="0.000114187269419605"
|
||||||
|
izz="0.0218809326193524" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_grasping_frame"
|
||||||
|
type="fixed">
|
||||||
|
<origin
|
||||||
|
xyz="0.0557 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link6" />
|
||||||
|
<child
|
||||||
|
link="link_grasping_frame" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 0" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link_gripper_left">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.0168448613850675 0.00562743947549268 -3.34157867051821E-05"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0185082696416839" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.65620668352765E-06"
|
||||||
|
ixy="3.12039082733024E-07"
|
||||||
|
ixz="-1.06123823321138E-09"
|
||||||
|
iyy="7.56731403094351E-06"
|
||||||
|
iyz="5.54920898904154E-11"
|
||||||
|
izz="6.37610666327876E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link_gripper_left.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="0.1 0.1 0.1 0.9" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link_gripper_left.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_gripper_left"
|
||||||
|
type="prismatic">
|
||||||
|
<origin
|
||||||
|
xyz="0.0695 0.033 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link6" />
|
||||||
|
<child
|
||||||
|
link="link_gripper_left" />
|
||||||
|
<axis
|
||||||
|
xyz="0 1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-0.034"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="link_gripper_right">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.0168448612901284 -0.00562743946967042 3.34158424380449E-05"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0185082695670405" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.65620670013429E-06"
|
||||||
|
ixy="-3.12039085367371E-07"
|
||||||
|
ixz="1.06121397059487E-09"
|
||||||
|
iyy="7.56731399585561E-06"
|
||||||
|
iyz="5.54960746810554E-11"
|
||||||
|
izz="6.37610660879392E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link_gripper_right.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="0.1 0.1 0.1 0.9" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://sagittarius_descriptions/meshes/link_gripper_right.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="joint_gripper_right"
|
||||||
|
type="prismatic">
|
||||||
|
<origin
|
||||||
|
xyz="0.0695 -0.033 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="link6" />
|
||||||
|
<child
|
||||||
|
link="link_gripper_right" />
|
||||||
|
<axis
|
||||||
|
xyz="0 -1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-0.034"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
</robot>
|
||||||
|
|
|
||||||
|
|
@ -1,9 +0,0 @@
|
||||||
<!--
|
|
||||||
机械臂与摄像头之间的校准功能。注意摄像头需要反着装，垂直向下。使用一个蓝色圆贴在吸盘的正上方当校准点。
|
|
||||||
-->
|
|
||||||
<launch>
|
|
||||||
|
|
||||||
<!--UARM机械臂-->
|
|
||||||
<include file="$(find swiftpro)/launch/pro_control_nomoveit.launch"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,153 +0,0 @@
|
||||||
#!/usr/bin/env python
|
|
||||||
# -*- coding: utf-8 -*-
|
|
||||||
import cv2
|
|
||||||
import numpy as np
|
|
||||||
import math
|
|
||||||
import os
|
|
||||||
import sys
|
|
||||||
import time
|
|
||||||
from PIL import Image, ImageDraw,ImageFont
|
|
||||||
import rospy
|
|
||||||
from spark_carry_object.msg import *
|
|
||||||
|
|
||||||
def mean_hsv(img,HSV_value):
|
|
||||||
HSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
|
||||||
HSV_value[0]+=np.mean(HSV[:, :, 0])
|
|
||||||
HSV_value[1]+=np.mean(HSV[:, :, 1])
|
|
||||||
HSV_value[2]+=np.mean(HSV[:, :, 2])
|
|
||||||
return HSV_value
|
|
||||||
|
|
||||||
|
|
||||||
def hsv_range(HSV_value):
|
|
||||||
# 设置HSV颜色值的范围
|
|
||||||
H_range = 12
|
|
||||||
S_range = 120
|
|
||||||
V_range = 120
|
|
||||||
|
|
||||||
lower_H = int(HSV_value[0] - H_range)
|
|
||||||
upper_H = int(HSV_value[0] + H_range)
|
|
||||||
|
|
||||||
lower_S = int(HSV_value[1] - S_range)
|
|
||||||
upper_S = int(HSV_value[1] + S_range)
|
|
||||||
|
|
||||||
lower_V = int(HSV_value[2] - V_range)
|
|
||||||
upper_V = int(HSV_value[2] + V_range)
|
|
||||||
|
|
||||||
if lower_H<0:
|
|
||||||
lower_H=0
|
|
||||||
if upper_H>180:
|
|
||||||
upper_H=180
|
|
||||||
|
|
||||||
if lower_S<50:
|
|
||||||
lower_S=50
|
|
||||||
if upper_S>255:
|
|
||||||
upper_S=255
|
|
||||||
|
|
||||||
if lower_V<50:
|
|
||||||
lower_V=50
|
|
||||||
if upper_V>255:
|
|
||||||
upper_V=255
|
|
||||||
|
|
||||||
lower_HSV = np.array([lower_H, lower_S, lower_V])
|
|
||||||
upper_HSV = np.array([upper_H, upper_S, upper_V])
|
|
||||||
return lower_HSV, upper_HSV
|
|
||||||
|
|
||||||
|
|
||||||
def test(lower_HSV, upper_HSV, image):
|
|
||||||
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
|
|
||||||
mask = cv2.inRange(hsv, lower_HSV, upper_HSV)
|
|
||||||
mask = cv2.erode(mask, None, iterations=2)
|
|
||||||
mask = cv2.dilate(mask, None, iterations=2)
|
|
||||||
mask = cv2.GaussianBlur(mask, (3, 3), 0)
|
|
||||||
cv2.putText(mask, 'Done! Press q to exit!', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
|
||||||
(255, 255, 255), 2, cv2.LINE_AA)
|
|
||||||
cv2.imshow("HSV_img", mask)
|
|
||||||
|
|
||||||
def arm_init():
|
|
||||||
pub1 = rospy.Publisher('position_write_topic', position, queue_size=1)
|
|
||||||
|
|
||||||
r1 = rospy.Rate(1)
|
|
||||||
r1.sleep()
|
|
||||||
pos = position()
|
|
||||||
pos.x = 120
|
|
||||||
pos.y = 0
|
|
||||||
pos.z = 35
|
|
||||||
pub1.publish(pos)
|
|
||||||
r1.sleep()
|
|
||||||
|
|
||||||
def save_hsv(name, lower_HSV, upper_HSV):
|
|
||||||
|
|
||||||
content = str(lower_HSV[0]) + ',' +str(lower_HSV[1])+ ',' + str(lower_HSV[2]) \
|
|
||||||
+ ' ' + str(upper_HSV[0])+ ',' + str(upper_HSV[1])+ ',' + str(upper_HSV[2]) + '\n'
|
|
||||||
# 将HSV值写入文件，文件在spark目录下
|
|
||||||
if name == 'color_block':
|
|
||||||
content = "block_HSV is :" + content
|
|
||||||
filename = os.environ['HOME'] + "/color_block_HSV.txt"
|
|
||||||
elif name == 'calibration':
|
|
||||||
content = "calibration_HSV is :" + content
|
|
||||||
filename = os.environ['HOME'] + "/calibration_HSV.txt"
|
|
||||||
|
|
||||||
with open(filename, "w") as f:
|
|
||||||
f.write(content)
|
|
||||||
|
|
||||||
print("HSV value has saved in" + filename)
|
|
||||||
print(content)
|
|
||||||
|
|
||||||
def main():
|
|
||||||
name = rospy.get_param("/spark_hsv_detection/color")
|
|
||||||
arm_init()
|
|
||||||
time.sleep(3)
|
|
||||||
HSV_value = [0,0,0]
|
|
||||||
count = 0
|
|
||||||
capture = cv2.VideoCapture(0)
|
|
||||||
# 颜色块中心点上下左右扩大60个像素
|
|
||||||
box_w = 60
|
|
||||||
# 吸盘上方颜色像素范围
|
|
||||||
cali_w = 20
|
|
||||||
cali_h = 30
|
|
||||||
# 收集300次取平均值
|
|
||||||
collect_times = 300
|
|
||||||
while True:
|
|
||||||
ret, img = capture.read()
|
|
||||||
if img is not None:
|
|
||||||
# 200次以内先做提醒，将颜色块放在矩形框中
|
|
||||||
if count < 200:
|
|
||||||
cv2.putText(img, 'please put the color being', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.putText(img, 'tested in rectangle box!', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
# 如果在200-500次以内开始收集hsv值，并求出平均值
|
|
||||||
elif count > 200 and count < 200+collect_times:
|
|
||||||
cv2.putText(img, 'HSV_value is collecting!', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
if name == 'color_block':
|
|
||||||
frame = img[120:120 + box_w, 300:300 + box_w]
|
|
||||||
elif name == 'calibration':
|
|
||||||
frame = img[310:310 + cali_h, 355:355 + cali_w]
|
|
||||||
HSV_value = mean_hsv(frame, HSV_value)
|
|
||||||
# 500次以后开始检测，查看是否有提取到矩形框中颜色的HSV值
|
|
||||||
elif count==200+collect_times:
|
|
||||||
for i in range(len(HSV_value)):
|
|
||||||
HSV_value[i] = HSV_value[i] / collect_times
|
|
||||||
lower_HSV, upper_HSV = hsv_range(HSV_value)
|
|
||||||
save_hsv(name, lower_HSV, upper_HSV)
|
|
||||||
|
|
||||||
elif count > 200+collect_times:
|
|
||||||
test(lower_HSV, upper_HSV, img)
|
|
||||||
|
|
||||||
count += 1
|
|
||||||
if name == 'color_block':
|
|
||||||
cv2.rectangle(img, (300, 120), (300+box_w, 120+box_w), (0, 255, 0), 3)
|
|
||||||
elif name == 'calibration':
|
|
||||||
cv2.rectangle(img, (355, 310), (355 + cali_w, 310 + cali_h), (0, 255, 0), 3)
|
|
||||||
cv2.imshow("RGB_img", img)
|
|
||||||
cv2.waitKey(1)
|
|
||||||
if cv2.waitKey(10) == ord('q'):
|
|
||||||
break
|
|
||||||
cv2.destroyAllWindows()
|
|
||||||
return lower_HSV, upper_HSV
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
rospy.init_node('hsv_detection', anonymous=True)
|
|
||||||
main()
|
|
||||||
rospy.spin()
|
|
||||||
|
|
@ -245,7 +245,7 @@ class ReleaseObject(State):
|
||||||
# go forward
|
# go forward
|
||||||
pos.x = 200
|
pos.x = 200
|
||||||
pos.y = 0
|
pos.y = 0
|
||||||
pos.z = -38 #-80
|
pos.z = -40 #-80
|
||||||
pub1.publish(pos)
|
pub1.publish(pos)
|
||||||
r1.sleep()
|
r1.sleep()
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -17,4 +17,4 @@ decel_factor: 1
|
||||||
# 0 - none (default)
|
# 0 - none (default)
|
||||||
# 1 - odometry
|
# 1 - odometry
|
||||||
# 2 - end robot commands
|
# 2 - end robot commands
|
||||||
robot_feedback: 0
|
robot_feedback: 1
|
||||||
|
|
|
||||||
|
|
@ -11,8 +11,8 @@ Panels:
|
||||||
- /Local Map1/Costmap1
|
- /Local Map1/Costmap1
|
||||||
- /Local Map1/Planner1
|
- /Local Map1/Planner1
|
||||||
- /PointCloud21
|
- /PointCloud21
|
||||||
Splitter Ratio: 0.6860465407371521
|
Splitter Ratio: 0.686046541
|
||||||
Tree Height: 188
|
Tree Height: 229
|
||||||
- Class: rviz/Selection
|
- Class: rviz/Selection
|
||||||
Name: Selection
|
Name: Selection
|
||||||
- Class: rviz/Tool Properties
|
- Class: rviz/Tool Properties
|
||||||
|
|
@ -20,7 +20,7 @@ Panels:
|
||||||
- /2D Pose Estimate1
|
- /2D Pose Estimate1
|
||||||
- /2D Nav Goal1
|
- /2D Nav Goal1
|
||||||
Name: Tool Properties
|
Name: Tool Properties
|
||||||
Splitter Ratio: 0.5886790156364441
|
Splitter Ratio: 0.588679016
|
||||||
- Class: rviz/Views
|
- Class: rviz/Views
|
||||||
Expanded:
|
Expanded:
|
||||||
- /Current View1
|
- /Current View1
|
||||||
|
|
@ -31,8 +31,6 @@ Panels:
|
||||||
Name: Time
|
Name: Time
|
||||||
SyncMode: 0
|
SyncMode: 0
|
||||||
SyncSource: Rtabmap cloud
|
SyncSource: Rtabmap cloud
|
||||||
Preferences:
|
|
||||||
PromptSaveOnExit: true
|
|
||||||
Toolbars:
|
Toolbars:
|
||||||
toolButtonStyle: 2
|
toolButtonStyle: 2
|
||||||
Visualization Manager:
|
Visualization Manager:
|
||||||
|
|
@ -44,7 +42,7 @@ Visualization Manager:
|
||||||
Color: 160; 160; 164
|
Color: 160; 160; 164
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Line Style:
|
Line Style:
|
||||||
Line Width: 0.029999999329447746
|
Line Width: 0.0299999993
|
||||||
Value: Lines
|
Value: Lines
|
||||||
Name: Grid
|
Name: Grid
|
||||||
Normal Cell Count: 0
|
Normal Cell Count: 0
|
||||||
|
|
@ -214,7 +212,6 @@ Visualization Manager:
|
||||||
Frame Timeout: 15
|
Frame Timeout: 15
|
||||||
Frames:
|
Frames:
|
||||||
All Enabled: false
|
All Enabled: false
|
||||||
Marker Alpha: 1
|
|
||||||
Marker Scale: 1
|
Marker Scale: 1
|
||||||
Name: TF
|
Name: TF
|
||||||
Show Arrows: true
|
Show Arrows: true
|
||||||
|
|
@ -239,13 +236,15 @@ Visualization Manager:
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Invert Rainbow: false
|
Invert Rainbow: false
|
||||||
Max Color: 255; 255; 255
|
Max Color: 255; 255; 255
|
||||||
|
Max Intensity: 4096
|
||||||
Min Color: 0; 0; 0
|
Min Color: 0; 0; 0
|
||||||
|
Min Intensity: 0
|
||||||
Name: LaserScan
|
Name: LaserScan
|
||||||
Position Transformer: XYZ
|
Position Transformer: XYZ
|
||||||
Queue Size: 10
|
Queue Size: 10
|
||||||
Selectable: true
|
Selectable: true
|
||||||
Size (Pixels): 3
|
Size (Pixels): 3
|
||||||
Size (m): 0.009999999776482582
|
Size (m): 0.00999999978
|
||||||
Style: Squares
|
Style: Squares
|
||||||
Topic: /scan
|
Topic: /scan
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
|
|
@ -267,20 +266,22 @@ Visualization Manager:
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Invert Rainbow: false
|
Invert Rainbow: false
|
||||||
Max Color: 255; 255; 255
|
Max Color: 255; 255; 255
|
||||||
|
Max Intensity: 4096
|
||||||
Min Color: 0; 0; 0
|
Min Color: 0; 0; 0
|
||||||
|
Min Intensity: 0
|
||||||
Name: Bumper Hit
|
Name: Bumper Hit
|
||||||
Position Transformer: XYZ
|
Position Transformer: XYZ
|
||||||
Queue Size: 10
|
Queue Size: 10
|
||||||
Selectable: true
|
Selectable: true
|
||||||
Size (Pixels): 3
|
Size (Pixels): 3
|
||||||
Size (m): 0.07999999821186066
|
Size (m): 0.0799999982
|
||||||
Style: Spheres
|
Style: Spheres
|
||||||
Topic: /mobile_base/sensors/bumper_pointcloud
|
Topic: /mobile_base/sensors/bumper_pointcloud
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
Use Fixed Frame: true
|
Use Fixed Frame: true
|
||||||
Use rainbow: true
|
Use rainbow: true
|
||||||
Value: true
|
Value: true
|
||||||
- Alpha: 0.699999988079071
|
- Alpha: 0.699999988
|
||||||
Class: rviz/Map
|
Class: rviz/Map
|
||||||
Color Scheme: map
|
Color Scheme: map
|
||||||
Draw Behind: false
|
Draw Behind: false
|
||||||
|
|
@ -292,7 +293,7 @@ Visualization Manager:
|
||||||
Value: true
|
Value: true
|
||||||
- Class: rviz/Group
|
- Class: rviz/Group
|
||||||
Displays:
|
Displays:
|
||||||
- Alpha: 0.699999988079071
|
- Alpha: 0.699999988
|
||||||
Class: rviz/Map
|
Class: rviz/Map
|
||||||
Color Scheme: costmap
|
Color Scheme: costmap
|
||||||
Draw Behind: true
|
Draw Behind: true
|
||||||
|
|
@ -307,11 +308,11 @@ Visualization Manager:
|
||||||
Class: rviz/Path
|
Class: rviz/Path
|
||||||
Color: 255; 0; 0
|
Color: 255; 0; 0
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Head Diameter: 0.30000001192092896
|
Head Diameter: 0.300000012
|
||||||
Head Length: 0.20000000298023224
|
Head Length: 0.200000003
|
||||||
Length: 0.30000001192092896
|
Length: 0.300000012
|
||||||
Line Style: Lines
|
Line Style: Lines
|
||||||
Line Width: 0.029999999329447746
|
Line Width: 0.0299999993
|
||||||
Name: Planner
|
Name: Planner
|
||||||
Offset:
|
Offset:
|
||||||
X: 0
|
X: 0
|
||||||
|
|
@ -319,10 +320,9 @@ Visualization Manager:
|
||||||
Z: 0
|
Z: 0
|
||||||
Pose Color: 255; 85; 255
|
Pose Color: 255; 85; 255
|
||||||
Pose Style: None
|
Pose Style: None
|
||||||
Queue Size: 10
|
Radius: 0.0299999993
|
||||||
Radius: 0.029999999329447746
|
Shaft Diameter: 0.100000001
|
||||||
Shaft Diameter: 0.10000000149011612
|
Shaft Length: 0.100000001
|
||||||
Shaft Length: 0.10000000149011612
|
|
||||||
Topic: /move_base/TrajectoryPlannerROS/global_plan
|
Topic: /move_base/TrajectoryPlannerROS/global_plan
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
Value: true
|
Value: true
|
||||||
|
|
@ -330,7 +330,7 @@ Visualization Manager:
|
||||||
Name: Global Map
|
Name: Global Map
|
||||||
- Class: rviz/Group
|
- Class: rviz/Group
|
||||||
Displays:
|
Displays:
|
||||||
- Alpha: 0.699999988079071
|
- Alpha: 0.699999988
|
||||||
Class: rviz/Map
|
Class: rviz/Map
|
||||||
Color Scheme: costmap
|
Color Scheme: costmap
|
||||||
Draw Behind: false
|
Draw Behind: false
|
||||||
|
|
@ -345,11 +345,11 @@ Visualization Manager:
|
||||||
Class: rviz/Path
|
Class: rviz/Path
|
||||||
Color: 0; 12; 255
|
Color: 0; 12; 255
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Head Diameter: 0.30000001192092896
|
Head Diameter: 0.300000012
|
||||||
Head Length: 0.20000000298023224
|
Head Length: 0.200000003
|
||||||
Length: 0.30000001192092896
|
Length: 0.300000012
|
||||||
Line Style: Lines
|
Line Style: Lines
|
||||||
Line Width: 0.029999999329447746
|
Line Width: 0.0299999993
|
||||||
Name: Planner
|
Name: Planner
|
||||||
Offset:
|
Offset:
|
||||||
X: 0
|
X: 0
|
||||||
|
|
@ -357,28 +357,26 @@ Visualization Manager:
|
||||||
Z: 0
|
Z: 0
|
||||||
Pose Color: 255; 85; 255
|
Pose Color: 255; 85; 255
|
||||||
Pose Style: None
|
Pose Style: None
|
||||||
Queue Size: 10
|
Radius: 0.0299999993
|
||||||
Radius: 0.029999999329447746
|
Shaft Diameter: 0.100000001
|
||||||
Shaft Diameter: 0.10000000149011612
|
Shaft Length: 0.100000001
|
||||||
Shaft Length: 0.10000000149011612
|
|
||||||
Topic: /move_base/TrajectoryPlannerROS/local_plan
|
Topic: /move_base/TrajectoryPlannerROS/local_plan
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
Value: true
|
Value: true
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Name: Local Map
|
Name: Local Map
|
||||||
- Alpha: 1
|
- Alpha: 1
|
||||||
Arrow Length: 0.20000000298023224
|
Arrow Length: 0.200000003
|
||||||
Axes Length: 0.30000001192092896
|
Axes Length: 0.300000012
|
||||||
Axes Radius: 0.009999999776482582
|
Axes Radius: 0.00999999978
|
||||||
Class: rviz/PoseArray
|
Class: rviz/PoseArray
|
||||||
Color: 0; 192; 0
|
Color: 0; 192; 0
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Head Length: 0.07000000029802322
|
Head Length: 0.0700000003
|
||||||
Head Radius: 0.029999999329447746
|
Head Radius: 0.0299999993
|
||||||
Name: Amcl Particles
|
Name: Amcl Particles
|
||||||
Queue Size: 10
|
Shaft Length: 0.230000004
|
||||||
Shaft Length: 0.23000000417232513
|
Shaft Radius: 0.00999999978
|
||||||
Shaft Radius: 0.009999999776482582
|
|
||||||
Shape: Arrow (Flat)
|
Shape: Arrow (Flat)
|
||||||
Topic: /particlecloud
|
Topic: /particlecloud
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
|
|
@ -396,25 +394,25 @@ Visualization Manager:
|
||||||
Cloud from scan: false
|
Cloud from scan: false
|
||||||
Cloud max depth (m): 4
|
Cloud max depth (m): 4
|
||||||
Cloud min depth (m): 0
|
Cloud min depth (m): 0
|
||||||
Cloud voxel size (m): 0.009999999776482582
|
Cloud voxel size (m): 0.00999999978
|
||||||
Color: 255; 255; 255
|
Color: 255; 255; 255
|
||||||
Color Transformer: RGB8
|
Color Transformer: RGB8
|
||||||
Download graph: false
|
Download graph: false
|
||||||
Download map: false
|
Download map: false
|
||||||
Download namespace: rtabmap
|
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Filter ceiling (m): 0
|
Filter ceiling (m): 0
|
||||||
Filter floor (m): 0
|
Filter floor (m): 0
|
||||||
Invert Rainbow: false
|
Invert Rainbow: false
|
||||||
Max Color: 255; 255; 255
|
Max Color: 255; 255; 255
|
||||||
|
Max Intensity: 4096
|
||||||
Min Color: 0; 0; 0
|
Min Color: 0; 0; 0
|
||||||
|
Min Intensity: 0
|
||||||
Name: Rtabmap cloud
|
Name: Rtabmap cloud
|
||||||
Node filtering angle (degrees): 0
|
Node filtering angle (degrees): 0
|
||||||
Node filtering radius (m): 0
|
Node filtering radius (m): 0
|
||||||
Position Transformer: XYZ
|
Position Transformer: XYZ
|
||||||
Queue Size: 10
|
|
||||||
Size (Pixels): 3
|
Size (Pixels): 3
|
||||||
Size (m): 0.009999999776482582
|
Size (m): 0.00999999978
|
||||||
Style: Flat Squares
|
Style: Flat Squares
|
||||||
Topic: /rtabmap/mapData
|
Topic: /rtabmap/mapData
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
|
|
@ -438,11 +436,11 @@ Visualization Manager:
|
||||||
Class: rviz/Path
|
Class: rviz/Path
|
||||||
Color: 25; 255; 0
|
Color: 25; 255; 0
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Head Diameter: 0.30000001192092896
|
Head Diameter: 0.300000012
|
||||||
Head Length: 0.20000000298023224
|
Head Length: 0.200000003
|
||||||
Length: 0.30000001192092896
|
Length: 0.300000012
|
||||||
Line Style: Lines
|
Line Style: Lines
|
||||||
Line Width: 0.029999999329447746
|
Line Width: 0.0299999993
|
||||||
Name: Path
|
Name: Path
|
||||||
Offset:
|
Offset:
|
||||||
X: 0
|
X: 0
|
||||||
|
|
@ -450,10 +448,9 @@ Visualization Manager:
|
||||||
Z: 0
|
Z: 0
|
||||||
Pose Color: 255; 85; 255
|
Pose Color: 255; 85; 255
|
||||||
Pose Style: None
|
Pose Style: None
|
||||||
Queue Size: 10
|
Radius: 0.0299999993
|
||||||
Radius: 0.029999999329447746
|
Shaft Diameter: 0.100000001
|
||||||
Shaft Diameter: 0.10000000149011612
|
Shaft Length: 0.100000001
|
||||||
Shaft Length: 0.10000000149011612
|
|
||||||
Topic: /move_base/DWAPlannerROS/global_plan
|
Topic: /move_base/DWAPlannerROS/global_plan
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
Value: true
|
Value: true
|
||||||
|
|
@ -462,11 +459,11 @@ Visualization Manager:
|
||||||
Class: rviz/Path
|
Class: rviz/Path
|
||||||
Color: 255; 0; 0
|
Color: 255; 0; 0
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Head Diameter: 0.30000001192092896
|
Head Diameter: 0.300000012
|
||||||
Head Length: 0.20000000298023224
|
Head Length: 0.200000003
|
||||||
Length: 0.30000001192092896
|
Length: 0.300000012
|
||||||
Line Style: Lines
|
Line Style: Lines
|
||||||
Line Width: 0.029999999329447746
|
Line Width: 0.0299999993
|
||||||
Name: Path
|
Name: Path
|
||||||
Offset:
|
Offset:
|
||||||
X: 0
|
X: 0
|
||||||
|
|
@ -474,10 +471,9 @@ Visualization Manager:
|
||||||
Z: 0
|
Z: 0
|
||||||
Pose Color: 255; 85; 255
|
Pose Color: 255; 85; 255
|
||||||
Pose Style: None
|
Pose Style: None
|
||||||
Queue Size: 10
|
Radius: 0.0299999993
|
||||||
Radius: 0.029999999329447746
|
Shaft Diameter: 0.100000001
|
||||||
Shaft Diameter: 0.10000000149011612
|
Shaft Length: 0.100000001
|
||||||
Shaft Length: 0.10000000149011612
|
|
||||||
Topic: /move_base/DWAPlannerROS/local_plan
|
Topic: /move_base/DWAPlannerROS/local_plan
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
Value: true
|
Value: true
|
||||||
|
|
@ -496,13 +492,15 @@ Visualization Manager:
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Invert Rainbow: false
|
Invert Rainbow: false
|
||||||
Max Color: 255; 255; 255
|
Max Color: 255; 255; 255
|
||||||
|
Max Intensity: 4096
|
||||||
Min Color: 0; 0; 0
|
Min Color: 0; 0; 0
|
||||||
|
Min Intensity: 0
|
||||||
Name: PointCloud2
|
Name: PointCloud2
|
||||||
Position Transformer: XYZ
|
Position Transformer: XYZ
|
||||||
Queue Size: 10
|
Queue Size: 10
|
||||||
Selectable: true
|
Selectable: true
|
||||||
Size (Pixels): 3
|
Size (Pixels): 3
|
||||||
Size (m): 0.009999999776482582
|
Size (m): 0.00999999978
|
||||||
Style: Flat Squares
|
Style: Flat Squares
|
||||||
Topic: /camera/depth_registered/points
|
Topic: /camera/depth_registered/points
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
|
|
@ -522,10 +520,7 @@ Visualization Manager:
|
||||||
Hide Inactive Objects: true
|
Hide Inactive Objects: true
|
||||||
- Class: rviz/Select
|
- Class: rviz/Select
|
||||||
- Class: rviz/SetInitialPose
|
- Class: rviz/SetInitialPose
|
||||||
Theta std deviation: 0.2617993950843811
|
|
||||||
Topic: /initialpose
|
Topic: /initialpose
|
||||||
X std deviation: 0.5
|
|
||||||
Y std deviation: 0.5
|
|
||||||
- Class: rviz/SetGoal
|
- Class: rviz/SetGoal
|
||||||
Topic: /move_base_simple/goal
|
Topic: /move_base_simple/goal
|
||||||
- Class: rviz/Measure
|
- Class: rviz/Measure
|
||||||
|
|
@ -533,35 +528,35 @@ Visualization Manager:
|
||||||
Views:
|
Views:
|
||||||
Current:
|
Current:
|
||||||
Class: rviz/Orbit
|
Class: rviz/Orbit
|
||||||
Distance: 5.954586982727051
|
Distance: 5.95458698
|
||||||
Enable Stereo Rendering:
|
Enable Stereo Rendering:
|
||||||
Stereo Eye Separation: 0.05999999865889549
|
Stereo Eye Separation: 0.0599999987
|
||||||
Stereo Focal Distance: 1
|
Stereo Focal Distance: 1
|
||||||
Swap Stereo Eyes: false
|
Swap Stereo Eyes: false
|
||||||
Value: false
|
Value: false
|
||||||
Field of View: 0.7853981852531433
|
|
||||||
Focal Point:
|
Focal Point:
|
||||||
X: 0.00901245977729559
|
X: 0.00901245978
|
||||||
Y: -0.1267929971218109
|
Y: -0.126792997
|
||||||
Z: 0.022313600406050682
|
Z: 0.0223136004
|
||||||
Focal Shape Fixed Size: true
|
Focal Shape Fixed Size: true
|
||||||
Focal Shape Size: 0.05000000074505806
|
Focal Shape Size: 0.0500000007
|
||||||
Invert Z Axis: false
|
Invert Z Axis: false
|
||||||
Name: Current View
|
Name: Current View
|
||||||
Near Clip Distance: 0.009999999776482582
|
Near Clip Distance: 0.00999999978
|
||||||
Pitch: 0.7447974681854248
|
Pitch: 0.51479733
|
||||||
Target Frame: base_footprint
|
Target Frame: base_footprint
|
||||||
Yaw: 3.7791357040405273
|
Value: Orbit (rviz)
|
||||||
|
Yaw: 4.26413631
|
||||||
Saved: ~
|
Saved: ~
|
||||||
Window Geometry:
|
Window Geometry:
|
||||||
Displays:
|
Displays:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Height: 536
|
Height: 576
|
||||||
Hide Left Dock: false
|
Hide Left Dock: false
|
||||||
Hide Right Dock: true
|
Hide Right Dock: true
|
||||||
Image:
|
Image:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
QMainWindow State: 000000ff00000000fd000000040000000000000179000001befc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000147000000c900fffffffb0000000a0049006d006100670065010000018a000000710000001600ffffff000000010000010f00000270fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000270000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000239000001be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
QMainWindow State: 000000ff00000000fd000000040000000000000179000001fafc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000174000000d700fffffffb0000000a0049006d00610067006501000001a2000000800000001600ffffff000000010000010f00000270fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000270000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000240000001fa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
Selection:
|
Selection:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Time:
|
Time:
|
||||||
|
|
@ -570,6 +565,6 @@ Window Geometry:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Views:
|
Views:
|
||||||
collapsed: true
|
collapsed: true
|
||||||
Width: 952
|
Width: 959
|
||||||
X: 72
|
X: 65
|
||||||
Y: 27
|
Y: 24
|
||||||
|
|
|
||||||
|
|
@ -1,209 +0,0 @@
|
||||||
cmake_minimum_required(VERSION 3.0.2)
|
|
||||||
project(spark_sagittarius_carry)
|
|
||||||
|
|
||||||
## Compile as C++11, supported in ROS Kinetic and newer
|
|
||||||
# add_compile_options(-std=c++11)
|
|
||||||
|
|
||||||
## 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
|
|
||||||
rospy
|
|
||||||
std_msgs
|
|
||||||
actionlib_msgs
|
|
||||||
message_generation
|
|
||||||
actionlib
|
|
||||||
)
|
|
||||||
|
|
||||||
## System dependencies are found with CMake's conventions
|
|
||||||
# find_package(Boost REQUIRED COMPONENTS system)
|
|
||||||
|
|
||||||
|
|
||||||
## Uncomment this if the package has a setup.py. This macro ensures
|
|
||||||
## modules and global scripts declared therein get installed
|
|
||||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
|
||||||
# catkin_python_setup()
|
|
||||||
|
|
||||||
################################################
|
|
||||||
## Declare ROS messages, services and actions ##
|
|
||||||
################################################
|
|
||||||
|
|
||||||
## To declare and build messages, services or actions from within this
|
|
||||||
## package, follow these steps:
|
|
||||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
|
||||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
|
||||||
## * In the file package.xml:
|
|
||||||
## * add a build_depend tag for "message_generation"
|
|
||||||
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
|
|
||||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
|
||||||
## but can be declared for certainty nonetheless:
|
|
||||||
## * add a exec_depend tag for "message_runtime"
|
|
||||||
## * In this file (CMakeLists.txt):
|
|
||||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
|
||||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
|
||||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
|
||||||
## catkin_package(CATKIN_DEPENDS ...)
|
|
||||||
## * uncomment the add_*_files sections below as needed
|
|
||||||
## and list every .msg/.srv/.action file to be processed
|
|
||||||
## * uncomment the generate_messages entry below
|
|
||||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
|
||||||
|
|
||||||
## Generate messages in the 'msg' folder
|
|
||||||
# add_message_files(
|
|
||||||
# FILES
|
|
||||||
# # position.msg
|
|
||||||
# # status.msg
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Generate services in the 'srv' folder
|
|
||||||
add_service_files(
|
|
||||||
FILES
|
|
||||||
scene.srv
|
|
||||||
)
|
|
||||||
|
|
||||||
## Generate actions in the 'action' folder
|
|
||||||
add_action_files(
|
|
||||||
FILES
|
|
||||||
SGRCtrl.action
|
|
||||||
# Action2.action
|
|
||||||
)
|
|
||||||
|
|
||||||
## Generate added messages and services with any dependencies listed here
|
|
||||||
generate_messages(
|
|
||||||
DEPENDENCIES
|
|
||||||
std_msgs # Or other packages containing msgs
|
|
||||||
actionlib_msgs
|
|
||||||
)
|
|
||||||
|
|
||||||
################################################
|
|
||||||
## Declare ROS dynamic reconfigure parameters ##
|
|
||||||
################################################
|
|
||||||
|
|
||||||
## To declare and build dynamic reconfigure parameters within this
|
|
||||||
## package, follow these steps:
|
|
||||||
## * In the file package.xml:
|
|
||||||
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
|
|
||||||
## * In this file (CMakeLists.txt):
|
|
||||||
## * add "dynamic_reconfigure" to
|
|
||||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
|
||||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
|
||||||
## and list every .cfg file to be processed
|
|
||||||
|
|
||||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
|
||||||
# generate_dynamic_reconfigure_options(
|
|
||||||
# cfg/DynReconf1.cfg
|
|
||||||
# cfg/DynReconf2.cfg
|
|
||||||
# )
|
|
||||||
|
|
||||||
###################################
|
|
||||||
## catkin specific configuration ##
|
|
||||||
###################################
|
|
||||||
## The catkin_package macro generates cmake config files for your package
|
|
||||||
## Declare things to be passed to dependent projects
|
|
||||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
|
||||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
|
||||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
|
||||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
|
||||||
catkin_package(
|
|
||||||
# INCLUDE_DIRS include
|
|
||||||
# LIBRARIES spark_sagittarius_carry
|
|
||||||
# CATKIN_DEPENDS roscpp rospy
|
|
||||||
# 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}
|
|
||||||
)
|
|
||||||
|
|
||||||
## Declare a C++ library
|
|
||||||
# add_library(${PROJECT_NAME}
|
|
||||||
# src/${PROJECT_NAME}/spark_sagittarius_carry.cpp
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Add cmake target dependencies of the library
|
|
||||||
## as an example, code may need to be generated before libraries
|
|
||||||
## either from message generation or dynamic reconfigure
|
|
||||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
|
||||||
|
|
||||||
## Declare a C++ executable
|
|
||||||
## With catkin_make all packages are built within a single CMake context
|
|
||||||
## The recommended prefix ensures that target names across packages don't collide
|
|
||||||
# add_executable(${PROJECT_NAME}_node src/spark_sagittarius_carry_node.cpp)
|
|
||||||
|
|
||||||
## Rename C++ executable without prefix
|
|
||||||
## The above recommended prefix causes long target names, the following renames the
|
|
||||||
## target back to the shorter version for ease of user use
|
|
||||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
|
||||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
|
||||||
|
|
||||||
## Add cmake target dependencies of the executable
|
|
||||||
## same as for the library above
|
|
||||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
|
||||||
|
|
||||||
## Specify libraries to link a library or executable target against
|
|
||||||
# target_link_libraries(${PROJECT_NAME}_node
|
|
||||||
# ${catkin_LIBRARIES}
|
|
||||||
# )
|
|
||||||
|
|
||||||
#############
|
|
||||||
## Install ##
|
|
||||||
#############
|
|
||||||
|
|
||||||
# all install targets should use catkin DESTINATION variables
|
|
||||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
|
||||||
|
|
||||||
## Mark executable scripts (Python etc.) for installation
|
|
||||||
## in contrast to setup.py, you can choose the destination
|
|
||||||
# catkin_install_python(PROGRAMS
|
|
||||||
# scripts/my_python_script
|
|
||||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Mark executables for installation
|
|
||||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
|
||||||
# install(TARGETS ${PROJECT_NAME}_node
|
|
||||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
|
||||||
# )
|
|
||||||
|
|
||||||
## 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}
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Mark cpp header files for installation
|
|
||||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
|
||||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
|
||||||
# FILES_MATCHING PATTERN "*.h"
|
|
||||||
# PATTERN ".svn" EXCLUDE
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
|
||||||
# install(FILES
|
|
||||||
# # myfile1
|
|
||||||
# # myfile2
|
|
||||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
# )
|
|
||||||
|
|
||||||
#############
|
|
||||||
## Testing ##
|
|
||||||
#############
|
|
||||||
|
|
||||||
## Add gtest based cpp test target and link libraries
|
|
||||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_spark_sagittarius_carry.cpp)
|
|
||||||
# if(TARGET ${PROJECT_NAME}-test)
|
|
||||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
|
||||||
# endif()
|
|
||||||
|
|
||||||
## Add folders to be run by python nosetests
|
|
||||||
# catkin_add_nosetests(test)
|
|
||||||
|
|
@ -1,50 +0,0 @@
|
||||||
# Define the goal
|
|
||||||
|
|
||||||
# grasp_type 参数的定义项
|
|
||||||
# 设置当前夹爪状态，在动作执行之前设置
|
|
||||||
uint8 GRASP_NONE = 0 # 自定义动作下使用，夹爪不做处理
|
|
||||||
uint8 GRASP_OPEN = 1 # 打开夹爪, 放下动作
|
|
||||||
uint8 GRASP_CLOSE = 2 # 闭合夹爪, 抓取动作
|
|
||||||
|
|
||||||
# action_type 参数的定义项
|
|
||||||
# 需要执行动作的类型，可以指定末端姿态或者自动计算
|
|
||||||
# 自定义姿态：是让机械臂到某个目标点
|
|
||||||
# 预设动作：是执行某个预设的一系列动作
|
|
||||||
uint8 ACTION_TYPE_XYZ = 0 # 自定义姿态：末端姿态使用动态计算
|
|
||||||
uint8 ACTION_TYPE_XYZ_RPY = 1 # 自定义姿态：末端姿态使用给定姿态
|
|
||||||
uint8 ACTION_TYPE_DEFINE_STAY = 2 # 预设动作：待机状态
|
|
||||||
uint8 ACTION_TYPE_PICK_XYZ = 3 # 预设动作：XYZ拾取动作
|
|
||||||
uint8 ACTION_TYPE_PICK_XYZ_RPY = 4 # 预设动作：XYZ+RPY拾取动作
|
|
||||||
uint8 ACTION_TYPE_PUT_XYZ = 5 # 预设动作：XYZ放下动作
|
|
||||||
uint8 ACTION_TYPE_PUT_XYZ_RPY = 6 # 预设动作：XYZ+RPY放下动作
|
|
||||||
|
|
||||||
uint8 grasp_type # 末端初始化状态
|
|
||||||
uint8 action_type # 动作类型
|
|
||||||
|
|
||||||
# 目标点位置
|
|
||||||
float64 pos_x
|
|
||||||
float64 pos_y
|
|
||||||
float64 pos_z
|
|
||||||
float64 pos_roll
|
|
||||||
float64 pos_pitch
|
|
||||||
float64 pos_yaw
|
|
||||||
|
|
||||||
---
|
|
||||||
# Define the result
|
|
||||||
uint8 SUCCESS = 0
|
|
||||||
uint8 ERROR = 1
|
|
||||||
uint8 PREEMPT = 2
|
|
||||||
uint8 PLAN_NOT_FOUND = 3
|
|
||||||
uint8 GRASP_FAILD = 4 # 抓空
|
|
||||||
|
|
||||||
# 执行结果
|
|
||||||
uint8 result
|
|
||||||
|
|
||||||
---
|
|
||||||
# Define a feedback message
|
|
||||||
uint8 PLANNING = 0
|
|
||||||
uint8 EXEC_POSITION = 1
|
|
||||||
uint8 EXEC_GRASP = 2
|
|
||||||
|
|
||||||
# 当前步骤
|
|
||||||
uint8 step
|
|
||||||
|
|
@ -1,6 +0,0 @@
|
||||||
h_max: 103
|
|
||||||
h_min: 91
|
|
||||||
s_max: 255
|
|
||||||
s_min: 65
|
|
||||||
v_max: 255
|
|
||||||
v_min: 63
|
|
||||||
|
|
@ -1,40 +0,0 @@
|
||||||
<!--
|
|
||||||
机械臂与摄像头之间的校准功能。
|
|
||||||
-->
|
|
||||||
<launch>
|
|
||||||
<!-- Camera -->
|
|
||||||
<arg name="camera_type_tel" default="d435" doc="camera type [astrapro, astra, d435...]"/>
|
|
||||||
<!-- 机械臂 -->
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default="$(arg robot_name)"/>
|
|
||||||
|
|
||||||
<!--Sagittarius 机械臂-->
|
|
||||||
<include file="$(find sagittarius_moveit)/launch/sgr532_moveit_in_spark.launch">
|
|
||||||
<arg name="use_rviz" value="false"/>
|
|
||||||
</include>
|
|
||||||
<node pkg="spark_sagittarius_carry" type="sgr_ctrl.py" name="sgr_ctrl_node" output="screen" ns="$(arg robot_name)"/>
|
|
||||||
|
|
||||||
<!--spark驱动，机器人描述，相机，底盘-->
|
|
||||||
<include file="$(find spark_bringup)/launch/driver_bringup.launch">
|
|
||||||
<arg name="camera_type_tel" value="$(arg camera_type_tel)" />
|
|
||||||
<arg name="enable_arm_tel" value="yes"/>
|
|
||||||
<arg name="arm_type_tel" value="sagittarius_arm"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<!--摄像头订阅与视觉定位-->
|
|
||||||
<node pkg="spark_sagittarius_carry" type="cali_cam_cv3.py" name="cali_cam_cv3_node" output="screen">
|
|
||||||
<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" if="$(eval arg('camera_type_tel')=='d435')"/>
|
|
||||||
<param name ="hsv_config" value = "$(find spark_sagittarius_carry)/config/hsv.yaml"/>
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<!--控制机械臂的定点移动-->
|
|
||||||
<node pkg="spark_sagittarius_carry" type="cali_pos.py" name="cali_pos_node" output="screen">
|
|
||||||
<param name="arm_name" value="$(arg robot_name)" />
|
|
||||||
</node>
|
|
||||||
<!--在rviz显示-->
|
|
||||||
<!--<arg name="rvizconfig" default="$(find spark_sagittarius_carry)/rviz/carry_object_$(arg camera_type_tel).rviz" />
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>-->
|
|
||||||
|
|
||||||
<!--是否开始 -->
|
|
||||||
<node pkg="spark_sagittarius_carry" type="publish_start_to_cali.sh" name="pstc" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,43 +0,0 @@
|
||||||
<!--
|
|
||||||
机械臂与摄像头之间的校准功能。
|
|
||||||
-->
|
|
||||||
<launch>
|
|
||||||
<!-- Camera -->
|
|
||||||
<arg name="camera_type_tel" default="d435" doc="camera type [astrapro, astra, d435...]"/>
|
|
||||||
<!-- 机械臂 -->
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default="$(arg robot_name)"/>
|
|
||||||
<arg name="fast_mode" default="true"/>
|
|
||||||
|
|
||||||
<!--Sagittarius 机械臂-->
|
|
||||||
<include file="$(find sagittarius_moveit)/launch/sgr532_moveit_in_spark.launch">
|
|
||||||
<arg name="use_rviz" value="false"/>
|
|
||||||
</include>
|
|
||||||
<node pkg="spark_sagittarius_carry" type="sgr_ctrl.py" name="sgr_ctrl_node" output="screen" ns="$(arg robot_name)"/>
|
|
||||||
|
|
||||||
<!--spark驱动，机器人描述，相机，底盘-->
|
|
||||||
<include file="$(find spark_bringup)/launch/driver_bringup.launch">
|
|
||||||
<arg name="camera_type_tel" value="$(arg camera_type_tel)" />
|
|
||||||
<arg name="enable_arm_tel" value="yes"/>
|
|
||||||
<arg name="arm_type_tel" value="sagittarius_arm"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<!--摄像头订阅与视觉定位-->
|
|
||||||
<node pkg="spark_sagittarius_carry" type="cali_cam_cv3.py" name="cali_cam_cv3_node" output="screen">
|
|
||||||
<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" if="$(eval arg('camera_type_tel')=='d435')"/>
|
|
||||||
<param name ="hsv_config" value = "$(find spark_sagittarius_carry)/config/hsv.yaml"/>
|
|
||||||
<param name="fast_mode" value="$(arg fast_mode)" />
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<!--控制机械臂的定点移动-->
|
|
||||||
<node pkg="spark_sagittarius_carry" type="cali_pos_auto.py" name="cali_pos_node" output="screen">
|
|
||||||
<param name="arm_name" value="$(arg robot_name)" />
|
|
||||||
<param name="fast_mode" value="$(arg fast_mode)" />
|
|
||||||
</node>
|
|
||||||
<!--在rviz显示-->
|
|
||||||
<!--<arg name="rvizconfig" default="$(find spark_sagittarius_carry)/rviz/carry_object_$(arg camera_type_tel).rviz" />
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>-->
|
|
||||||
|
|
||||||
<!--是否开始 -->
|
|
||||||
<node pkg="spark_sagittarius_carry" type="publish_start_to_cali_auto.sh" name="pstc"/>
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,40 +0,0 @@
|
||||||
<!-- NXROBO SPARK -->
|
|
||||||
<launch>
|
|
||||||
<arg name="lidar_type_tel" default="3iroboticslidar2" doc="lidar type [3iroboticslidar2, ydlidar_g2]"/>
|
|
||||||
<!-- 摄像机类型 -->
|
|
||||||
<arg name="camera_type_tel" default="d435" doc="camera type [astrapro, astra, d435...]"/>
|
|
||||||
<!-- 机械臂 -->
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default="$(arg robot_name)"/>
|
|
||||||
|
|
||||||
<!--Sagittarius 机械臂-->
|
|
||||||
<include file="$(find sagittarius_moveit)/launch/sgr532_moveit_in_spark.launch">
|
|
||||||
<arg name="use_rviz" value="false"/>
|
|
||||||
</include>
|
|
||||||
<node pkg="spark_sagittarius_carry" type="sgr_ctrl.py" name="sgr_ctrl_node" output="screen" ns="$(arg robot_name)"/>
|
|
||||||
|
|
||||||
<!--spark驱动，机器人描述，相机，底盘-->
|
|
||||||
<include file="$(find spark_bringup)/launch/driver_bringup.launch">
|
|
||||||
<arg name="camera_type_tel" value="$(arg camera_type_tel)" />
|
|
||||||
<arg name="enable_arm_tel" value="yes"/>
|
|
||||||
<arg name="arm_type_tel" value="sagittarius_arm"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<!--搬运物体 -->
|
|
||||||
<node pkg="spark_sagittarius_carry" type="s_s_carry_object_cv3.py" name="spark_sagittarius_carry_node" output="screen">
|
|
||||||
<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" if="$(eval arg('camera_type_tel')=='d435')"/>
|
|
||||||
<param name ="a_Pose" value = "0,0,0,0,0,0,1"/>
|
|
||||||
<param name ="b_Pose" value = "0.5,0.5,0,0,0,0,1"/>
|
|
||||||
<param name ="turnbody_min_z" value = "-0.2"/>
|
|
||||||
<param name ="turnbody_max_z" value = "0.2"/>
|
|
||||||
<param name ="hsv_config" value = "$(find spark_sagittarius_carry)/config/hsv.yaml"/>
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<!--在rviz显示 -->
|
|
||||||
<arg name="rvizconfig" default="$(find spark_sagittarius_carry)/rviz/rviz_$(arg camera_type_tel).rviz" />
|
|
||||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
|
||||||
|
|
||||||
<!--是否开始抓取 -->
|
|
||||||
<node pkg="spark_sagittarius_carry" type="cmd_spark_sgr_carry_start.sh" name="cscs" />
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,348 +0,0 @@
|
||||||
#!/usr/bin/python3
|
|
||||||
# -*- coding: UTF-8 -*-
|
|
||||||
import roslib
|
|
||||||
import sys
|
|
||||||
import rospy
|
|
||||||
import cv2
|
|
||||||
import numpy as np
|
|
||||||
import math
|
|
||||||
import os
|
|
||||||
import yaml
|
|
||||||
#import pandas as pd
|
|
||||||
from std_msgs.msg import String
|
|
||||||
from sensor_msgs.msg import Image
|
|
||||||
from cv_bridge import CvBridge, CvBridgeError
|
|
||||||
from sklearn.linear_model import LinearRegression
|
|
||||||
#from PIL import Image, ImageDraw, ImageFont
|
|
||||||
xc = 0
|
|
||||||
yc = 0
|
|
||||||
|
|
||||||
count = 9
|
|
||||||
index = 0
|
|
||||||
# first_findContours = True
|
|
||||||
# contours = []
|
|
||||||
|
|
||||||
arm_cmd_sub = None
|
|
||||||
|
|
||||||
xarray = None
|
|
||||||
yarray = None
|
|
||||||
xarray_list = None
|
|
||||||
yarray_list = None
|
|
||||||
xc_array = None
|
|
||||||
yc_array = None
|
|
||||||
|
|
||||||
cali_origin_x = 335
|
|
||||||
cali_origin_y = 320
|
|
||||||
cali_w = 15
|
|
||||||
cali_h = 15
|
|
||||||
collect_times = 200
|
|
||||||
start_flag = 1
|
|
||||||
put_cube = 1
|
|
||||||
move_position = 0
|
|
||||||
collecting = 0
|
|
||||||
c_cnt = 0
|
|
||||||
HSV_value = [0, 0, 0]
|
|
||||||
lower_HSV = np.array([0, 0, 0])
|
|
||||||
upper_HSV = np.array([0, 0, 0])
|
|
||||||
|
|
||||||
|
|
||||||
def mean_hsv(img, hsv_value):
|
|
||||||
HSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
|
|
||||||
hsv_value[0] += np.mean(HSV[:, :, 0])
|
|
||||||
hsv_value[1] += np.mean(HSV[:, :, 1])
|
|
||||||
hsv_value[2] += np.mean(HSV[:, :, 2])
|
|
||||||
return hsv_value
|
|
||||||
|
|
||||||
|
|
||||||
def hsv_range(hsv_value):
|
|
||||||
|
|
||||||
H_range = 6
|
|
||||||
S_range = 120
|
|
||||||
V_range = 120
|
|
||||||
|
|
||||||
lower_H = int(hsv_value[0] - H_range)
|
|
||||||
upper_H = int(hsv_value[0] + H_range)
|
|
||||||
|
|
||||||
lower_S = int(hsv_value[1] - S_range)
|
|
||||||
upper_S = int(hsv_value[1] + S_range)
|
|
||||||
|
|
||||||
lower_V = int(hsv_value[2] - V_range)
|
|
||||||
upper_V = int(hsv_value[2] + V_range)
|
|
||||||
|
|
||||||
if lower_H < 0:
|
|
||||||
lower_H = 0
|
|
||||||
if upper_H > 180:
|
|
||||||
upper_H = 180
|
|
||||||
|
|
||||||
if lower_S < 50:
|
|
||||||
lower_S = 50
|
|
||||||
if upper_S > 255:
|
|
||||||
upper_S = 255
|
|
||||||
|
|
||||||
if lower_V < 50:
|
|
||||||
lower_V = 50
|
|
||||||
if upper_V > 255:
|
|
||||||
upper_V = 255
|
|
||||||
|
|
||||||
lower_HSV = np.array([lower_H, lower_S, lower_V])
|
|
||||||
upper_HSV = np.array([upper_H, upper_S, upper_V])
|
|
||||||
return lower_HSV, upper_HSV
|
|
||||||
|
|
||||||
|
|
||||||
def test(lower_HSV, upper_HSV, image):
|
|
||||||
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
|
|
||||||
mask = cv2.inRange(hsv, lower_HSV, upper_HSV)
|
|
||||||
mask = cv2.erode(mask, None, iterations=2)
|
|
||||||
mask = cv2.dilate(mask, None, iterations=2)
|
|
||||||
mask = cv2.GaussianBlur(mask, (3, 3), 0)
|
|
||||||
cv2.putText(mask, 'Done! Press q to exit!', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
|
||||||
(255, 255, 255), 2, cv2.LINE_AA)
|
|
||||||
cv2.imshow("HSV_img", mask)
|
|
||||||
|
|
||||||
|
|
||||||
def image_callback(data):
|
|
||||||
global xc, yc
|
|
||||||
global move_position
|
|
||||||
global collecting
|
|
||||||
global c_cnt
|
|
||||||
global HSV_value
|
|
||||||
global lower_HSV, upper_HSV
|
|
||||||
global first_findContours
|
|
||||||
global contours
|
|
||||||
|
|
||||||
if start_flag:
|
|
||||||
return
|
|
||||||
# change to opencv
|
|
||||||
try:
|
|
||||||
cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
|
|
||||||
except CvBridgeError as e:
|
|
||||||
print(e)
|
|
||||||
cv_image_draw = cv_image1.copy()
|
|
||||||
if(put_cube):
|
|
||||||
cv2.putText(cv_image_draw, 'please place the blue cube directly under', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.putText(cv_image_draw, 'the gripper of the robotic arm!', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.putText(cv_image_draw, 'then press the \'ENTER\' ', (30, 130), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.putText(cv_image_draw, 'in another terminal!', (30, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
c_cnt = 0
|
|
||||||
cv2.imshow("win_draw", cv_image_draw)
|
|
||||||
cv2.waitKey(1)
|
|
||||||
return
|
|
||||||
elif(move_position):
|
|
||||||
cv2.putText(cv_image_draw, 'please move the camera to make ', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.putText(cv_image_draw, 'the blue cube color in the rectangle ', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.putText(cv_image_draw, 'green box! then press the \'ENTER\' ', (30, 130), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.putText(cv_image_draw, 'in another terminal!', (30, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.rectangle(cv_image_draw, (cali_origin_x, cali_origin_y),
|
|
||||||
(cali_origin_x + cali_w, cali_origin_y + cali_h), (0, 255, 0), 3)
|
|
||||||
c_cnt = 0
|
|
||||||
cv2.imshow("win_draw", cv_image_draw)
|
|
||||||
cv2.waitKey(1)
|
|
||||||
return
|
|
||||||
elif(collecting):
|
|
||||||
c_cnt = c_cnt+1
|
|
||||||
cv2.putText(cv_image_draw, 'collecting the hsv!', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.putText(cv_image_draw, ' please wait for 5s.', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 1.2,
|
|
||||||
(0, 255, 0), 2, cv2.LINE_AA)
|
|
||||||
cv2.rectangle(cv_image_draw, (cali_origin_x, cali_origin_y),
|
|
||||||
(cali_origin_x + cali_w, cali_origin_y + cali_h), (0, 255, 0), 3)
|
|
||||||
frame = cv_image1[cali_origin_y + 5:cali_origin_y +
|
|
||||||
cali_h-5, cali_origin_x + 5:cali_origin_x + cali_w-5]
|
|
||||||
HSV_value = mean_hsv(frame, HSV_value)
|
|
||||||
cv2.imshow("win_draw", cv_image_draw)
|
|
||||||
cv2.waitKey(1)
|
|
||||||
|
|
||||||
if(c_cnt >= collect_times):
|
|
||||||
for i in range(len(HSV_value)):
|
|
||||||
HSV_value[i] = HSV_value[i] / collect_times
|
|
||||||
print(i, len(HSV_value), HSV_value[i])
|
|
||||||
lower_HSV, upper_HSV = hsv_range(HSV_value)
|
|
||||||
#save_hsv(name, lower_HSV, upper_HSV)
|
|
||||||
print(lower_HSV, upper_HSV)
|
|
||||||
collecting = 0
|
|
||||||
cv2.destroyWindow("win_draw")
|
|
||||||
rospy.Subscriber("cali_pix_topic", String, command_callback)
|
|
||||||
arm_cmd_sub.publish(String("next"))
|
|
||||||
return
|
|
||||||
# test(lower_HSV, upper_HSV, cv_image1)
|
|
||||||
|
|
||||||
cv_image_cp = cv_image1.copy()
|
|
||||||
cv_image_hsv = cv2.cvtColor(cv_image_cp, cv2.COLOR_BGR2HSV)
|
|
||||||
cv_image_gray = cv2.inRange(cv_image_hsv, lower_HSV, upper_HSV)
|
|
||||||
# smooth and clean noise
|
|
||||||
cv_image_gray = cv2.erode(cv_image_gray, None, iterations=2)
|
|
||||||
cv_image_gray = cv2.dilate(cv_image_gray, None, iterations=2)
|
|
||||||
cv_image_gray = cv2.GaussianBlur(cv_image_gray, (5, 5), 0)
|
|
||||||
# detect contour
|
|
||||||
cv2.imshow("win1", cv_image1)
|
|
||||||
cv2.imshow("win2", cv_image_gray)
|
|
||||||
cv2.waitKey(1)
|
|
||||||
contours, hier = cv2.findContours(
|
|
||||||
cv_image_gray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
|
|
||||||
size = []
|
|
||||||
size_max = 0
|
|
||||||
for i, c in enumerate(contours):
|
|
||||||
rect = cv2.minAreaRect(c)
|
|
||||||
box = cv2.boxPoints(rect)
|
|
||||||
box = np.int0(box)
|
|
||||||
x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
|
|
||||||
y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
|
|
||||||
w = math.sqrt((box[0][0] - box[1][0])**2 + (box[0][1] - box[1][1])**2)
|
|
||||||
h = math.sqrt((box[0][0] - box[3][0])**2 + (box[0][1] - box[3][1])**2)
|
|
||||||
size.append(w * h)
|
|
||||||
if size[i] > size_max:
|
|
||||||
size_max = size[i]
|
|
||||||
index = i
|
|
||||||
xc = x_mid
|
|
||||||
yc = y_mid
|
|
||||||
|
|
||||||
# if first_findContours:
|
|
||||||
# # for i in list(enumerate(contours))[index][1]:
|
|
||||||
# # contours.append("%d,%d" % (i[0][0], i[0][1]))
|
|
||||||
# contours = ["%d,%d" % (int(i[0][0]), int(i[0][1])) for i in list(enumerate(contours))[index][1]]
|
|
||||||
# rospy.loginfo(contours)
|
|
||||||
# first_findContours = False
|
|
||||||
# cv2.imshow("win1", cv_image1)
|
|
||||||
|
|
||||||
|
|
||||||
def command_callback(data):
|
|
||||||
global xc_array
|
|
||||||
global yc_array
|
|
||||||
global xarray
|
|
||||||
global yarray
|
|
||||||
global index
|
|
||||||
global lower_HSV, upper_HSV
|
|
||||||
# s = '' + str(xc) + ' ' + str(yc) + '\n'
|
|
||||||
# file_pix = open('thefile.txt', 'a')
|
|
||||||
# file_pix.write(s)
|
|
||||||
# file_pix.close()
|
|
||||||
# print(xc, yc)
|
|
||||||
if index < count:
|
|
||||||
xc_array[index] = xc
|
|
||||||
yc_array[index] = yc
|
|
||||||
xarray[index] = xarray_list[index]
|
|
||||||
yarray[index] = yarray_list[index]
|
|
||||||
print("%d/%d,pose x,y: %.4f,%.4f. cam x,y: %d,%d" %
|
|
||||||
(index+1, count, xarray[index], yarray[index], xc, yc))
|
|
||||||
# reshape to 2D array for linear regression
|
|
||||||
xc_array = xc_array.reshape(-1, 1)
|
|
||||||
yc_array = yc_array.reshape(-1, 1)
|
|
||||||
xarray = xarray.reshape(-1, 1)
|
|
||||||
yarray = yarray.reshape(-1, 1)
|
|
||||||
index = index + 1
|
|
||||||
arm_cmd_sub.publish(String("next"))
|
|
||||||
if index == count:
|
|
||||||
Reg_x_yc = LinearRegression().fit(yc_array, xarray)
|
|
||||||
Reg_y_xc = LinearRegression().fit(xc_array, yarray)
|
|
||||||
k1 = Reg_x_yc.coef_[0][0]
|
|
||||||
b1 = Reg_x_yc.intercept_[0]
|
|
||||||
k2 = Reg_y_xc.coef_[0][0]
|
|
||||||
b2 = Reg_y_xc.intercept_[0]
|
|
||||||
s = '' + str(k1) + ' ' + str(b1) + ' ' + str(k2) + ' ' + str(b2) + '\n'
|
|
||||||
|
|
||||||
# contours.append('\n')
|
|
||||||
# s2 = ' '.join(contours)
|
|
||||||
filename = os.environ['HOME'] + "/thefile.txt"
|
|
||||||
file_pix = open(filename, 'w')
|
|
||||||
file_pix.write(s)
|
|
||||||
# file_pix.write(s2)
|
|
||||||
file_pix.close()
|
|
||||||
print("calibration params path: " + filename)
|
|
||||||
print("Linear Regression for x and yc is : x = %.5fyc + (%.5f)" % (k1, b1))
|
|
||||||
print("Linear Regression for y and xc is : y = %.5fxc + (%.5f)" % (k2, b2))
|
|
||||||
index = 0
|
|
||||||
try:
|
|
||||||
with open(rospy.get_param("~hsv_config", ""), "w") as f:
|
|
||||||
hsv_dist = {
|
|
||||||
'h_max':upper_HSV[0].item(),
|
|
||||||
'h_min':lower_HSV[0].item(),
|
|
||||||
's_max':upper_HSV[1].item(),
|
|
||||||
's_min':lower_HSV[1].item(),
|
|
||||||
'v_max':upper_HSV[2].item(),
|
|
||||||
'v_min':lower_HSV[2].item()
|
|
||||||
}
|
|
||||||
yaml.dump(hsv_dist, f)
|
|
||||||
except:
|
|
||||||
rospy.logwarn("can't open hsv config file")
|
|
||||||
rospy.logwarn("HSV (upper, lower)")
|
|
||||||
rospy.logwarn("H (%3d, %3d)" % (upper_HSV[0].item(), lower_HSV[0].item()))
|
|
||||||
rospy.logwarn("S (%3d, %3d)" % (upper_HSV[1].item(), lower_HSV[1].item()))
|
|
||||||
rospy.logwarn("V (%3d, %3d)" % (upper_HSV[2].item(), lower_HSV[2].item()))
|
|
||||||
print("******************************************************")
|
|
||||||
print(" finish the calibration. Press ctrl-c to exit ")
|
|
||||||
print(" 标定完成. 然后 Ctrl-C 退出程序 ")
|
|
||||||
print("******************************************************")
|
|
||||||
|
|
||||||
|
|
||||||
def msg_callback(data):
|
|
||||||
global start_flag
|
|
||||||
global put_cube
|
|
||||||
global move_position
|
|
||||||
global collecting
|
|
||||||
if(data.data == "start"):
|
|
||||||
start_flag = 0
|
|
||||||
put_cube = 1
|
|
||||||
elif(data.data == "start2"):
|
|
||||||
put_cube = 0
|
|
||||||
move_position = 1
|
|
||||||
elif(data.data == "start3"):
|
|
||||||
move_position = 0
|
|
||||||
collecting = 1
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
global arm_cmd_sub
|
|
||||||
global xarray_list
|
|
||||||
global yarray_list
|
|
||||||
global xarray
|
|
||||||
global yarray
|
|
||||||
global xc_array
|
|
||||||
global yc_array
|
|
||||||
global count
|
|
||||||
rospy.init_node('image_converter', anonymous=True)
|
|
||||||
r1 = rospy.Rate(1) # 1s
|
|
||||||
|
|
||||||
if rospy.get_param("~fast_mode", True):
|
|
||||||
# xarray_list = [0.22, 0.35, 0.35, 0.35, 0.22]
|
|
||||||
# yarray_list = [-0.1, -0.1, 0, 0.1, 0.1]
|
|
||||||
# count = 5
|
|
||||||
xarray_list = [0.22, 0.35, 0.35]
|
|
||||||
yarray_list = [0, -0.1, 0.1]
|
|
||||||
count = 3
|
|
||||||
else:
|
|
||||||
xarray_list = [0.28, 0.22, 0.22, 0.28, 0.35, 0.35, 0.35, 0.28, 0.22]
|
|
||||||
yarray_list = [0, 0, -0.1, -0.1, -0.1, 0, 0.1, 0.1, 0.1]
|
|
||||||
count = 9
|
|
||||||
xarray = np.zeros(count)
|
|
||||||
yarray = np.zeros(count)
|
|
||||||
xc_array = np.zeros(count)
|
|
||||||
yc_array = np.zeros(count)
|
|
||||||
|
|
||||||
print("Calibration node wait to start----")
|
|
||||||
# while(start_flag == 0):
|
|
||||||
# r1.sleep()
|
|
||||||
# if(start_flag == 0):
|
|
||||||
# return
|
|
||||||
|
|
||||||
sub1 = rospy.Subscriber("/camera/rgb/image_raw", Image, image_callback)
|
|
||||||
# sub2 = rospy.Subscriber("cali_pix_topic", String, command_callback)
|
|
||||||
sub3 = rospy.Subscriber("cali_cmd_topic", String, msg_callback)
|
|
||||||
arm_cmd_sub = rospy.Publisher('cali_arm_cmd_topic', String, queue_size=5)
|
|
||||||
|
|
||||||
try:
|
|
||||||
rospy.spin()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("Shutting down")
|
|
||||||
cv2.destroyAllWindows()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
|
|
@ -1,115 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
import sys
|
|
||||||
import rospy
|
|
||||||
import time
|
|
||||||
import _thread
|
|
||||||
from std_msgs.msg import String
|
|
||||||
|
|
||||||
import actionlib
|
|
||||||
from spark_sagittarius_carry.msg import SGRCtrlAction, SGRCtrlGoal
|
|
||||||
start_cali = 0
|
|
||||||
go_cali_pos = 0
|
|
||||||
next_cali_pos = 0
|
|
||||||
|
|
||||||
|
|
||||||
def msg_callback(data):
|
|
||||||
global start_cali
|
|
||||||
global go_cali_pos
|
|
||||||
global next_cali_pos
|
|
||||||
|
|
||||||
if(data.data == "start"):
|
|
||||||
start_cali = 1
|
|
||||||
elif(data.data == "go"):
|
|
||||||
go_cali_pos = 1
|
|
||||||
elif(data.data == "next"):
|
|
||||||
next_cali_pos = 1
|
|
||||||
|
|
||||||
|
|
||||||
def talker(threadName, delay):
|
|
||||||
global start_cali
|
|
||||||
global go_cali_pos
|
|
||||||
global next_cali_pos
|
|
||||||
|
|
||||||
# pub1 = rospy.Publisher('position_write_topic', position, queue_size=5)
|
|
||||||
client = actionlib.SimpleActionClient(
|
|
||||||
rospy.get_param("~arm_name", "sgr532") + '/' + 'sgr_ctrl', SGRCtrlAction)
|
|
||||||
client.wait_for_server()
|
|
||||||
pub2 = rospy.Publisher('cali_pix_topic', String, queue_size=5)
|
|
||||||
sub3 = rospy.Subscriber("cali_arm_cmd_topic", String, msg_callback)
|
|
||||||
goal = SGRCtrlGoal()
|
|
||||||
goal_stay = SGRCtrlGoal()
|
|
||||||
r1 = rospy.Rate(1) # 1s
|
|
||||||
r2 = rospy.Rate(0.05) # 20s
|
|
||||||
r3 = rospy.Rate(0.2) # 5s
|
|
||||||
r4 = rospy.Rate(0.4) # 2.5s
|
|
||||||
r1.sleep()
|
|
||||||
|
|
||||||
goal_stay.grasp_type = goal.GRASP_OPEN
|
|
||||||
goal_stay.action_type = goal.ACTION_TYPE_DEFINE_STAY
|
|
||||||
client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30))
|
|
||||||
|
|
||||||
goal.action_type = goal.ACTION_TYPE_XYZ_RPY
|
|
||||||
goal.pos_z = -0.1
|
|
||||||
goal.pos_pitch = 1.57
|
|
||||||
point_list = [
|
|
||||||
[0.28, 0], # 第一次用于引导用户调整摄像头位置的姿态
|
|
||||||
[0.28, 0],
|
|
||||||
[0.22, 0],
|
|
||||||
[0.22, -0.1],
|
|
||||||
[0.28, -0.1],
|
|
||||||
[0.35, -0.1],
|
|
||||||
[0.35, 0],
|
|
||||||
[0.35, 0.1],
|
|
||||||
[0.28, 0.1],
|
|
||||||
[0.22, 0.1]
|
|
||||||
]
|
|
||||||
|
|
||||||
print("Calibration pose node wait to start----")
|
|
||||||
while(start_cali == 0):
|
|
||||||
r1.sleep()
|
|
||||||
if(start_cali == 0):
|
|
||||||
return
|
|
||||||
|
|
||||||
print("start to move the sagittarius----")
|
|
||||||
for i in range(len(point_list)):
|
|
||||||
if rospy.is_shutdown():
|
|
||||||
break
|
|
||||||
# r1.sleep()
|
|
||||||
|
|
||||||
# 移动到标定位置，引导用户放置方块的地方
|
|
||||||
goal.pos_x = point_list[i][0]
|
|
||||||
goal.pos_y = point_list[i][1]
|
|
||||||
client.send_goal_and_wait(goal, rospy.Duration.from_sec(30))
|
|
||||||
print("Point %d: %.4f, %.4f" % (i + 1, goal.pos_x, goal.pos_y))
|
|
||||||
|
|
||||||
|
|
||||||
# 等待用户放好方块后，机械臂移动到其他地方，不遮挡摄像头
|
|
||||||
while(go_cali_pos == 0):
|
|
||||||
r1.sleep()
|
|
||||||
if(go_cali_pos == 0):
|
|
||||||
return
|
|
||||||
go_cali_pos = 0
|
|
||||||
client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30))
|
|
||||||
pub2.publish("")
|
|
||||||
|
|
||||||
# 等待标定数据采集后进行下一个动作
|
|
||||||
while(next_cali_pos == 0):
|
|
||||||
r1.sleep()
|
|
||||||
if(next_cali_pos == 0):
|
|
||||||
return
|
|
||||||
next_cali_pos = 0
|
|
||||||
|
|
||||||
r4.sleep()
|
|
||||||
|
|
||||||
print("end move----")
|
|
||||||
r3.sleep()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
rospy.init_node('cali_pos', anonymous=True)
|
|
||||||
# sub3 = rospy.Subscriber("cali_arm_cmd_topic", String, msg_callback)
|
|
||||||
_thread.start_new_thread(talker, ("Thread-1", 2, ))
|
|
||||||
try:
|
|
||||||
rospy.spin()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("Shutting down")
|
|
||||||
|
|
@ -1,145 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
import sys
|
|
||||||
import rospy
|
|
||||||
import time
|
|
||||||
import _thread
|
|
||||||
from std_msgs.msg import String
|
|
||||||
|
|
||||||
import actionlib
|
|
||||||
from spark_sagittarius_carry.msg import SGRCtrlAction, SGRCtrlGoal
|
|
||||||
start_cali = 0
|
|
||||||
go_cali_pos = 0
|
|
||||||
next_cali_pos = 0
|
|
||||||
|
|
||||||
|
|
||||||
def msg_callback(data):
|
|
||||||
global start_cali
|
|
||||||
global go_cali_pos
|
|
||||||
global next_cali_pos
|
|
||||||
|
|
||||||
if(data.data == "start"):
|
|
||||||
start_cali = 1
|
|
||||||
elif(data.data == "go"):
|
|
||||||
go_cali_pos = 1
|
|
||||||
elif(data.data == "next"):
|
|
||||||
next_cali_pos = 1
|
|
||||||
|
|
||||||
|
|
||||||
def talker(threadName, delay):
|
|
||||||
global start_cali
|
|
||||||
global go_cali_pos
|
|
||||||
global next_cali_pos
|
|
||||||
|
|
||||||
# pub1 = rospy.Publisher('position_write_topic', position, queue_size=5)
|
|
||||||
client = actionlib.SimpleActionClient(
|
|
||||||
rospy.get_param("~arm_name", "sgr532") + '/' + 'sgr_ctrl', SGRCtrlAction)
|
|
||||||
client.wait_for_server()
|
|
||||||
pub2 = rospy.Publisher('cali_pix_topic', String, queue_size=5)
|
|
||||||
sub3 = rospy.Subscriber("cali_arm_cmd_topic", String, msg_callback)
|
|
||||||
goal = SGRCtrlGoal()
|
|
||||||
goal_stay = SGRCtrlGoal()
|
|
||||||
r1 = rospy.Rate(1) # 1s
|
|
||||||
r2 = rospy.Rate(0.05) # 20s
|
|
||||||
r3 = rospy.Rate(0.2) # 5s
|
|
||||||
r4 = rospy.Rate(0.4) # 2.5s
|
|
||||||
r1.sleep()
|
|
||||||
|
|
||||||
goal_stay.grasp_type = goal_stay.GRASP_OPEN
|
|
||||||
goal_stay.action_type = goal_stay.ACTION_TYPE_DEFINE_STAY
|
|
||||||
client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30))
|
|
||||||
|
|
||||||
goal.action_type = goal.ACTION_TYPE_XYZ_RPY
|
|
||||||
goal.pos_x = 0.28
|
|
||||||
goal.pos_z = -0.12
|
|
||||||
goal.pos_pitch = 1.57
|
|
||||||
|
|
||||||
if rospy.get_param("~fast_mode", True):
|
|
||||||
# point_list = [
|
|
||||||
# [0.28, 0],
|
|
||||||
# [0.22, -0.1],
|
|
||||||
# [0.35, -0.1],
|
|
||||||
# [0.35, 0],
|
|
||||||
# [0.35, 0.1],
|
|
||||||
# [0.22, 0.1]
|
|
||||||
# ]
|
|
||||||
point_list = [
|
|
||||||
[0.28, 0],
|
|
||||||
[0.22, 0],
|
|
||||||
[0.35, -0.1],
|
|
||||||
[0.35, 0.1]
|
|
||||||
]
|
|
||||||
else:
|
|
||||||
point_list = [
|
|
||||||
[0.28, 0], # 第一次用于引导用户调整摄像头位置的姿态
|
|
||||||
[0.28, 0],
|
|
||||||
[0.22, 0],
|
|
||||||
[0.22, -0.1],
|
|
||||||
[0.28, -0.1],
|
|
||||||
[0.35, -0.1],
|
|
||||||
[0.35, 0],
|
|
||||||
[0.35, 0.1],
|
|
||||||
[0.28, 0.1],
|
|
||||||
[0.22, 0.1]
|
|
||||||
]
|
|
||||||
|
|
||||||
print("Calibration pose node wait to start----")
|
|
||||||
while(start_cali == 0):
|
|
||||||
r1.sleep()
|
|
||||||
if(start_cali == 0):
|
|
||||||
return
|
|
||||||
|
|
||||||
client.send_goal_and_wait(goal, rospy.Duration.from_sec(30))
|
|
||||||
while(go_cali_pos == 0):
|
|
||||||
r1.sleep()
|
|
||||||
if(go_cali_pos == 0):
|
|
||||||
return
|
|
||||||
go_cali_pos = 0
|
|
||||||
client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30))
|
|
||||||
pub2.publish("")
|
|
||||||
while(next_cali_pos == 0):
|
|
||||||
r1.sleep()
|
|
||||||
if(next_cali_pos == 0):
|
|
||||||
return
|
|
||||||
next_cali_pos = 0
|
|
||||||
|
|
||||||
print("start to move the sagittarius----")
|
|
||||||
for i in range(1, len(point_list)):
|
|
||||||
if rospy.is_shutdown():
|
|
||||||
break
|
|
||||||
# r1.sleep()
|
|
||||||
|
|
||||||
# 移动到标定位置
|
|
||||||
goal.action_type = goal.ACTION_TYPE_PICK_XYZ_RPY
|
|
||||||
goal.pos_x = point_list[i - 1][0]
|
|
||||||
goal.pos_y = point_list[i - 1][1]
|
|
||||||
client.send_goal_and_wait(goal, rospy.Duration.from_sec(30))
|
|
||||||
|
|
||||||
goal.action_type = goal.ACTION_TYPE_PUT_XYZ_RPY
|
|
||||||
goal.pos_x = point_list[i][0]
|
|
||||||
goal.pos_y = point_list[i][1]
|
|
||||||
client.send_goal_and_wait(goal, rospy.Duration.from_sec(30))
|
|
||||||
client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30))
|
|
||||||
print("Point %d: %.4f, %.4f" % (i + 1, goal.pos_x, goal.pos_y))
|
|
||||||
pub2.publish("")
|
|
||||||
|
|
||||||
# 等待标定数据采集后进行下一个动作
|
|
||||||
while(next_cali_pos == 0):
|
|
||||||
r1.sleep()
|
|
||||||
if(next_cali_pos == 0):
|
|
||||||
return
|
|
||||||
next_cali_pos = 0
|
|
||||||
|
|
||||||
r4.sleep()
|
|
||||||
|
|
||||||
print("end move----")
|
|
||||||
r3.sleep()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
rospy.init_node('cali_pos', anonymous=True)
|
|
||||||
# sub3 = rospy.Subscriber("cali_arm_cmd_topic", String, msg_callback)
|
|
||||||
_thread.start_new_thread(talker, ("Thread-1", 2, ))
|
|
||||||
try:
|
|
||||||
rospy.spin()
|
|
||||||
except KeyboardInterrupt:
|
|
||||||
print("Shutting down")
|
|
||||||
|
|
@ -1,745 +0,0 @@
|
||||||
#!/usr/bin/env python3
|
|
||||||
# -*- coding: utf-8 -*-
|
|
||||||
|
|
||||||
import cv2
|
|
||||||
import os
|
|
||||||
import time
|
|
||||||
import random
|
|
||||||
import ctypes
|
|
||||||
|
|
||||||
import yaml
|
|
||||||
import roslib
|
|
||||||
import rospy
|
|
||||||
import smach
|
|
||||||
import smach_ros
|
|
||||||
import threading
|
|
||||||
import _thread
|
|
||||||
import string
|
|
||||||
import math
|
|
||||||
import cv2
|
|
||||||
import numpy as np
|
|
||||||
from geometry_msgs.msg import Twist
|
|
||||||
from std_msgs.msg import String
|
|
||||||
from spark_sagittarius_carry.srv import *
|
|
||||||
from spark_sagittarius_carry.msg import *
|
|
||||||
from cv_bridge import CvBridge, CvBridgeError
|
|
||||||
from sensor_msgs.msg import Image
|
|
||||||
|
|
||||||
from tf import TransformListener
|
|
||||||
import tf
|
|
||||||
|
|
||||||
import actionlib
|
|
||||||
from actionlib import GoalStatus
|
|
||||||
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionFeedback
|
|
||||||
from spark_sagittarius_carry.msg import SGRCtrlAction, SGRCtrlGoal
|
|
||||||
|
|
||||||
from smach import State, StateMachine, Concurrence, Container, UserData
|
|
||||||
from smach_ros import MonitorState, ServiceState, SimpleActionState, IntrospectionServer
|
|
||||||
from geometry_msgs.msg import Pose, Point, Quaternion
|
|
||||||
|
|
||||||
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
|
|
||||||
|
|
||||||
|
|
||||||
class GraspObject(State):
|
|
||||||
'''
|
|
||||||
抓取物体 功能
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
#global sub, pub1, pub2
|
|
||||||
|
|
||||||
State.__init__(self, outcomes=['succeeded', 'aborted', 'pose_notfound'])
|
|
||||||
self.task = 'grasp object'
|
|
||||||
self.is_found_object = False
|
|
||||||
hsv_path = rospy.get_param("~hsv_config")
|
|
||||||
try:
|
|
||||||
with open(hsv_path, "r") as f:
|
|
||||||
content = yaml.load(f.read())
|
|
||||||
self.hsv_upper = np.array([content['h_max'], content['s_max'], content['v_max']])
|
|
||||||
self.hsv_lower = np.array([content['h_min'], content['s_min'], content['v_min']])
|
|
||||||
except:
|
|
||||||
rospy.logerr("can't not open hsv file: ", hsv_path)
|
|
||||||
|
|
||||||
def execute(self, userdata):
|
|
||||||
'''
|
|
||||||
执行状态
|
|
||||||
:param userdata:
|
|
||||||
'''
|
|
||||||
self.is_found_object = False
|
|
||||||
self.onInit()
|
|
||||||
rate = rospy.Rate(10)
|
|
||||||
|
|
||||||
while not self.is_found_object:
|
|
||||||
rate.sleep()
|
|
||||||
print("not found\n")
|
|
||||||
print("unregisting sub\n")
|
|
||||||
self.sub.unregister()
|
|
||||||
print("unregisted sub\n")
|
|
||||||
if self.grasp() == GoalStatus.SUCCEEDED:
|
|
||||||
return 'succeeded'
|
|
||||||
else:
|
|
||||||
return 'pose_notfound'
|
|
||||||
|
|
||||||
def onInit(self):
|
|
||||||
global xc, yc, xc_prev, yc_prev, found_count
|
|
||||||
xc = 0
|
|
||||||
yc = 0
|
|
||||||
xc_prev = xc
|
|
||||||
yc_prev = yc
|
|
||||||
found_count = 0
|
|
||||||
#rospy.init_node('image_converter', anonymous=True)
|
|
||||||
self.sub = rospy.Subscriber(
|
|
||||||
"/camera/rgb/image_raw", Image, self.image_cb, queue_size=1)
|
|
||||||
self.client = actionlib.SimpleActionClient(
|
|
||||||
rospy.get_param("~arm_name", "sgr532") + '/' + 'sgr_ctrl', SGRCtrlAction)
|
|
||||||
self.client.wait_for_server()
|
|
||||||
|
|
||||||
def grasp(self):
|
|
||||||
print("start to grasp\n")
|
|
||||||
global xc, yc, found_count
|
|
||||||
# stop function
|
|
||||||
|
|
||||||
filename = os.environ['HOME'] + "/thefile.txt"
|
|
||||||
file_pix = open(filename, 'r')
|
|
||||||
s = file_pix.read()
|
|
||||||
file_pix.close()
|
|
||||||
print(s)
|
|
||||||
arr = s.split()
|
|
||||||
a1 = arr[0]
|
|
||||||
a2 = arr[1]
|
|
||||||
a3 = arr[2]
|
|
||||||
a4 = arr[3]
|
|
||||||
|
|
||||||
a = [0]*2
|
|
||||||
b = [0]*2
|
|
||||||
a[0] = float(a1)
|
|
||||||
a[1] = float(a2)
|
|
||||||
b[0] = float(a3)
|
|
||||||
b[1] = float(a4)
|
|
||||||
print('k and b value:', a[0], a[1], b[0], b[1])
|
|
||||||
|
|
||||||
goal_pick = SGRCtrlGoal()
|
|
||||||
goal_pick.grasp_type = SGRCtrlGoal.GRASP_OPEN
|
|
||||||
goal_pick.action_type = SGRCtrlGoal.ACTION_TYPE_PICK_XYZ
|
|
||||||
goal_pick.pos_z = -0.12
|
|
||||||
goal_stay = SGRCtrlGoal()
|
|
||||||
goal_stay.action_type = goal_stay.ACTION_TYPE_DEFINE_STAY
|
|
||||||
|
|
||||||
|
|
||||||
# go forward
|
|
||||||
goal_pick.pos_x = a[0] * yc + a[1]
|
|
||||||
goal_pick.pos_y = b[0] * xc + b[1]
|
|
||||||
ret = self.client.send_goal_and_wait(goal_pick, rospy.Duration.from_sec(30))
|
|
||||||
self.client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30))
|
|
||||||
return ret
|
|
||||||
|
|
||||||
def image_cb(self, data):
|
|
||||||
#print("received image\n")
|
|
||||||
global xc, yc, xc_prev, yc_prev, found_count
|
|
||||||
# change to opencv
|
|
||||||
try:
|
|
||||||
cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8")
|
|
||||||
except CvBridgeError as e:
|
|
||||||
print('error')
|
|
||||||
# change rgb to hsv
|
|
||||||
cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV)
|
|
||||||
|
|
||||||
# extract blue
|
|
||||||
# LowerBlue = np.array([88, 90, 80])
|
|
||||||
# UpperBlue = np.array([108, 255, 255])
|
|
||||||
LowerBlue = self.hsv_lower
|
|
||||||
UpperBlue = self.hsv_upper
|
|
||||||
mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue)
|
|
||||||
cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask)
|
|
||||||
|
|
||||||
# gray process
|
|
||||||
cv_image4 = cv_image3[:, :, 0]
|
|
||||||
|
|
||||||
# smooth and clean noise
|
|
||||||
blurred = cv2.blur(cv_image4, (9, 9))
|
|
||||||
(_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY)
|
|
||||||
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25))
|
|
||||||
cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel)
|
|
||||||
cv_image5 = cv2.erode(cv_image5, None, iterations=4)
|
|
||||||
cv_image5 = cv2.dilate(cv_image5, None, iterations=4)
|
|
||||||
|
|
||||||
edges = cv2.Canny(cv_image5,100,200)
|
|
||||||
cv2.waitKey(1)
|
|
||||||
|
|
||||||
# detect contour
|
|
||||||
cv2.imshow("win1", cv_image1)
|
|
||||||
cv2.imshow("win2", cv_image5)
|
|
||||||
cv2.imshow("edges", edges)
|
|
||||||
|
|
||||||
|
|
||||||
contours, hier = cv2.findContours(
|
|
||||||
cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
|
|
||||||
|
|
||||||
# if find contours, pick the biggest box
|
|
||||||
if len(contours) > 0:
|
|
||||||
size = []
|
|
||||||
size_max = 0
|
|
||||||
for i, c in enumerate(contours):
|
|
||||||
rect = cv2.minAreaRect(c)
|
|
||||||
box = cv2.boxPoints(rect)
|
|
||||||
box = np.int0(box)
|
|
||||||
x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4
|
|
||||||
y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4
|
|
||||||
w = math.sqrt((box[0][0] - box[1][0]) **
|
|
||||||
2 + (box[0][1] - box[1][1]) ** 2)
|
|
||||||
h = math.sqrt((box[0][0] - box[3][0]) **
|
|
||||||
2 + (box[0][1] - box[3][1]) ** 2)
|
|
||||||
size.append(w * h)
|
|
||||||
if size[i] > size_max:
|
|
||||||
size_max = size[i]
|
|
||||||
index = i
|
|
||||||
xc = x_mid
|
|
||||||
yc = y_mid
|
|
||||||
# if box is not moving for 20 times
|
|
||||||
# print found_count
|
|
||||||
if found_count >= 20:
|
|
||||||
self.is_found_object = True
|
|
||||||
else:
|
|
||||||
# if box is not moving
|
|
||||||
if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2:
|
|
||||||
found_count = found_count + 1
|
|
||||||
else:
|
|
||||||
found_count = 0
|
|
||||||
else:
|
|
||||||
found_count = 0
|
|
||||||
xc_prev = xc
|
|
||||||
yc_prev = yc
|
|
||||||
|
|
||||||
|
|
||||||
class ReleaseObject(State):
|
|
||||||
'''
|
|
||||||
释放物体 功能
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
global pub1, pub2
|
|
||||||
State.__init__(self, outcomes=['succeeded', 'aborted'])
|
|
||||||
self.task = 'release object'
|
|
||||||
#rospy.init_node('image_converter', anonymous=True)
|
|
||||||
# pub1 = rospy.Publisher('position_write_topic', position, queue_size=10)
|
|
||||||
# pub2 = rospy.Publisher('pump_topic', status, queue_size=4)
|
|
||||||
self.client = actionlib.SimpleActionClient(
|
|
||||||
rospy.get_param("~arm_name", "sgr532") + '/' + 'sgr_ctrl', SGRCtrlAction)
|
|
||||||
self.client.wait_for_server()
|
|
||||||
|
|
||||||
def execute(self, userdata):
|
|
||||||
'''
|
|
||||||
执行状态
|
|
||||||
:param userdata:
|
|
||||||
'''
|
|
||||||
global pub1, pub2
|
|
||||||
r1 = rospy.Rate(0.15) # 5s
|
|
||||||
r2 = rospy.Rate(1) # 1s
|
|
||||||
# pos = position()
|
|
||||||
goal_put = SGRCtrlGoal()
|
|
||||||
goal_put.grasp_type = SGRCtrlGoal.GRASP_NONE
|
|
||||||
goal_put.action_type = SGRCtrlGoal.ACTION_TYPE_PUT_XYZ
|
|
||||||
goal_put.pos_x = 0.28
|
|
||||||
goal_put.pos_y = 0
|
|
||||||
goal_put.pos_z = -0.08
|
|
||||||
goal_stay = SGRCtrlGoal()
|
|
||||||
goal_stay.action_type = goal_stay.ACTION_TYPE_DEFINE_STAY
|
|
||||||
|
|
||||||
self.client.send_goal_and_wait(goal_put, rospy.Duration.from_sec(30))
|
|
||||||
self.client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30))
|
|
||||||
# go forward
|
|
||||||
# pos.x = 200
|
|
||||||
# pos.y = 0
|
|
||||||
# pos.z = -40 # -80
|
|
||||||
# pub1.publish(pos)
|
|
||||||
# r1.sleep()
|
|
||||||
|
|
||||||
# # stop pump
|
|
||||||
# pub2.publish(0)
|
|
||||||
# r2.sleep()
|
|
||||||
# r1.sleep()
|
|
||||||
# # go back home
|
|
||||||
# pos.x = 120
|
|
||||||
# pos.y = 0
|
|
||||||
# pos.z = 35
|
|
||||||
# pub1.publish(pos)
|
|
||||||
r1.sleep()
|
|
||||||
return 'succeeded'
|
|
||||||
|
|
||||||
|
|
||||||
# 获得厨房位置
|
|
||||||
class GetLocation(State):
|
|
||||||
'''
|
|
||||||
生成导航位置
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
State.__init__(self, outcomes=[
|
|
||||||
'succeeded', 'aborted'], output_keys=['waypoint_out'])
|
|
||||||
self.nav_pose = [rospy.get_param(
|
|
||||||
"~a_Pose"), rospy.get_param("~b_Pose")]
|
|
||||||
self.pose_idx = 0
|
|
||||||
|
|
||||||
def execute(self, userdata):
|
|
||||||
'''
|
|
||||||
状态执行函数
|
|
||||||
:param userdata:
|
|
||||||
'''
|
|
||||||
self.loc_pose = self.get_pose()
|
|
||||||
if self.loc_pose is None:
|
|
||||||
return "aborted"
|
|
||||||
userdata.waypoint_out = self.loc_pose
|
|
||||||
return 'succeeded'
|
|
||||||
|
|
||||||
def get_pose(self):
|
|
||||||
'''
|
|
||||||
获得导航点的位置和姿态
|
|
||||||
'''
|
|
||||||
self.pose_idx = (self.pose_idx + 1) % 2
|
|
||||||
# to generate pose
|
|
||||||
pose = self.split_str(self.nav_pose[self.pose_idx])
|
|
||||||
print(pose)
|
|
||||||
return pose
|
|
||||||
|
|
||||||
def split_str(self, str):
|
|
||||||
|
|
||||||
loc_array = str.split(',')
|
|
||||||
if len(loc_array) != 7:
|
|
||||||
return None
|
|
||||||
pose = Pose(Point(0, 0, 0), Quaternion(0.0, 0.0, 0.0, 1.0))
|
|
||||||
pose.position.x = float(loc_array[0])
|
|
||||||
pose.position.y = float(loc_array[1])
|
|
||||||
pose.position.z = float(loc_array[2])
|
|
||||||
pose.orientation.x = float(loc_array[3])
|
|
||||||
pose.orientation.y = float(loc_array[4])
|
|
||||||
pose.orientation.z = float(loc_array[5])
|
|
||||||
pose.orientation.w = float(loc_array[6])
|
|
||||||
|
|
||||||
return pose
|
|
||||||
|
|
||||||
|
|
||||||
class Nav2Waypoint(State):
|
|
||||||
'''
|
|
||||||
导航：巡航到指定的地点，通过move base
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
State.__init__(self, outcomes=['succeeded', 'aborted'],
|
|
||||||
input_keys=['waypoint_in'])
|
|
||||||
|
|
||||||
# Subscribe to the move_base action server
|
|
||||||
# self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
|
|
||||||
# Wait up to 60 seconds for the action server to become available
|
|
||||||
global move_base
|
|
||||||
move_base.wait_for_server(rospy.Duration(5))
|
|
||||||
|
|
||||||
rospy.loginfo("Connected to move_base action server")
|
|
||||||
|
|
||||||
self.goal = MoveBaseGoal()
|
|
||||||
self.goal.target_pose.header.frame_id = "/map"
|
|
||||||
|
|
||||||
def execute(self, userdata):
|
|
||||||
'''
|
|
||||||
状态执行函数
|
|
||||||
:param userdata:用户数据
|
|
||||||
'''
|
|
||||||
self.goal.target_pose.pose = userdata.waypoint_in
|
|
||||||
|
|
||||||
# return 'succeeded'
|
|
||||||
|
|
||||||
self.goal.target_pose.header.stamp = rospy.Time.now()
|
|
||||||
if self.goal.target_pose.pose is None:
|
|
||||||
return 'aborted'
|
|
||||||
# Send the goal pose to the MoveBaseAction server
|
|
||||||
global move_base
|
|
||||||
move_base.send_goal(self.goal)
|
|
||||||
|
|
||||||
if self.preempt_requested():
|
|
||||||
self.service_preempt()
|
|
||||||
return 'preempted'
|
|
||||||
rospy.loginfo('wait for result')
|
|
||||||
|
|
||||||
# Allow 1 minute to get there
|
|
||||||
finished_within_time = move_base.wait_for_result(rospy.Duration(60))
|
|
||||||
|
|
||||||
# If the robot don't get there in time, abort the goal
|
|
||||||
if not finished_within_time:
|
|
||||||
move_base.cancel_goal()
|
|
||||||
rospy.loginfo("Timed out achieving goal")
|
|
||||||
return 'aborted'
|
|
||||||
else:
|
|
||||||
# We made it!
|
|
||||||
state = move_base.get_state()
|
|
||||||
if state == GoalStatus.SUCCEEDED:
|
|
||||||
rospy.loginfo("Goal succeeded!")
|
|
||||||
return 'succeeded'
|
|
||||||
|
|
||||||
|
|
||||||
class TurnBody(State):
|
|
||||||
'''
|
|
||||||
转动身体 功能
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
State.__init__(self, outcomes=['succeeded', 'aborted'])
|
|
||||||
self.task = 'turn body'
|
|
||||||
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
|
|
||||||
self.min_z = rospy.get_param("~turnbody_min_z", 0.2)
|
|
||||||
self.max_z = rospy.get_param("~turnbody_max_z", 0.6)
|
|
||||||
|
|
||||||
def execute(self, userdata):
|
|
||||||
'''
|
|
||||||
执行状态
|
|
||||||
:param userdata:
|
|
||||||
'''
|
|
||||||
cmd_vel = Twist()
|
|
||||||
cmd_vel.angular.z = random.uniform(self.min_z, self.max_z)
|
|
||||||
print(cmd_vel.angular.z)
|
|
||||||
rate = rospy.Rate(25)
|
|
||||||
for i in range(25):
|
|
||||||
self.cmd_vel_pub.publish(cmd_vel)
|
|
||||||
rate.sleep()
|
|
||||||
|
|
||||||
# self.cmd_vel_pub.publish(cmd)
|
|
||||||
return 'succeeded'
|
|
||||||
|
|
||||||
|
|
||||||
class Go2Goal(State):
|
|
||||||
'''
|
|
||||||
转动身体 功能
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
State.__init__(self, outcomes=['succeeded', 'aborted'])
|
|
||||||
self.task = 'go line'
|
|
||||||
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
|
|
||||||
self.min_z = rospy.get_param("~turnbody_min_z", 0.2)
|
|
||||||
self.max_z = rospy.get_param("~turnbody_max_z", 0.6)
|
|
||||||
|
|
||||||
def execute(self, userdata):
|
|
||||||
'''
|
|
||||||
执行状态
|
|
||||||
:param userdata:
|
|
||||||
'''
|
|
||||||
cmd_vel = Twist()
|
|
||||||
cmd_vel.angular.z = 0.25
|
|
||||||
print(cmd_vel.angular.z)
|
|
||||||
rate = rospy.Rate(10)
|
|
||||||
for i in range(50):
|
|
||||||
self.cmd_vel_pub.publish(cmd_vel)
|
|
||||||
rate.sleep()
|
|
||||||
|
|
||||||
cmd_vel = Twist()
|
|
||||||
#cmd_vel.angular.z = random.uniform(self.min_z, self.max_z)
|
|
||||||
cmd_vel.linear.x = 0.15
|
|
||||||
print(cmd_vel.angular.z)
|
|
||||||
#rate = rospy.Rate(10)
|
|
||||||
for i in range(25):
|
|
||||||
self.cmd_vel_pub.publish(cmd_vel)
|
|
||||||
rate.sleep()
|
|
||||||
|
|
||||||
# self.cmd_vel_pub.publish(cmd)
|
|
||||||
return 'succeeded'
|
|
||||||
|
|
||||||
|
|
||||||
class TurnBodyQuar(State):
|
|
||||||
'''
|
|
||||||
转动身体 功能
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
State.__init__(self, outcomes=['succeeded', 'aborted'])
|
|
||||||
self.task = 'turn body quar'
|
|
||||||
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
|
|
||||||
self.min_z = rospy.get_param("~turnbody_min_z", 0.2)
|
|
||||||
self.max_z = rospy.get_param("~turnbody_max_z", 0.6)
|
|
||||||
|
|
||||||
def execute(self, userdata):
|
|
||||||
'''
|
|
||||||
执行状态
|
|
||||||
:param userdata:
|
|
||||||
'''
|
|
||||||
cmd_vel = Twist()
|
|
||||||
cmd_vel.angular.z = 0.25
|
|
||||||
print(cmd_vel.angular.z)
|
|
||||||
rate = rospy.Rate(10)
|
|
||||||
for i in range(100):
|
|
||||||
self.cmd_vel_pub.publish(cmd_vel)
|
|
||||||
rate.sleep()
|
|
||||||
|
|
||||||
# self.cmd_vel_pub.publish(cmd)
|
|
||||||
return 'succeeded'
|
|
||||||
|
|
||||||
|
|
||||||
class EndAbort(State):
|
|
||||||
'''
|
|
||||||
提示结果失败
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
State.__init__(self, outcomes=['succeeded'])
|
|
||||||
# self.play_sound = PlaySound()
|
|
||||||
self.task = 'end abort'
|
|
||||||
|
|
||||||
def execute(self, userdata):
|
|
||||||
'''
|
|
||||||
执行状态
|
|
||||||
:param userdata:
|
|
||||||
'''
|
|
||||||
return 'succeeded'
|
|
||||||
|
|
||||||
|
|
||||||
class EndSuccess(State):
|
|
||||||
'''
|
|
||||||
提示结果成功
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
State.__init__(self, outcomes=['succeeded'])
|
|
||||||
# self.play_sound = PlaySound()
|
|
||||||
self.task = 'end success'
|
|
||||||
|
|
||||||
def execute(self, userdata):
|
|
||||||
'''
|
|
||||||
执行状态
|
|
||||||
:param userdata:
|
|
||||||
'''
|
|
||||||
self.sayword_pub.publish("任务完成")
|
|
||||||
return 'succeeded'
|
|
||||||
|
|
||||||
|
|
||||||
class CarryObject():
|
|
||||||
'''
|
|
||||||
场景：搬运物体
|
|
||||||
功能：将目标运行到指定的位置
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
'''
|
|
||||||
初始化
|
|
||||||
'''
|
|
||||||
rospy.init_node('s_carry_object_node', anonymous=False)
|
|
||||||
|
|
||||||
rospy.on_shutdown(self.shutdown)
|
|
||||||
self.switch_status = 0
|
|
||||||
self.sm_carry_object = None
|
|
||||||
self.thr = None
|
|
||||||
self.init_service_and_topic()
|
|
||||||
# 控制
|
|
||||||
self.stopworld = "停止处理"
|
|
||||||
self.loaction_name = None
|
|
||||||
|
|
||||||
def scene_status_swith_cb(self, req: sceneResponse):
|
|
||||||
'''
|
|
||||||
场景状态切换回调函数，用于启动场景或者关闭场景
|
|
||||||
:param req:请求参数
|
|
||||||
'''
|
|
||||||
print("-----------req status:%d" % req.END)
|
|
||||||
return_flag = -1
|
|
||||||
if req.type == req.END:
|
|
||||||
self.switch_status = 0
|
|
||||||
global move_base
|
|
||||||
move_base.cancel_goal()
|
|
||||||
for i in range(10):
|
|
||||||
self.cmd_vel_pub.publish(Twist())
|
|
||||||
|
|
||||||
if not self.thr is None:
|
|
||||||
try:
|
|
||||||
self.terminate_thread(self.thr)
|
|
||||||
except:
|
|
||||||
pass
|
|
||||||
|
|
||||||
return_flag = 0
|
|
||||||
if req.type == req.RUN:
|
|
||||||
if self.switch_status == 0:
|
|
||||||
# thread.start_new_thread(self.timer, (1,1))
|
|
||||||
self.thr = threading.Thread(target=self.timer, args=(1, 1))
|
|
||||||
self.thr.start()
|
|
||||||
self.switch_status = 1
|
|
||||||
return_flag = 1
|
|
||||||
|
|
||||||
# self.master_s_pub.publish("",0)
|
|
||||||
print("s_carry_object status:%d" % return_flag)
|
|
||||||
return sceneResponse(return_flag)
|
|
||||||
|
|
||||||
def timer(self, no, interval):
|
|
||||||
'''
|
|
||||||
计时器
|
|
||||||
:param no:
|
|
||||||
:param interval:计时器间隔
|
|
||||||
'''
|
|
||||||
# time.sleep(interval);
|
|
||||||
rospy.loginfo("req infomaion, status:%s" % (self.switch_status))
|
|
||||||
if self.switch_status == 1:
|
|
||||||
rospy.loginfo("starting to sm")
|
|
||||||
self.build_state_machine()
|
|
||||||
self.execute_sm()
|
|
||||||
|
|
||||||
self.switch_status = 0
|
|
||||||
rospy.loginfo("stopped to sm")
|
|
||||||
thread.exit_thread()
|
|
||||||
|
|
||||||
def terminate_thread(self, thread):
|
|
||||||
"""
|
|
||||||
Terminates a python thread from another thread.
|
|
||||||
中止结束线程
|
|
||||||
:param thread: a threading.Thread instance
|
|
||||||
"""
|
|
||||||
try:
|
|
||||||
if not thread.isAlive():
|
|
||||||
return
|
|
||||||
|
|
||||||
exc = ctypes.py_object(SystemExit)
|
|
||||||
res = ctypes.pythonapi.PyThreadState_SetAsyncExc(
|
|
||||||
ctypes.c_long(thread.ident), exc)
|
|
||||||
if res == 0:
|
|
||||||
raise ValueError("nonexistent thread id")
|
|
||||||
elif res > 1:
|
|
||||||
# """if it returns a number greater than one, you're in trouble,
|
|
||||||
# and you should call it again with exc=NULL to revert the effect"""
|
|
||||||
ctypes.pythonapi.PyThreadState_SetAsyncExc(thread.ident, None)
|
|
||||||
raise SystemError("PyThreadState_SetAsyncExc failed")
|
|
||||||
except :
|
|
||||||
pass
|
|
||||||
self.cmd_vel_pub.publish(Twist())
|
|
||||||
|
|
||||||
def init_service_and_topic(self):
|
|
||||||
'''
|
|
||||||
初始化服务和话题
|
|
||||||
'''
|
|
||||||
self.switch_status_service = rospy.Service(
|
|
||||||
'/s_carry_object', scene, self.scene_status_swith_cb)
|
|
||||||
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
|
|
||||||
|
|
||||||
def build_state_machine(self):
|
|
||||||
'''
|
|
||||||
建立状态机
|
|
||||||
'''
|
|
||||||
if not self.sm_carry_object is None:
|
|
||||||
self.sm_carry_object = None
|
|
||||||
self.sm_carry_object = StateMachine(
|
|
||||||
outcomes=['succeeded', 'aborted', 'preempted'])
|
|
||||||
with self.sm_carry_object:
|
|
||||||
StateMachine.add('GRASP_OBJECT', GraspObject(), transitions={'succeeded': 'NAV_TO_WAYPOINT',
|
|
||||||
'pose_notfound': 'GRASP_OBJECT',
|
|
||||||
'aborted': 'END_ABORT'})
|
|
||||||
StateMachine.add('GET_LOCATION', GetLocation(), transitions={'succeeded': 'NAV_TO_WAYPOINT',
|
|
||||||
'aborted': 'END_ABORT'},
|
|
||||||
remapping={'waypoint_out': 'nav_waypoint'})
|
|
||||||
# StateMachine.add('NAV_TO_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'RELEASE_OBJECT','aborted':'END_ABORT'},
|
|
||||||
|
|
||||||
StateMachine.add('NAV_TO_WAYPOINT', Go2Goal(), transitions={'succeeded': 'RELEASE_OBJECT',
|
|
||||||
'aborted': 'END_ABORT'})
|
|
||||||
# remapping={'waypoint_in':'nav_waypoint'})
|
|
||||||
StateMachine.add('RELEASE_OBJECT', ReleaseObject(), transitions={'succeeded': 'TURN_BODY_QUAR',
|
|
||||||
'aborted': 'END_ABORT'})
|
|
||||||
#StateMachine.add('TURN_BODY', TurnBody(), transitions={'succeeded':'GRASP_OBJECT','aborted':'END_ABORT'})
|
|
||||||
StateMachine.add('TURN_BODY_QUAR', TurnBodyQuar(), transitions={
|
|
||||||
'succeeded': 'GRASP_OBJECT', 'aborted': 'END_ABORT'})
|
|
||||||
StateMachine.add('END_ABORT', EndAbort(), transitions={
|
|
||||||
'succeeded': 'succeeded'})
|
|
||||||
StateMachine.add('END_SUCESS', EndSuccess(),
|
|
||||||
transitions={'succeeded': 'succeeded'})
|
|
||||||
|
|
||||||
def stop_cb(self, msg):
|
|
||||||
'''
|
|
||||||
停止回调函数
|
|
||||||
:param msg:
|
|
||||||
'''
|
|
||||||
# rospy.loginfo("停止指令："+msg.data)
|
|
||||||
print(msg.data)
|
|
||||||
if msg.data == self.stopworld:
|
|
||||||
switch_client = rospy.ServiceProxy('s_carry_object', scene)
|
|
||||||
type = 0
|
|
||||||
param = ""
|
|
||||||
respon = switch_client(type, param)
|
|
||||||
|
|
||||||
def execute_sm(self):
|
|
||||||
'''
|
|
||||||
运行状态即机
|
|
||||||
'''
|
|
||||||
intro_server = IntrospectionServer(
|
|
||||||
'sm_carry_object', self.sm_carry_object, '/SM_ROOT')
|
|
||||||
intro_server.start()
|
|
||||||
# Execute the state machine
|
|
||||||
sm_outcome = self.sm_carry_object.execute()
|
|
||||||
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
|
|
||||||
intro_server.stop()
|
|
||||||
|
|
||||||
def shutdown(self):
|
|
||||||
'''
|
|
||||||
关闭状态机
|
|
||||||
'''
|
|
||||||
rospy.loginfo("Stopping carry object")
|
|
||||||
rospy.sleep(1)
|
|
||||||
self.cmd_vel_pub.publish(Twist())
|
|
||||||
# rospy.spin()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
try:
|
|
||||||
rospy.loginfo("Init carry object")
|
|
||||||
arm_name = rospy.get_param("~arm_name", "sgr532")
|
|
||||||
filename = os.environ['HOME'] + "/thefile.txt"
|
|
||||||
file_pix = open(filename, 'r')
|
|
||||||
# s = file_pix.read()
|
|
||||||
# file_pix.close()
|
|
||||||
# print(s)
|
|
||||||
# arr = s.split()
|
|
||||||
s = file_pix.readline()
|
|
||||||
print(s)
|
|
||||||
arr = s.split()
|
|
||||||
# s = file_pix.readline()
|
|
||||||
# print(s)
|
|
||||||
# arr2 = s.split()
|
|
||||||
# print("1111111111111111", arr2)
|
|
||||||
file_pix.close()
|
|
||||||
a1 = arr[0]
|
|
||||||
a2 = arr[1]
|
|
||||||
a3 = arr[2]
|
|
||||||
a4 = arr[3]
|
|
||||||
# a4=arr[3].replace('\n','') #readline 读取文件的时候，默认加上“\n"
|
|
||||||
print(a1)
|
|
||||||
print(a2)
|
|
||||||
print(a3)
|
|
||||||
print(a4)
|
|
||||||
n1 = float(a1)*2
|
|
||||||
a = [0]*2
|
|
||||||
b = [0]*2
|
|
||||||
a[0] = float(a1)
|
|
||||||
a[1] = float(a2)
|
|
||||||
b[0] = float(a3)
|
|
||||||
b[1] = float(a4)
|
|
||||||
print('k and b value:', a[0], a[1], b[0], b[1])
|
|
||||||
|
|
||||||
CarryObject()
|
|
||||||
rospy.spin()
|
|
||||||
except rospy.ROSInterruptException:
|
|
||||||
rospy.loginfo("Finised carry object")
|
|
||||||
|
|
@ -1,318 +0,0 @@
|
||||||
#!/usr/bin/env python
|
|
||||||
# -*- coding: UTF-8 -*-
|
|
||||||
'''
|
|
||||||
Descripttion: Saggitarius moveit application action
|
|
||||||
version: 1.00
|
|
||||||
Author: shudong.hong@nxrobo.com
|
|
||||||
Company: NXROBO (深圳创想未来机器人有限公司)
|
|
||||||
LastEditors: shudong.hong@nxrobo.com
|
|
||||||
'''
|
|
||||||
|
|
||||||
import math
|
|
||||||
import numpy as np
|
|
||||||
import rospy
|
|
||||||
import sys
|
|
||||||
from moveit_msgs.msg import MoveGroupActionFeedback
|
|
||||||
import moveit_commander
|
|
||||||
import tf.transformations as transformations
|
|
||||||
|
|
||||||
import actionlib
|
|
||||||
from spark_sagittarius_carry.msg import SGRCtrlAction, SGRCtrlGoal, SGRCtrlResult, SGRCtrlFeedback
|
|
||||||
from sdk_sagittarius_arm.srv import ServoRtInfo, ServoRtInfoRequest
|
|
||||||
|
|
||||||
ispython3 = True if sys.version_info.major == 3 else False
|
|
||||||
|
|
||||||
|
|
||||||
class MoveItSGRTool:
|
|
||||||
def __init__(self, init_pose=True, end_effector=None):
|
|
||||||
moveit_commander.roscpp_initialize(sys.argv)
|
|
||||||
self.robot_name = rospy.get_param("~robot_name", "sgr532")
|
|
||||||
|
|
||||||
# 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线
|
|
||||||
self.cartesian = rospy.get_param('~cartesian', False)
|
|
||||||
|
|
||||||
# 初始化需要使用move group控制的机械臂中的arm group
|
|
||||||
self.arm_group = moveit_commander.MoveGroupCommander('sagittarius_arm')
|
|
||||||
|
|
||||||
# 初始化需要使用move group控制的机械臂中的gripper group
|
|
||||||
self.gripper = moveit_commander.MoveGroupCommander(
|
|
||||||
'sagittarius_gripper')
|
|
||||||
|
|
||||||
# 当运动规划失败后，允许重新规划
|
|
||||||
self.arm_group.allow_replanning(False)
|
|
||||||
|
|
||||||
self.reference_frame = rospy.get_namespace()[1:] + 'base_link'
|
|
||||||
# 设置目标位置所使用的参考坐标系
|
|
||||||
self.arm_group.set_pose_reference_frame(self.reference_frame)
|
|
||||||
|
|
||||||
# 设置目标位置所使用的参考坐标系
|
|
||||||
self.gripper.set_pose_reference_frame(self.reference_frame)
|
|
||||||
|
|
||||||
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
|
|
||||||
self.arm_group.set_goal_position_tolerance(0.001)
|
|
||||||
self.arm_group.set_goal_orientation_tolerance(0.001)
|
|
||||||
|
|
||||||
self.gripper.set_goal_joint_tolerance(0.001)
|
|
||||||
# 设置允许的最大速度和加速度
|
|
||||||
self.arm_group.set_max_acceleration_scaling_factor(0.5)
|
|
||||||
self.arm_group.set_max_velocity_scaling_factor(0.5)
|
|
||||||
|
|
||||||
# 设置末端效应点link的名称
|
|
||||||
if (end_effector is not None):
|
|
||||||
self.arm_group.set_end_effector_link(end_effector)
|
|
||||||
|
|
||||||
# 获取终端link的名称
|
|
||||||
self.end_effector_link = self.arm_group.get_end_effector_link()
|
|
||||||
rospy.loginfo("end effector link: %s" % self.end_effector_link)
|
|
||||||
|
|
||||||
#
|
|
||||||
rospy.Subscriber("move_group/feedback", MoveGroupActionFeedback,
|
|
||||||
self._move_group_feedback_callback)
|
|
||||||
self.moveit_group_status = "IDLE"
|
|
||||||
|
|
||||||
# 控制机械臂先回到初始化位置
|
|
||||||
if init_pose:
|
|
||||||
self.arm_group.set_named_target('home')
|
|
||||||
self.arm_group.go()
|
|
||||||
rospy.sleep(1)
|
|
||||||
self.pose_stay()
|
|
||||||
|
|
||||||
def __del__(self):
|
|
||||||
moveit_commander.roscpp_shutdown()
|
|
||||||
moveit_commander.os._exit(0)
|
|
||||||
|
|
||||||
def _move_group_feedback_callback(self, fb):
|
|
||||||
self.moveit_group_status = fb.feedback.state
|
|
||||||
|
|
||||||
def isPlanSuccess(self, px, py, pz, roll=0, pitch=0, yaw=0):
|
|
||||||
self.arm_group.set_pose_target([px, py, pz, roll, pitch, yaw])
|
|
||||||
plan = self.arm_group.plan()
|
|
||||||
|
|
||||||
if ispython3:
|
|
||||||
plan = plan[1]
|
|
||||||
|
|
||||||
return len(plan.joint_trajectory.points) != 0
|
|
||||||
|
|
||||||
def gripper_catch(self):
|
|
||||||
self.gripper.set_joint_value_target([-0.021, -0.021])
|
|
||||||
ret = self.gripper.go()
|
|
||||||
if ret:
|
|
||||||
rospy.sleep(1)
|
|
||||||
return ret
|
|
||||||
|
|
||||||
def gripper_open(self):
|
|
||||||
self.gripper.set_joint_value_target([0.0, 0.0])
|
|
||||||
ret = self.gripper.go()
|
|
||||||
if ret:
|
|
||||||
rospy.sleep(1)
|
|
||||||
return ret
|
|
||||||
|
|
||||||
def to_pose_eular(self, times, px, py, pz, roll=0, pitch=0, yaw=0):
|
|
||||||
self.arm_group.set_pose_target([px, py, pz, roll, pitch, yaw])
|
|
||||||
plan = self.arm_group.plan()
|
|
||||||
|
|
||||||
if ispython3: # python3 返回的是列表，下标1是 RobotTrajectory，也可以用列表中 error_code:MoveItErrorCodes 判断
|
|
||||||
plan = plan[1]
|
|
||||||
|
|
||||||
if len(plan.joint_trajectory.points) != 0:
|
|
||||||
self.arm_group.execute(plan)
|
|
||||||
rospy.sleep(times)
|
|
||||||
return True
|
|
||||||
else:
|
|
||||||
rospy.logwarn("plan can not found!")
|
|
||||||
return False
|
|
||||||
|
|
||||||
def ee_target_offset(self, px, py, pz, roll=0, pitch=0, yaw=0, ee_type='grasp'):
|
|
||||||
M = transformations.compose_matrix(
|
|
||||||
angles=[roll, pitch, yaw], translate=[px, py, pz])
|
|
||||||
if ee_type == 'grasp':
|
|
||||||
M1 = np.dot(M, transformations.translation_matrix([-0.07, 0, 0]))
|
|
||||||
else:
|
|
||||||
M1 = M
|
|
||||||
scale, shear, angles, translate, perspective = transformations.decompose_matrix(
|
|
||||||
M1)
|
|
||||||
return translate[0], translate[1], translate[2], angles[0], angles[1], angles[2]
|
|
||||||
|
|
||||||
def ee_xyz_get_rpy(self, x, y, z):
|
|
||||||
# Get yaw by Arctangent of x and y
|
|
||||||
yaw = math.atan2(y, x)
|
|
||||||
roll = 0
|
|
||||||
pitch = 0
|
|
||||||
|
|
||||||
# 使用三角函数求 pitch 的近似值
|
|
||||||
# 模型：机械臂抽象成等腰三角形，机械臂末端到坐标原点的距离为底，机械臂臂长为两腰总和
|
|
||||||
# 求底边角
|
|
||||||
a = 0.532 / 2
|
|
||||||
b = 0.532 / 2
|
|
||||||
c = math.sqrt(x*x + y*y + z*z)
|
|
||||||
if a + b <= c:
|
|
||||||
# 三角形不成立
|
|
||||||
pitch = 0
|
|
||||||
else:
|
|
||||||
# 已知三角边长求指定角度
|
|
||||||
pitch = math.acos((a*a - b*b - c*c)/(-2*b*c)) - math.asin(z / c)
|
|
||||||
|
|
||||||
# 限定范围
|
|
||||||
if pitch > 1.57:
|
|
||||||
pitch = 1.57
|
|
||||||
elif pitch < 0:
|
|
||||||
pitch = 0
|
|
||||||
return roll, pitch, yaw
|
|
||||||
|
|
||||||
def pose_stay(self):
|
|
||||||
self.arm_group.set_joint_value_target(
|
|
||||||
[0, 1.4, -1.48, 0, 1.6, 0])
|
|
||||||
self.arm_group.go()
|
|
||||||
|
|
||||||
def stop(self):
|
|
||||||
self.arm_group.set_named_target('home')
|
|
||||||
self.arm_group.go()
|
|
||||||
rospy.sleep(1)
|
|
||||||
self.arm_group.set_named_target('sleep')
|
|
||||||
self.arm_group.go()
|
|
||||||
rospy.sleep(1)
|
|
||||||
self.gripper.set_joint_value_target([0, 0])
|
|
||||||
self.gripper.go()
|
|
||||||
rospy.sleep(0.5)
|
|
||||||
|
|
||||||
|
|
||||||
class CencelException(Exception):
|
|
||||||
pass
|
|
||||||
|
|
||||||
|
|
||||||
class SGRCtrlActionServer:
|
|
||||||
def __init__(self) -> None:
|
|
||||||
self.sgr_tool = MoveItSGRTool()
|
|
||||||
rospy.wait_for_service('get_servo_info', 3)
|
|
||||||
self.servo_info_srv = rospy.ServiceProxy('get_servo_info', ServoRtInfo)
|
|
||||||
self._server = actionlib.SimpleActionServer(
|
|
||||||
'sgr_ctrl', SGRCtrlAction, self.execute, False)
|
|
||||||
self._server.start()
|
|
||||||
|
|
||||||
def execute(self, goal:SGRCtrlGoal):
|
|
||||||
r = rospy.Rate(1)
|
|
||||||
|
|
||||||
resp = SGRCtrlResult()
|
|
||||||
fb = SGRCtrlFeedback()
|
|
||||||
|
|
||||||
# 初始化夹爪
|
|
||||||
fb.step = fb.EXEC_GRASP
|
|
||||||
self._server.publish_feedback(fb)
|
|
||||||
if goal.grasp_type is SGRCtrlGoal.GRASP_CLOSE:
|
|
||||||
self.sgr_tool.gripper_catch()
|
|
||||||
elif goal.grasp_type is SGRCtrlGoal.GRASP_OPEN:
|
|
||||||
self.sgr_tool.gripper_open()
|
|
||||||
|
|
||||||
# 开始规划
|
|
||||||
fb.step = fb.PLANNING
|
|
||||||
self._server.publish_feedback(fb)
|
|
||||||
if goal.action_type is goal.ACTION_TYPE_DEFINE_STAY:
|
|
||||||
# 指定动作
|
|
||||||
fb.step = fb.EXEC_POSITION
|
|
||||||
self._server.publish_feedback(fb)
|
|
||||||
self.sgr_tool.pose_stay()
|
|
||||||
else:
|
|
||||||
if goal.action_type in [SGRCtrlGoal.ACTION_TYPE_XYZ_RPY, SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY, SGRCtrlGoal.ACTION_TYPE_PUT_XYZ_RPY]:
|
|
||||||
# 指定末端姿态
|
|
||||||
roll = goal.pos_roll
|
|
||||||
pitch = goal.pos_pitch
|
|
||||||
yaw = goal.pos_yaw
|
|
||||||
else:
|
|
||||||
# 动态计算末端姿态
|
|
||||||
roll, pitch, yaw = self.sgr_tool.ee_xyz_get_rpy(goal.pos_x, goal.pos_y, goal.pos_z)
|
|
||||||
x, y, z, roll, pitch, yaw = self.sgr_tool.ee_target_offset(
|
|
||||||
goal.pos_x, goal.pos_y, goal.pos_z,
|
|
||||||
roll, pitch, yaw
|
|
||||||
)
|
|
||||||
|
|
||||||
# 检查姿态是否能到达
|
|
||||||
try:
|
|
||||||
if not self.sgr_tool.isPlanSuccess(x, y, z, roll, pitch, yaw):
|
|
||||||
raise
|
|
||||||
if goal.action_type in [
|
|
||||||
SGRCtrlGoal.ACTION_TYPE_PICK_XYZ,
|
|
||||||
SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY,
|
|
||||||
SGRCtrlGoal.ACTION_TYPE_PUT_XYZ,
|
|
||||||
SGRCtrlGoal.ACTION_TYPE_PUT_XYZ_RPY
|
|
||||||
]:
|
|
||||||
if not self.sgr_tool.isPlanSuccess(x, y, z + 0.04, roll, pitch, yaw):
|
|
||||||
raise
|
|
||||||
if not self.sgr_tool.isPlanSuccess(x, y, z + 0.12, roll, pitch, yaw):
|
|
||||||
raise
|
|
||||||
except:
|
|
||||||
rospy.logwarn("Plan could not found")
|
|
||||||
resp.result = resp.PLAN_NOT_FOUND
|
|
||||||
self._server.set_aborted(resp)
|
|
||||||
return
|
|
||||||
|
|
||||||
# 执行规划
|
|
||||||
fb.step = fb.EXEC_POSITION
|
|
||||||
self._server.publish_feedback(fb)
|
|
||||||
try:
|
|
||||||
# 抓取与放置动作
|
|
||||||
if goal.action_type in [
|
|
||||||
SGRCtrlGoal.ACTION_TYPE_PICK_XYZ,
|
|
||||||
SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY,
|
|
||||||
SGRCtrlGoal.ACTION_TYPE_PUT_XYZ,
|
|
||||||
SGRCtrlGoal.ACTION_TYPE_PUT_XYZ_RPY
|
|
||||||
]:
|
|
||||||
if self._server.is_preempt_requested():
|
|
||||||
raise CencelException("Preempt !!!")
|
|
||||||
self.sgr_tool.to_pose_eular(1, x, y, z + 0.04, roll, pitch, yaw)
|
|
||||||
|
|
||||||
if self._server.is_preempt_requested():
|
|
||||||
raise CencelException("Preempt !!!")
|
|
||||||
self.sgr_tool.to_pose_eular(0.2, x, y, z, roll, pitch, yaw)
|
|
||||||
|
|
||||||
fb.step = fb.EXEC_GRASP
|
|
||||||
self._server.publish_feedback(fb)
|
|
||||||
if self._server.is_preempt_requested():
|
|
||||||
raise CencelException("Preempt !!!")
|
|
||||||
if goal.action_type in [SGRCtrlGoal.ACTION_TYPE_PICK_XYZ, SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY]:
|
|
||||||
self.sgr_tool.gripper_catch()
|
|
||||||
elif goal.action_type in [SGRCtrlGoal.ACTION_TYPE_PUT_XYZ, SGRCtrlGoal.ACTION_TYPE_PUT_XYZ_RPY]:
|
|
||||||
self.sgr_tool.gripper_open()
|
|
||||||
|
|
||||||
fb.step = fb.EXEC_POSITION
|
|
||||||
self._server.publish_feedback(fb)
|
|
||||||
if self._server.is_preempt_requested():
|
|
||||||
raise CencelException("Preempt !!!")
|
|
||||||
self.sgr_tool.to_pose_eular(0.2, x, y, z + 0.12, roll, pitch, yaw)
|
|
||||||
|
|
||||||
# if self._server.is_preempt_requested():
|
|
||||||
# raise CencelException("Preempt !!!")
|
|
||||||
# self.sgr_tool.pose_stay()
|
|
||||||
|
|
||||||
# 夹爪判断
|
|
||||||
if goal.action_type in [SGRCtrlGoal.ACTION_TYPE_PICK_XYZ, SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY]:
|
|
||||||
ret = self.servo_info_srv.call(ServoRtInfoRequest(servo_id=7))
|
|
||||||
if abs(ret.payload) < 24:
|
|
||||||
resp.result = resp.GRASP_FAILD
|
|
||||||
self._server.set_aborted(resp)
|
|
||||||
return
|
|
||||||
|
|
||||||
# 执行某个动作
|
|
||||||
else:
|
|
||||||
if self._server.is_preempt_requested():
|
|
||||||
raise CencelException("Preempt !!!")
|
|
||||||
self.sgr_tool.to_pose_eular(0.2, x, y, z, roll, pitch, yaw)
|
|
||||||
except CencelException as e:
|
|
||||||
rospy.logwarn(e)
|
|
||||||
resp.result = resp.PREEMPT
|
|
||||||
self._server.set_aborted()
|
|
||||||
except Exception as e:
|
|
||||||
rospy.logerr(e)
|
|
||||||
resp.result = resp.ERROR
|
|
||||||
self._server.set_aborted()
|
|
||||||
|
|
||||||
resp.result = resp.SUCCESS
|
|
||||||
self._server.set_succeeded(resp)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
rospy.init_node("sgr_ctrl_action", anonymous=True)
|
|
||||||
rospy.sleep(5)
|
|
||||||
|
|
||||||
ser = SGRCtrlActionServer()
|
|
||||||
|
|
||||||
rospy.spin()
|
|
||||||
|
|
@ -1,78 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<package format="2">
|
|
||||||
<name>spark_sagittarius_carry</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>The spark_sagittarius_carry package</description>
|
|
||||||
|
|
||||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
|
||||||
<!-- Example: -->
|
|
||||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
|
||||||
<maintainer email="shudong.hong@nxrobo.com">shudong.hong</maintainer>
|
|
||||||
|
|
||||||
|
|
||||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
|
||||||
<!-- Commonly used license strings: -->
|
|
||||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
|
||||||
<license>TODO</license>
|
|
||||||
|
|
||||||
|
|
||||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
|
||||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
|
||||||
<!-- Example: -->
|
|
||||||
<!-- <url type="website">http://wiki.ros.org/spark_sagittarius_carry</url> -->
|
|
||||||
|
|
||||||
|
|
||||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
|
||||||
<!-- Authors do not have to be maintainers, but could be -->
|
|
||||||
<!-- Example: -->
|
|
||||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
|
||||||
|
|
||||||
|
|
||||||
<!-- The *depend tags are used to specify dependencies -->
|
|
||||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
|
||||||
<!-- Examples: -->
|
|
||||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
|
||||||
<!-- <depend>roscpp</depend> -->
|
|
||||||
<!-- Note that this is equivalent to the following: -->
|
|
||||||
<!-- <build_depend>roscpp</build_depend> -->
|
|
||||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
|
||||||
<!-- Use build_depend for packages you need at compile time: -->
|
|
||||||
<!-- <build_depend>message_generation</build_depend> -->
|
|
||||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
|
||||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
|
||||||
<!-- Use buildtool_depend for build tool packages: -->
|
|
||||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
|
||||||
<!-- Use exec_depend for packages you need at runtime: -->
|
|
||||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
|
||||||
<!-- Use test_depend for packages you need only for testing: -->
|
|
||||||
<!-- <test_depend>gtest</test_depend> -->
|
|
||||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
|
||||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
<build_depend>roscpp</build_depend>
|
|
||||||
<build_depend>rospy</build_depend>
|
|
||||||
<build_depend>actionlib</build_depend>
|
|
||||||
<build_depend>std_msgs</build_depend>
|
|
||||||
<build_depend>actionlib_msgs</build_depend>
|
|
||||||
<build_depend>message_generation</build_depend>
|
|
||||||
<build_export_depend>roscpp</build_export_depend>
|
|
||||||
<build_export_depend>rospy</build_export_depend>
|
|
||||||
<build_export_depend>actionlib</build_export_depend>
|
|
||||||
<build_export_depend>std_msgs</build_export_depend>
|
|
||||||
<build_export_depend>actionlib_msgs</build_export_depend>
|
|
||||||
<build_export_depend>message_generation</build_export_depend>
|
|
||||||
<exec_depend>roscpp</exec_depend>
|
|
||||||
<exec_depend>rospy</exec_depend>
|
|
||||||
<exec_depend>actionlib</exec_depend>
|
|
||||||
<exec_depend>std_msgs</exec_depend>
|
|
||||||
<exec_depend>actionlib_msgs</exec_depend>
|
|
||||||
<exec_depend>message_runtime</exec_depend>
|
|
||||||
<exec_depend>sdk_sagittarius_arm</exec_depend>
|
|
||||||
|
|
||||||
|
|
||||||
<!-- The export tag contains other, unspecified, tags -->
|
|
||||||
<export>
|
|
||||||
<!-- Other tools can request additional information be placed here -->
|
|
||||||
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
|
|
@ -1,365 +0,0 @@
|
||||||
Panels:
|
|
||||||
- Class: rviz/Displays
|
|
||||||
Help Height: 78
|
|
||||||
Name: Displays
|
|
||||||
Property Tree Widget:
|
|
||||||
Expanded: ~
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
Tree Height: 219
|
|
||||||
- Class: rviz/Selection
|
|
||||||
Name: Selection
|
|
||||||
- Class: rviz/Tool Properties
|
|
||||||
Expanded:
|
|
||||||
- /2D Pose Estimate1
|
|
||||||
- /2D Nav Goal1
|
|
||||||
- /Publish Point1
|
|
||||||
Name: Tool Properties
|
|
||||||
Splitter Ratio: 0.5886790156364441
|
|
||||||
- Class: rviz/Views
|
|
||||||
Expanded:
|
|
||||||
- /Current View1
|
|
||||||
Name: Views
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
- Class: rviz/Time
|
|
||||||
Experimental: false
|
|
||||||
Name: Time
|
|
||||||
SyncMode: 0
|
|
||||||
SyncSource: Camera
|
|
||||||
Preferences:
|
|
||||||
PromptSaveOnExit: true
|
|
||||||
Toolbars:
|
|
||||||
toolButtonStyle: 2
|
|
||||||
Visualization Manager:
|
|
||||||
Class: ""
|
|
||||||
Displays:
|
|
||||||
- Alpha: 0.5
|
|
||||||
Cell Size: 1
|
|
||||||
Class: rviz/Grid
|
|
||||||
Color: 160; 160; 164
|
|
||||||
Enabled: true
|
|
||||||
Line Style:
|
|
||||||
Line Width: 0.029999999329447746
|
|
||||||
Value: Lines
|
|
||||||
Name: Grid
|
|
||||||
Normal Cell Count: 0
|
|
||||||
Offset:
|
|
||||||
X: 0
|
|
||||||
Y: 0
|
|
||||||
Z: 0
|
|
||||||
Plane: XY
|
|
||||||
Plane Cell Count: 10
|
|
||||||
Reference Frame: <Fixed Frame>
|
|
||||||
Value: true
|
|
||||||
- Alpha: 1
|
|
||||||
Class: rviz/RobotModel
|
|
||||||
Collision Enabled: false
|
|
||||||
Enabled: true
|
|
||||||
Links:
|
|
||||||
All Links Enabled: true
|
|
||||||
Expand Joint Details: false
|
|
||||||
Expand Link Details: false
|
|
||||||
Expand Tree: false
|
|
||||||
IMU_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Link Tree Style: Links in Alphabetic Order
|
|
||||||
base_footprint:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
base_gyro_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
base_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
camera_bottom_screw_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
camera_color_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
camera_color_optical_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
camera_depth_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
camera_depth_optical_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
camera_left_ir_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
camera_left_ir_optical_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
camera_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
camera_right_ir_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
camera_right_ir_optical_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
cliff_back_left_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
cliff_back_right_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
cliff_front_left_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
cliff_front_right_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
cliff_left_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
cliff_right_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
ir_bumper_back_left_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
ir_bumper_back_right_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
ir_bumper_front_left_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
ir_bumper_front_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
ir_bumper_front_right_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
ir_bumper_left_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
ir_bumper_rear_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
ir_bumper_right_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
left_wheel_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
lidar_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
right_wheel_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
room_base_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
spark_stack:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: robot_description
|
|
||||||
TF Prefix: ""
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
Visual Enabled: true
|
|
||||||
- Alpha: 1
|
|
||||||
Class: rviz/RobotModel
|
|
||||||
Collision Enabled: false
|
|
||||||
Enabled: true
|
|
||||||
Links:
|
|
||||||
All Links Enabled: true
|
|
||||||
Expand Joint Details: false
|
|
||||||
Expand Link Details: false
|
|
||||||
Expand Tree: false
|
|
||||||
Link Tree Style: Links in Alphabetic Order
|
|
||||||
base_footprint:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
sgr532/ar_tag_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
sgr532/base_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link1:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link2:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link3:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link4:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link5:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link6:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link_grasping_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
sgr532/link_gripper_left:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link_gripper_right:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/usb_cam_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: sgr532/robot_description
|
|
||||||
TF Prefix: ""
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
Visual Enabled: true
|
|
||||||
- Class: rviz/Camera
|
|
||||||
Enabled: true
|
|
||||||
Image Rendering: background and overlay
|
|
||||||
Image Topic: /camera/color/image_raw
|
|
||||||
Name: Camera
|
|
||||||
Overlay Alpha: 0.5
|
|
||||||
Queue Size: 2
|
|
||||||
Transport Hint: raw
|
|
||||||
Unreliable: false
|
|
||||||
Value: true
|
|
||||||
Visibility:
|
|
||||||
Grid: true
|
|
||||||
RobotModel: true
|
|
||||||
Value: false
|
|
||||||
Zoom Factor: 1
|
|
||||||
Enabled: true
|
|
||||||
Global Options:
|
|
||||||
Background Color: 48; 48; 48
|
|
||||||
Default Light: true
|
|
||||||
Fixed Frame: base_link
|
|
||||||
Frame Rate: 30
|
|
||||||
Name: root
|
|
||||||
Tools:
|
|
||||||
- Class: rviz/Interact
|
|
||||||
Hide Inactive Objects: true
|
|
||||||
- Class: rviz/MoveCamera
|
|
||||||
- Class: rviz/Select
|
|
||||||
- Class: rviz/FocusCamera
|
|
||||||
- Class: rviz/Measure
|
|
||||||
- Class: rviz/SetInitialPose
|
|
||||||
Theta std deviation: 0.2617993950843811
|
|
||||||
Topic: /initialpose
|
|
||||||
X std deviation: 0.5
|
|
||||||
Y std deviation: 0.5
|
|
||||||
- Class: rviz/SetGoal
|
|
||||||
Topic: /move_base_simple/goal
|
|
||||||
- Class: rviz/PublishPoint
|
|
||||||
Single click: true
|
|
||||||
Topic: /clicked_point
|
|
||||||
Value: true
|
|
||||||
Views:
|
|
||||||
Current:
|
|
||||||
Class: rviz/Orbit
|
|
||||||
Distance: 2.036864995956421
|
|
||||||
Enable Stereo Rendering:
|
|
||||||
Stereo Eye Separation: 0.05999999865889549
|
|
||||||
Stereo Focal Distance: 1
|
|
||||||
Swap Stereo Eyes: false
|
|
||||||
Value: false
|
|
||||||
Field of View: 0.7853981852531433
|
|
||||||
Focal Point:
|
|
||||||
X: 0.08599551767110825
|
|
||||||
Y: -0.17910736799240112
|
|
||||||
Z: 0.06584001332521439
|
|
||||||
Focal Shape Fixed Size: true
|
|
||||||
Focal Shape Size: 0.05000000074505806
|
|
||||||
Invert Z Axis: false
|
|
||||||
Name: Current View
|
|
||||||
Near Clip Distance: 0.009999999776482582
|
|
||||||
Pitch: 0.785398006439209
|
|
||||||
Target Frame: <Fixed Frame>
|
|
||||||
Yaw: 0.785398006439209
|
|
||||||
Saved: ~
|
|
||||||
Window Geometry:
|
|
||||||
Camera:
|
|
||||||
collapsed: false
|
|
||||||
Displays:
|
|
||||||
collapsed: false
|
|
||||||
Height: 696
|
|
||||||
Hide Left Dock: false
|
|
||||||
Hide Right Dock: true
|
|
||||||
QMainWindow State: 000000ff00000000fd0000000400000000000001560000021afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000166000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001a9000000ae0000001600ffffff000000010000010f000001cbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001cb000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003970000021a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
|
||||||
Selection:
|
|
||||||
collapsed: false
|
|
||||||
Time:
|
|
||||||
collapsed: false
|
|
||||||
Tool Properties:
|
|
||||||
collapsed: false
|
|
||||||
Views:
|
|
||||||
collapsed: true
|
|
||||||
Width: 1267
|
|
||||||
X: 256
|
|
||||||
Y: 96
|
|
||||||
|
|
@ -1,7 +0,0 @@
|
||||||
#!/bin/bash
|
|
||||||
|
|
||||||
Info="${Green_font_prefix}[信息]${Font_color_suffix}"
|
|
||||||
echo -e "${Info} 是否现在开始视觉抓取？"
|
|
||||||
echo -e "${Info} 开始请按按任意键，否则退出请输入：Ctrl + c "
|
|
||||||
echo && stty erase '^H' && read -p "按任意键开始："
|
|
||||||
gnome-terminal -x bash -c "rosservice call /s_carry_object 'type: 1'"
|
|
||||||
|
|
@ -1,6 +0,0 @@
|
||||||
#!/bin/bash
|
|
||||||
|
|
||||||
BASEPATH=$(cd `dirname $0`; pwd)
|
|
||||||
gnome-terminal -x bash -c "$BASEPATH/action_carry.sh"
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,6 +0,0 @@
|
||||||
#!/bin/bash
|
|
||||||
|
|
||||||
BASEPATH=$(cd `dirname $0`; pwd)
|
|
||||||
gnome-terminal -x bash -c "$BASEPATH/send_topic.sh"
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,6 +0,0 @@
|
||||||
#!/bin/bash
|
|
||||||
|
|
||||||
BASEPATH=$(cd `dirname $0`; pwd)
|
|
||||||
gnome-terminal -x bash -c "$BASEPATH/send_topic_auto.sh"
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,98 +0,0 @@
|
||||||
#!/bin/bash
|
|
||||||
|
|
||||||
Green_font_prefix="\033[32m" && Red_font_prefix="\033[31m" && Green_background_prefix="\033[42;37m" && Red_background_prefix="\033[41;37m" && Yellow_background_prefix="\033[43;37m" && Font_color_suffix="\033[0m" && Yellow_font_prefix="\e[1;33m" && Blue_font_prefix="\e[0;34m"
|
|
||||||
Info="${Green_font_prefix}[信息]${Font_color_suffix}"
|
|
||||||
echo -e "${Info} 流程如下："
|
|
||||||
echo -e "${Info} 1. 获取方块的 hsv"
|
|
||||||
echo -e "${Info} 1). 机械臂会运行到指定位置，请把方块放置到机械臂夹爪正下方"
|
|
||||||
echo -e "${Info} 2). 调整摄像头位置，让绿色框对准蓝色方块顶面"
|
|
||||||
echo -e "${Info} 3). 获取绿色框内 HSV 均值"
|
|
||||||
echo -e "${Info} 2. 使用九点标定法标定机械臂"
|
|
||||||
echo -e "${Info} 1). 机械臂会运行到指定位置，请把方块放置到机械臂夹爪正下方"
|
|
||||||
echo -e "${Info} 2). 程序根据 HSV 获取并保存方块在摄像头上的位置"
|
|
||||||
echo -e "${Info} 3). 运行下一个标定点，直到九个标定点都完成"
|
|
||||||
echo -e "${Info} 3. 计算线性回归后的值，并保存到文件"
|
|
||||||
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} 一切准备好？\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'start'" -1 >/dev/null
|
|
||||||
rostopic pub /cali_cmd_topic std_msgs/String "data: 'start'" -1 >/dev/null
|
|
||||||
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} 请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
rostopic pub /cali_cmd_topic std_msgs/String "data: 'start2'" -1 >/dev/null
|
|
||||||
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} 请调整摄像头，把绿色框对准了蓝色方块顶面位置\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_cmd_topic std_msgs/String "data: 'start3'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 5
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} (1/9) No.01"
|
|
||||||
echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 3
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} (2/9) No.02"
|
|
||||||
echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 3
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} (3/9) No.03"
|
|
||||||
echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 3
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} (4/9) No.04"
|
|
||||||
echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 3
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} (5/9) No.05"
|
|
||||||
echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 3
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} (6/9) No.06"
|
|
||||||
echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 3
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} (7/9) No.07"
|
|
||||||
echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 3
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} (8/9) No.08"
|
|
||||||
echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 3
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} (9/9) No.09"
|
|
||||||
echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 5
|
|
||||||
echo && stty erase '^H' && read -p "标定结束，按任意键退出..."
|
|
||||||
exit
|
|
||||||
|
|
@ -1,36 +0,0 @@
|
||||||
#!/bin/bash
|
|
||||||
|
|
||||||
Green_font_prefix="\033[32m" && Red_font_prefix="\033[31m" && Green_background_prefix="\033[42;37m" && Red_background_prefix="\033[41;37m" && Yellow_background_prefix="\033[43;37m" && Font_color_suffix="\033[0m" && Yellow_font_prefix="\e[1;33m" && Blue_font_prefix="\e[0;34m"
|
|
||||||
Info="${Green_font_prefix}[信息]${Font_color_suffix}"
|
|
||||||
echo -e "${Info} 流程如下："
|
|
||||||
echo -e "${Info} 1. 获取方块的 hsv"
|
|
||||||
echo -e "${Info} 1). 机械臂会运行到指定位置, 请把方块放置到机械臂夹爪正下方"
|
|
||||||
echo -e "${Info} 2). 调整摄像头位置, 让绿色框对准蓝色方块顶面"
|
|
||||||
echo -e "${Info} 3). 程序获取绿色框内 HSV 均值"
|
|
||||||
echo -e "${Info} 2. 机械臂开始进行标定, 过程自动"
|
|
||||||
echo -e "${Info} 3. 计算线性回归后的值, 并保存到文件"
|
|
||||||
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} 一切准备好？\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c, 完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'start'" -1 >/dev/null
|
|
||||||
rostopic pub /cali_cmd_topic std_msgs/String "data: 'start'" -1 >/dev/null
|
|
||||||
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} 请把蓝色方块放到机械臂夹爪的正下方\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c, 完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null
|
|
||||||
rostopic pub /cali_cmd_topic std_msgs/String "data: 'start2'" -1 >/dev/null
|
|
||||||
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} 请调整摄像头, 把绿色框对准了蓝色方块顶面位置\c"
|
|
||||||
echo && stty erase '^H' && read -p "退出请输入 Ctrl + c, 完成后按回车键继续..."
|
|
||||||
rostopic pub /cali_cmd_topic std_msgs/String "data: 'start3'" -1 >/dev/null
|
|
||||||
|
|
||||||
sleep 5
|
|
||||||
echo -e ""
|
|
||||||
echo -e "${Info} 机械臂开始自动标定"
|
|
||||||
echo -e "${Info} 当机械臂停止运动, 而且原来窗口显示完成标定的提示"
|
|
||||||
echo -e "${Info} 说明标定完成, 可在原来的窗口按 Ctrl-C 退出程序"
|
|
||||||
echo && stty erase '^H' && read -p "当前窗口可退出, 按回车键退出当前窗口..."
|
|
||||||
exit
|
|
||||||
|
|
@ -1,14 +0,0 @@
|
||||||
uint8 END = 0
|
|
||||||
uint8 RUN = 1
|
|
||||||
uint8 PEND = 2
|
|
||||||
uint8 SEARCH = 3
|
|
||||||
|
|
||||||
uint8 type
|
|
||||||
string param
|
|
||||||
---
|
|
||||||
uint8 SUCCESS = 0
|
|
||||||
uint8 RUNNING = 1
|
|
||||||
uint8 PENDING = 2
|
|
||||||
uint8 FAILURE = 3
|
|
||||||
|
|
||||||
uint8 status
|
|
||||||
|
|
@ -1,55 +0,0 @@
|
||||||
|
|
||||||
#检查系统要求
|
|
||||||
check_sys(){
|
|
||||||
ubuntu_version=$(lsb_release -r --short)
|
|
||||||
if [[ "${ubuntu_version}" == "16.04" ]]; then
|
|
||||||
ROS_Ver="kinetic"
|
|
||||||
elif [[ "${ubuntu_version}" == "18.04" ]]; then
|
|
||||||
ROS_Ver="melodic"
|
|
||||||
elif [[ "${ubuntu_version}" == "20.04" ]]; then
|
|
||||||
ROS_Ver="noetic"
|
|
||||||
else
|
|
||||||
echo -e "${Error} SPARK暂不支持当前系统 ${OSDescription} !" && exit 1
|
|
||||||
fi
|
|
||||||
}
|
|
||||||
BASEPATH=$(cd `dirname $0`; pwd)
|
|
||||||
echo "sudo cp $BASEPATH/sdk_sagittarius_arm/rules/sagittarius-usb-serial.rules /etc/udev/rules.d/"
|
|
||||||
sudo cp $BASEPATH/sdk_sagittarius_arm/rules/sagittarius-usb-serial.rules /etc/udev/rules.d/
|
|
||||||
sudo udevadm trigger
|
|
||||||
check_sys
|
|
||||||
sudo apt -y install ros-${ROS_Ver}-dynamixel-workbench-toolbox ros-${ROS_Ver}-moveit-msgs ros-${ROS_Ver}-moveit-ros ros-${ROS_Ver}-ompl ros-${ROS_Ver}-moveit-planners-ompl ros-${ROS_Ver}-moveit-simple-controller-manager ros-${ROS_Ver}-joint-state-publisher-gui ros-${ROS_Ver}-moveit-commander ros-${ROS_Ver}-trac-ik-kinematics-plugin ros-${ROS_Ver}-moveit-setup-assistant ros-${ROS_Ver}-moveit-fake-controller-manager ros-${ROS_Ver}-moveit-visual-tools ros-${ROS_Ver}-joy ros-${ROS_Ver}-joint-trajectory-controller ros-${ROS_Ver}-joint-state-controller ros-${ROS_Ver}-gazebo-ros-control ros-${ROS_Ver}-effort-controllers ros-${ROS_Ver}-chomp-motion-planner ros-${ROS_Ver}-moveit-planners-chomp ros-${ROS_Ver}-pilz-industrial-motion-planner
|
|
||||||
|
|
||||||
if [ $(dpkg-query -W -f='${Status}' librealsense2 2>/dev/null | grep -c "ok installed") -eq 0 ]; then
|
|
||||||
echo "Installing librealsense2..."
|
|
||||||
sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
|
|
||||||
sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -sc) main" -u
|
|
||||||
if [ $ubuntu_version == "16.04" ]; then
|
|
||||||
version="2.48.0-0~realsense0.4974"
|
|
||||||
elif [ $ubuntu_version == "18.04" ]; then
|
|
||||||
version="2.48.0-0~realsense0.4975"
|
|
||||||
elif [ $ubuntu_version == "20.04" ]; then
|
|
||||||
version="2.48.0-0~realsense0.4976"
|
|
||||||
fi
|
|
||||||
|
|
||||||
sudo apt -y install librealsense2-udev-rules=${version}
|
|
||||||
sudo apt -y install librealsense2-dkms
|
|
||||||
sudo apt -y install librealsense2=${version}
|
|
||||||
sudo apt -y install librealsense2-gl=${version}
|
|
||||||
sudo apt -y install librealsense2-gl-dev=${version}
|
|
||||||
sudo apt -y install librealsense2-gl-dbg=${version}
|
|
||||||
sudo apt -y install librealsense2-net=${version}
|
|
||||||
sudo apt -y install librealsense2-net-dev=${version}
|
|
||||||
sudo apt -y install librealsense2-net-dbg=${version}
|
|
||||||
sudo apt -y install librealsense2-utils=${version}
|
|
||||||
sudo apt -y install librealsense2-dev=${version}
|
|
||||||
sudo apt -y install librealsense2-dbg=${version}
|
|
||||||
sudo apt-mark hold librealsense2*
|
|
||||||
sudo apt -y install ros-${ROS_Ver}-ddynamic-reconfigure
|
|
||||||
else
|
|
||||||
echo "librealsense2 already installed!"
|
|
||||||
fi
|
|
||||||
|
|
||||||
echo "sudo cp $BASEPATH/sdk_sagittarius_arm/rules/usb_cam.rules /etc/udev/rules.d/"
|
|
||||||
sudo cp $BASEPATH/sdk_sagittarius_arm/rules/usb_cam.rules /etc/udev/rules.d/
|
|
||||||
sudo udevadm trigger
|
|
||||||
check_sys
|
|
||||||
|
|
@ -1,216 +0,0 @@
|
||||||
cmake_minimum_required(VERSION 3.0.2)
|
|
||||||
project(sagittarius_joy)
|
|
||||||
|
|
||||||
## Compile as C++11, supported in ROS Kinetic and newer
|
|
||||||
add_compile_options(-std=c++11)
|
|
||||||
|
|
||||||
## 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
|
|
||||||
rospy
|
|
||||||
joy
|
|
||||||
sdk_sagittarius_arm
|
|
||||||
sensor_msgs
|
|
||||||
message_generation
|
|
||||||
std_msgs
|
|
||||||
)
|
|
||||||
|
|
||||||
## System dependencies are found with CMake's conventions
|
|
||||||
# find_package(Boost REQUIRED COMPONENTS system)
|
|
||||||
|
|
||||||
|
|
||||||
## Uncomment this if the package has a setup.py. This macro ensures
|
|
||||||
## modules and global scripts declared therein get installed
|
|
||||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
|
||||||
# catkin_python_setup()
|
|
||||||
|
|
||||||
################################################
|
|
||||||
## Declare ROS messages, services and actions ##
|
|
||||||
################################################
|
|
||||||
|
|
||||||
## To declare and build messages, services or actions from within this
|
|
||||||
## package, follow these steps:
|
|
||||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
|
||||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
|
||||||
## * In the file package.xml:
|
|
||||||
## * add a build_depend tag for "message_generation"
|
|
||||||
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
|
|
||||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
|
||||||
## but can be declared for certainty nonetheless:
|
|
||||||
## * add a exec_depend tag for "message_runtime"
|
|
||||||
## * In this file (CMakeLists.txt):
|
|
||||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
|
||||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
|
||||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
|
||||||
## catkin_package(CATKIN_DEPENDS ...)
|
|
||||||
## * uncomment the add_*_files sections below as needed
|
|
||||||
## and list every .msg/.srv/.action file to be processed
|
|
||||||
## * uncomment the generate_messages entry below
|
|
||||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
|
||||||
|
|
||||||
## Generate messages in the 'msg' folder
|
|
||||||
add_message_files(
|
|
||||||
FILES
|
|
||||||
arm_joy.msg
|
|
||||||
)
|
|
||||||
|
|
||||||
## Generate services in the 'srv' folder
|
|
||||||
# add_service_files(
|
|
||||||
# FILES
|
|
||||||
# Service1.srv
|
|
||||||
# Service2.srv
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Generate actions in the 'action' folder
|
|
||||||
# add_action_files(
|
|
||||||
# FILES
|
|
||||||
# Action1.action
|
|
||||||
# Action2.action
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Generate added messages and services with any dependencies listed here
|
|
||||||
generate_messages(
|
|
||||||
DEPENDENCIES
|
|
||||||
geometry_msgs
|
|
||||||
sensor_msgs
|
|
||||||
std_msgs
|
|
||||||
)
|
|
||||||
|
|
||||||
################################################
|
|
||||||
## Declare ROS dynamic reconfigure parameters ##
|
|
||||||
################################################
|
|
||||||
|
|
||||||
## To declare and build dynamic reconfigure parameters within this
|
|
||||||
## package, follow these steps:
|
|
||||||
## * In the file package.xml:
|
|
||||||
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
|
|
||||||
## * In this file (CMakeLists.txt):
|
|
||||||
## * add "dynamic_reconfigure" to
|
|
||||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
|
||||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
|
||||||
## and list every .cfg file to be processed
|
|
||||||
|
|
||||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
|
||||||
# generate_dynamic_reconfigure_options(
|
|
||||||
# cfg/DynReconf1.cfg
|
|
||||||
# cfg/DynReconf2.cfg
|
|
||||||
# )
|
|
||||||
|
|
||||||
###################################
|
|
||||||
## catkin specific configuration ##
|
|
||||||
###################################
|
|
||||||
## The catkin_package macro generates cmake config files for your package
|
|
||||||
## Declare things to be passed to dependent projects
|
|
||||||
## INCLUDE_DIRS: uncomment this if your package contains header files
|
|
||||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
|
||||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
|
||||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
|
||||||
catkin_package(
|
|
||||||
INCLUDE_DIRS
|
|
||||||
LIBRARIES sagittarius_joy
|
|
||||||
CATKIN_DEPENDS roscpp rospy joy sdk_sagittarius_arm sensor_msgs
|
|
||||||
# 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}
|
|
||||||
)
|
|
||||||
|
|
||||||
## Declare a C++ library
|
|
||||||
# add_library(${PROJECT_NAME}
|
|
||||||
# src/${PROJECT_NAME}/sagittarius_joy.cpp
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Add cmake target dependencies of the library
|
|
||||||
## as an example, code may need to be generated before libraries
|
|
||||||
## either from message generation or dynamic reconfigure
|
|
||||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
|
||||||
|
|
||||||
## Declare a C++ executable
|
|
||||||
## With catkin_make all packages are built within a single CMake context
|
|
||||||
## The recommended prefix ensures that target names across packages don't collide
|
|
||||||
# add_executable(${PROJECT_NAME}_node src/sagittarius_joy_node.cpp)
|
|
||||||
|
|
||||||
|
|
||||||
## Rename C++ executable without prefix
|
|
||||||
## The above recommended prefix causes long target names, the following renames the
|
|
||||||
## target back to the shorter version for ease of user use
|
|
||||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
|
||||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
|
||||||
|
|
||||||
## Add cmake target dependencies of the executable
|
|
||||||
## same as for the library above
|
|
||||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
|
||||||
|
|
||||||
## Specify libraries to link a library or executable target against
|
|
||||||
# target_link_libraries(${PROJECT_NAME}_node
|
|
||||||
# ${catkin_LIBRARIES}
|
|
||||||
# )
|
|
||||||
|
|
||||||
add_executable(sagittarius_joy src/sagittarius_joy.cpp)
|
|
||||||
target_link_libraries(sagittarius_joy ${catkin_LIBRARIES})
|
|
||||||
|
|
||||||
|
|
||||||
#############
|
|
||||||
## Install ##
|
|
||||||
#############
|
|
||||||
|
|
||||||
# all install targets should use catkin DESTINATION variables
|
|
||||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
|
||||||
|
|
||||||
## Mark executable scripts (Python etc.) for installation
|
|
||||||
## in contrast to setup.py, you can choose the destination
|
|
||||||
# catkin_install_python(PROGRAMS
|
|
||||||
# scripts/my_python_script
|
|
||||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Mark executables for installation
|
|
||||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
|
|
||||||
# install(TARGETS ${PROJECT_NAME}_node
|
|
||||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
|
||||||
# )
|
|
||||||
|
|
||||||
## 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}
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Mark cpp header files for installation
|
|
||||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
|
||||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
|
||||||
# FILES_MATCHING PATTERN "*.h"
|
|
||||||
# PATTERN ".svn" EXCLUDE
|
|
||||||
# )
|
|
||||||
|
|
||||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
|
||||||
# install(FILES
|
|
||||||
# # myfile1
|
|
||||||
# # myfile2
|
|
||||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
|
||||||
# )
|
|
||||||
|
|
||||||
#############
|
|
||||||
## Testing ##
|
|
||||||
#############
|
|
||||||
|
|
||||||
## Add gtest based cpp test target and link libraries
|
|
||||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_sagittarius_joy.cpp)
|
|
||||||
# if(TARGET ${PROJECT_NAME}-test)
|
|
||||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
|
||||||
# endif()
|
|
||||||
|
|
||||||
## Add folders to be run by python nosetests
|
|
||||||
# catkin_add_nosetests(test)
|
|
||||||
|
|
@ -1,37 +0,0 @@
|
||||||
使用方式
|
|
||||||
|
|
||||||
连接 xbox 或 ps4, 新终端输入下面指令
|
|
||||||
|
|
||||||
```
|
|
||||||
roslaunch sagittarius_joy joy_demo.launch
|
|
||||||
```
|
|
||||||
`controller_type` 是手柄类型，可选为[xbox360, ps4]，默认使用 xbox 手柄
|
|
||||||
|
|
||||||
然后可以使用手柄控制机械臂
|
|
||||||
|
|
||||||
控制方式
|
|
||||||
|
|
||||||
左摇杆前/后: end-effect x 加/减
|
|
||||||
左摇杆左/右: 腰部旋转 左摆/右摆
|
|
||||||
左摇杆摁下: 无
|
|
||||||
|
|
||||||
右摇杆前/后: end-effect pitch 往下看/往上看
|
|
||||||
右摇杆左/右: end-effect y 加/减
|
|
||||||
右摇杆摁下: 特殊设置 摆正夹爪
|
|
||||||
|
|
||||||
LB(L1): 高度减少
|
|
||||||
RB(R1): 高度增加
|
|
||||||
|
|
||||||
BACK(SHARE): 逆时针翻滚
|
|
||||||
START(OPTION): 顺时针翻滚
|
|
||||||
|
|
||||||
LT(L2): 夹爪减少间距
|
|
||||||
RT(R2): 夹爪增加间距
|
|
||||||
|
|
||||||
方向键上/下: 速度增加/减少
|
|
||||||
方向键上/下: 无
|
|
||||||
|
|
||||||
B(cycle): 预设姿态 home
|
|
||||||
A(cross): 预设姿态 sleep
|
|
||||||
|
|
||||||
长按 power 1秒: 扭矩输出切换
|
|
||||||
|
|
@ -1,18 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<launch>
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="controller" default="xbox360" />
|
|
||||||
<arg name="topic_joy_raw" default="/commands/joy_raw"/>
|
|
||||||
<arg name="threshold" default="0.75"/>
|
|
||||||
|
|
||||||
<remap from="commands/joy_raw" to="$(arg topic_joy_raw)"/>
|
|
||||||
|
|
||||||
<node pkg="sagittarius_joy" type="sagittarius_joy" name="sagittarius_joy" output="screen" ns="$(arg robot_name)">
|
|
||||||
<param name="controller" value="$(arg controller)"/>
|
|
||||||
<param name="threshold" value="$(arg threshold)"/>
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<node pkg="sagittarius_joy" type="sagittarius_arm" name="sagittarius_arm" output="screen" ns="$(arg robot_name)">
|
|
||||||
</node>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,42 +0,0 @@
|
||||||
<launch>
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default="$(arg robot_name)"/>
|
|
||||||
<arg name="controller_dev" default="/dev/input/js0"/>
|
|
||||||
<arg name="controller_type" default="xbox360"/>
|
|
||||||
<arg name="topic_joy_raw" default="/commands/joy_raw"/>
|
|
||||||
<arg name="threshold" default="0.75"/>
|
|
||||||
|
|
||||||
<include file="$(find sdk_sagittarius_arm)/launch/rviz_control_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name)" />
|
|
||||||
<arg name="robot_model" value="$(arg robot_model)" />
|
|
||||||
<arg name="arm_velocity" value="2048" />
|
|
||||||
<arg name="arm_acceleration" value="10" />
|
|
||||||
<arg name="servo_torque1" default="600"/>
|
|
||||||
<arg name="servo_torque2" default="600"/>
|
|
||||||
<arg name="servo_torque3" default="600"/>
|
|
||||||
<arg name="servo_torque4" default="600"/>
|
|
||||||
<arg name="servo_torque5" default="600"/>
|
|
||||||
<arg name="servo_torque6" default="600"/>
|
|
||||||
<arg name="servo_torque7" default="200"/>
|
|
||||||
<arg name="joint_pub_gui" value="false" />
|
|
||||||
<arg name="use_joint_pub" value="true"/>
|
|
||||||
<arg name="just_rviz_control" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<node pkg="joy" type="joy_node" name="joy_node" output="screen">
|
|
||||||
<param name="dev" value="$(arg controller_dev)" />
|
|
||||||
<remap from="joy" to="commands/joy_raw"/>
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<include file="$(find sagittarius_joy)/launch/joy.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name)" />
|
|
||||||
<arg name="controller" value="$(arg controller_type)" />
|
|
||||||
<arg name="topic_joy_raw" value="$(arg topic_joy_raw)" />
|
|
||||||
<param name="threshold" value="$(arg threshold)"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,85 +0,0 @@
|
||||||
|
|
||||||
# This message is used specifically in the sagittarius_joy package
|
|
||||||
#
|
|
||||||
# Maps raw 'joy' commands to more specific ones to control an sagittarius
|
|
||||||
|
|
||||||
# enum values that define the joystick controls for the robot
|
|
||||||
|
|
||||||
#########################################################################################################
|
|
||||||
|
|
||||||
# Control the motion of the virtual 'ee_gripper_link' or end effector using the modern_robotics_ik engine
|
|
||||||
# Position Control
|
|
||||||
int8 EE_X_INC = 1
|
|
||||||
int8 EE_X_DEC = 2
|
|
||||||
int8 EE_Y_INC = 3
|
|
||||||
int8 EE_Y_DEC = 4
|
|
||||||
int8 EE_Z_INC = 5
|
|
||||||
int8 EE_Z_DEC = 6
|
|
||||||
|
|
||||||
# Orientation Control
|
|
||||||
int8 EE_ROLL_CCW = 7
|
|
||||||
int8 EE_ROLL_CW = 8
|
|
||||||
int8 EE_PITCH_UP = 9
|
|
||||||
int8 EE_PITCH_DOWN = 10
|
|
||||||
int8 EE_YAW_LEFT = 11
|
|
||||||
int8 EE_YAW_RIGHT = 12
|
|
||||||
|
|
||||||
#########################################################################################################
|
|
||||||
|
|
||||||
# Control the motion of independent joints on the Arm or send predefined robot poses
|
|
||||||
# Pose Control
|
|
||||||
int8 UP_POSE = 13
|
|
||||||
int8 HOME_POSE = 14
|
|
||||||
int8 SLEEP_POSE = 15
|
|
||||||
|
|
||||||
# Position and Orientation reset
|
|
||||||
int8 POSITION_RESET = 16
|
|
||||||
int8 ORIENTATION_RESET = 17
|
|
||||||
|
|
||||||
#########################################################################################################
|
|
||||||
|
|
||||||
# Customize configurations for the Sagittarius Arm
|
|
||||||
# Inc/Dec Joint speed
|
|
||||||
int8 SPEED_INC = 18
|
|
||||||
int8 SPEED_DEC = 19
|
|
||||||
|
|
||||||
# Inc/Dec Gripper spacing
|
|
||||||
int8 GRIPPER_SPACING_INC = 20
|
|
||||||
int8 GRIPPER_SPACING_DEC = 21
|
|
||||||
|
|
||||||
# Torque On/Off all servos
|
|
||||||
int8 TORQUE_ON = 22
|
|
||||||
int8 TORQUE_OFF = 23
|
|
||||||
|
|
||||||
# Torque Inc/Dec all servos
|
|
||||||
int8 TORQUE_INC = 24
|
|
||||||
int8 TORQUE_DEC = 25
|
|
||||||
|
|
||||||
#########################################################################################################
|
|
||||||
|
|
||||||
# Other function for the Sagittarius Arm
|
|
||||||
# Super Key
|
|
||||||
int8 SUPER_KEY = 26
|
|
||||||
|
|
||||||
#########################################################################################################
|
|
||||||
|
|
||||||
# Control the motion of the Sagittarius Arm
|
|
||||||
int8 ee_x_cmd
|
|
||||||
int8 ee_y_cmd
|
|
||||||
int8 ee_z_cmd
|
|
||||||
int8 ee_roll_cmd
|
|
||||||
int8 ee_pitch_cmd
|
|
||||||
int8 ee_yaw_cmd
|
|
||||||
|
|
||||||
# Independent Joint/Pose Control
|
|
||||||
int8 pose_cmd
|
|
||||||
int8 reset_cmd
|
|
||||||
|
|
||||||
# Misc. Configs
|
|
||||||
int8 speed_cmd
|
|
||||||
int8 gripper_spacing_cmd
|
|
||||||
int8 torque_cmd
|
|
||||||
int8 torque_lev_cmd
|
|
||||||
|
|
||||||
# Super key
|
|
||||||
int8 super_cmd
|
|
||||||
|
|
@ -1,79 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<package format="2">
|
|
||||||
<name>sagittarius_joy</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>The sagittarius_joy package</description>
|
|
||||||
|
|
||||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
|
||||||
<!-- Example: -->
|
|
||||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
|
||||||
<maintainer email="spark@todo.todo">spark</maintainer>
|
|
||||||
|
|
||||||
|
|
||||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
|
||||||
<!-- Commonly used license strings: -->
|
|
||||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
|
||||||
<license>TODO</license>
|
|
||||||
|
|
||||||
|
|
||||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
|
||||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
|
||||||
<!-- Example: -->
|
|
||||||
<!-- <url type="website">http://wiki.ros.org/sagittarius_joy</url> -->
|
|
||||||
|
|
||||||
|
|
||||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
|
||||||
<!-- Authors do not have to be maintainers, but could be -->
|
|
||||||
<!-- Example: -->
|
|
||||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
|
||||||
|
|
||||||
|
|
||||||
<!-- The *depend tags are used to specify dependencies -->
|
|
||||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
|
||||||
<!-- Examples: -->
|
|
||||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
|
||||||
<!-- <depend>roscpp</depend> -->
|
|
||||||
<!-- Note that this is equivalent to the following: -->
|
|
||||||
<!-- <build_depend>roscpp</build_depend> -->
|
|
||||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
|
||||||
<!-- Use build_depend for packages you need at compile time: -->
|
|
||||||
<!-- <build_depend>message_generation</build_depend> -->
|
|
||||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
|
||||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
|
||||||
<!-- Use buildtool_depend for build tool packages: -->
|
|
||||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
|
||||||
<!-- Use exec_depend for packages you need at runtime: -->
|
|
||||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
|
||||||
<!-- Use test_depend for packages you need only for testing: -->
|
|
||||||
<!-- <test_depend>gtest</test_depend> -->
|
|
||||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
|
||||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
<build_depend>joy</build_depend>
|
|
||||||
<build_depend>roscpp</build_depend>
|
|
||||||
<build_depend>rospy</build_depend>
|
|
||||||
<build_depend>sdk_sagittarius_arm</build_depend>
|
|
||||||
<build_depend>sensor_msgs</build_depend>
|
|
||||||
<build_depend>std_msgs</build_depend>
|
|
||||||
<build_depend>message_generation</build_depend>
|
|
||||||
<build_export_depend>joy</build_export_depend>
|
|
||||||
<build_export_depend>roscpp</build_export_depend>
|
|
||||||
<build_export_depend>rospy</build_export_depend>
|
|
||||||
<build_export_depend>sdk_sagittarius_arm</build_export_depend>
|
|
||||||
<build_export_depend>sensor_msgs</build_export_depend>
|
|
||||||
<build_export_depend>std_msgs</build_export_depend>
|
|
||||||
<exec_depend>joy</exec_depend>
|
|
||||||
<exec_depend>roscpp</exec_depend>
|
|
||||||
<exec_depend>rospy</exec_depend>
|
|
||||||
<exec_depend>sdk_sagittarius_arm</exec_depend>
|
|
||||||
<exec_depend>sensor_msgs</exec_depend>
|
|
||||||
<exec_depend>std_msgs</exec_depend>
|
|
||||||
<exec_depend>message_generation</exec_depend>
|
|
||||||
|
|
||||||
|
|
||||||
<!-- The export tag contains other, unspecified, tags -->
|
|
||||||
<export>
|
|
||||||
<!-- Other tools can request additional information be placed here -->
|
|
||||||
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
|
|
@ -1,482 +0,0 @@
|
||||||
#!/usr/bin/env python
|
|
||||||
# -*- coding: UTF-8 -*-
|
|
||||||
|
|
||||||
import copy
|
|
||||||
import threading
|
|
||||||
import rospy
|
|
||||||
import numpy as np
|
|
||||||
import math
|
|
||||||
from sagittarius_joy.msg import arm_joy
|
|
||||||
import tf.transformations as tf_transformations
|
|
||||||
import sdk_sagittarius_arm.msg
|
|
||||||
import std_msgs.msg
|
|
||||||
import modern_robotics as mr
|
|
||||||
from sensor_msgs.msg import JointState
|
|
||||||
|
|
||||||
|
|
||||||
class AngleManipulation:
|
|
||||||
|
|
||||||
# Inverts a homogeneous transformation matrix
|
|
||||||
@staticmethod
|
|
||||||
def transInv(T):
|
|
||||||
R, p = T[:3, :3], T[:3, 3]
|
|
||||||
Rt = np.array(R).T
|
|
||||||
return np.r_[np.c_[Rt, -np.dot(Rt, p)], [[0, 0, 0, 1]]]
|
|
||||||
|
|
||||||
# Calculates 2D Rotation Matrix given a desired yaw angle
|
|
||||||
@staticmethod
|
|
||||||
def yawToRotationMatrix(yaw):
|
|
||||||
R_z = np.array([[math.cos(yaw), -math.sin(yaw)],
|
|
||||||
[math.sin(yaw), math.cos(yaw)],
|
|
||||||
])
|
|
||||||
return R_z
|
|
||||||
|
|
||||||
|
|
||||||
class SagittariusCtrlTool:
|
|
||||||
''' 机械臂简单控制工具箱
|
|
||||||
|
|
||||||
'''
|
|
||||||
_real_joint_degree = [0, 0, 0, 0, 0, 0]
|
|
||||||
_joint_degree = [0, 0, 0, 0, 0, 0]
|
|
||||||
lower_joint_limits = [-2, -1.57, -1.48, -2.9, -1.8, -3.1]
|
|
||||||
upper_joint_limits = [2, 1.4, 1.8, 2.9, 1.6, 3.1]
|
|
||||||
Slist = np.array([[0.0, 0.0, -1.0, 0.0, 0.045, 0.0],
|
|
||||||
[0.0, -1.0, 0.0, 0.125, 0.0, -0.045],
|
|
||||||
[0.0, -1.0, 0.0, 0.304, 0.0, -0.078],
|
|
||||||
[-1.0, 0.0, 0.0, 0.0, -0.304, -0.0002],
|
|
||||||
[0.0, -1.0, 0.0, 0.304, 0.0, -0.24685],
|
|
||||||
[-1.0, 0.0, 0.0, 0.0, -0.304, 0.00045]]).T
|
|
||||||
M = np.array([[1.0, 0.0, 0.0, 0.363937],
|
|
||||||
[0.0, 1.0, 0.0, 0.00045],
|
|
||||||
[0.0, 0.0, 1.0, 0.304],
|
|
||||||
[0.0, 0.0, 0.0, 1.0]])
|
|
||||||
|
|
||||||
_time_start = False
|
|
||||||
_time_last = 0
|
|
||||||
|
|
||||||
def __init__(self) -> None:
|
|
||||||
# publish torque command and joint angle to arm
|
|
||||||
self.torque_pub = rospy.Publisher(
|
|
||||||
"control_torque", std_msgs.msg.String, queue_size=3)
|
|
||||||
self.joint_pub = rospy.Publisher(
|
|
||||||
"joint/commands", sdk_sagittarius_arm.msg.ArmRadControl, queue_size=3)
|
|
||||||
self.gripper_pub = rospy.Publisher(
|
|
||||||
"gripper/command", std_msgs.msg.Float64, queue_size=3)
|
|
||||||
|
|
||||||
# subscribe joint states
|
|
||||||
self.joint_sub = rospy.Subscriber(
|
|
||||||
'joint_states', JointState, callback=self.joint_status_callback, queue_size=30)
|
|
||||||
|
|
||||||
# get current posture
|
|
||||||
self.joint_mutex = threading.Lock()
|
|
||||||
rospy.sleep(1)
|
|
||||||
self._joint_degree = copy.deepcopy(self._real_joint_degree)
|
|
||||||
self.set_gripper(0)
|
|
||||||
|
|
||||||
def get_FK(self, joint_list):
|
|
||||||
''' 正向运动学
|
|
||||||
|
|
||||||
return: 4x4 end-effect 矩阵
|
|
||||||
'''
|
|
||||||
return mr.FKinSpace(self.M, self.Slist, joint_list)
|
|
||||||
|
|
||||||
def get_IK(self, matrix, custom_guess=None, joint_num=6):
|
|
||||||
''' 逆向运动学
|
|
||||||
|
|
||||||
return: 如果有解, 返回关节角度列表, 否则返回 None
|
|
||||||
'''
|
|
||||||
solution_found = False
|
|
||||||
default_guess = [[0.0] * 6 for i in range(3)]
|
|
||||||
default_guess[1][0] = math.radians(-120)
|
|
||||||
default_guess[2][0] = math.radians(120)
|
|
||||||
if(custom_guess is None):
|
|
||||||
initial_guesses = default_guess
|
|
||||||
else:
|
|
||||||
initial_guesses = [list(custom_guess)] + default_guess
|
|
||||||
T_sd = matrix
|
|
||||||
for i in range(len(initial_guesses)):
|
|
||||||
if (joint_num == 6):
|
|
||||||
theta_list, success = mr.IKinSpace(
|
|
||||||
self.Slist, self.M, T_sd, initial_guesses[i], 0.001, 0.001)
|
|
||||||
else:
|
|
||||||
theta_list, success = mr.IKinSpace(
|
|
||||||
self.Slist[:, :joint_num], self.M, T_sd, initial_guesses[i][:joint_num], 0.001, 0.001)
|
|
||||||
if (success):
|
|
||||||
for i in range(len(theta_list)):
|
|
||||||
if (theta_list[i] <= -math.pi * 2):
|
|
||||||
theta_list[i] %= -math.pi * 2
|
|
||||||
elif (theta_list[i] >= math.pi * 2):
|
|
||||||
theta_list[i] %= math.pi * 2
|
|
||||||
if (round(theta_list[i], 3) < round(self.lower_joint_limits[i], 3)):
|
|
||||||
theta_list[i] += math.pi * 2
|
|
||||||
elif (round(theta_list[i], 3) > round(self.upper_joint_limits[i], 3)):
|
|
||||||
theta_list[i] -= math.pi * 2
|
|
||||||
if not (self.lower_joint_limits[i] <= theta_list[i] <= self.upper_joint_limits[i]):
|
|
||||||
if (len(initial_guesses) == 4 and i == 0):
|
|
||||||
rospy.logwarn(
|
|
||||||
"custom_guess faild, using default guess.")
|
|
||||||
break
|
|
||||||
solution_found = True
|
|
||||||
break
|
|
||||||
|
|
||||||
if (solution_found):
|
|
||||||
return theta_list.tolist()
|
|
||||||
else:
|
|
||||||
return None
|
|
||||||
|
|
||||||
def joint_status_callback(self, msg):
|
|
||||||
''' joint states 回调函数
|
|
||||||
'''
|
|
||||||
with self.joint_mutex:
|
|
||||||
self._real_joint_degree = list(copy.deepcopy(msg).position[:6])
|
|
||||||
|
|
||||||
def set_torque(self, enable):
|
|
||||||
''' 设置扭矩输出
|
|
||||||
|
|
||||||
enable: True 为输出扭矩, False 为不输出扭矩
|
|
||||||
|
|
||||||
'''
|
|
||||||
if (enable):
|
|
||||||
self.torque_pub.publish(std_msgs.msg.String("open"))
|
|
||||||
else:
|
|
||||||
self.torque_pub.publish(std_msgs.msg.String("off"))
|
|
||||||
return True
|
|
||||||
|
|
||||||
def set_gripper(self, line):
|
|
||||||
''' 设置夹爪距离
|
|
||||||
|
|
||||||
line: 夹爪距离, 单位是 m
|
|
||||||
'''
|
|
||||||
self.gripper_pub.publish(std_msgs.msg.Float64(line))
|
|
||||||
return True
|
|
||||||
|
|
||||||
def get_single_joint(self, index):
|
|
||||||
''' 返回单个关节的角度
|
|
||||||
|
|
||||||
index: 关节编号
|
|
||||||
|
|
||||||
return: 关节角度, 单位为弧度
|
|
||||||
'''
|
|
||||||
return self._joint_degree[index - 1]
|
|
||||||
|
|
||||||
def set_single_joint(self, index, degree):
|
|
||||||
''' 设置单个关节角度
|
|
||||||
|
|
||||||
index: 关节编号
|
|
||||||
'''
|
|
||||||
self._joint_degree[index] = degree
|
|
||||||
self.set_all_joint(self._joint_degree)
|
|
||||||
return True
|
|
||||||
|
|
||||||
def set_all_joint(self, degrees):
|
|
||||||
''' 设置所有关节角度
|
|
||||||
|
|
||||||
degrees: 关节角度的列表
|
|
||||||
|
|
||||||
return: 设置结果, 成功为True, 失败为False
|
|
||||||
'''
|
|
||||||
for i in range(len(degrees)):
|
|
||||||
if not (self.lower_joint_limits[i] <= degrees[i] <= self.upper_joint_limits[i]):
|
|
||||||
return False
|
|
||||||
self.joint_pub.publish(
|
|
||||||
sdk_sagittarius_arm.msg.ArmRadControl(rad=degrees))
|
|
||||||
self._joint_degree = degrees
|
|
||||||
return True
|
|
||||||
|
|
||||||
def get_ee_posture(self):
|
|
||||||
''' 获取 end-effect 实际的位姿
|
|
||||||
|
|
||||||
return: 4x4 end-effect 矩阵
|
|
||||||
'''
|
|
||||||
return self.get_FK(self._real_joint_degree)
|
|
||||||
|
|
||||||
def get_ee_posture_command(self):
|
|
||||||
''' 获取 end-effect 最后一次命令的位姿
|
|
||||||
|
|
||||||
return: 4x4 end-effect 矩阵
|
|
||||||
'''
|
|
||||||
return self.get_FK(self._joint_degree)
|
|
||||||
|
|
||||||
def set_ee_posture(self, matrix):
|
|
||||||
''' 设置 end-effect 的位姿
|
|
||||||
|
|
||||||
matrix: 4x4 end-effect 矩阵
|
|
||||||
return: 设置结果, 成功为True, 失败为False
|
|
||||||
'''
|
|
||||||
theta = self.get_IK(matrix, self._real_joint_degree)
|
|
||||||
if (theta is not None):
|
|
||||||
if (self.set_all_joint(theta)):
|
|
||||||
return True
|
|
||||||
if (self._time_start == False):
|
|
||||||
rospy.loginfo("The posture can not found or out of range.")
|
|
||||||
self._time_last = rospy.Time.now().to_sec()
|
|
||||||
self._time_start = True
|
|
||||||
else:
|
|
||||||
if (rospy.Time.now().to_sec() - self._time_last > 0):
|
|
||||||
self._time_start = False
|
|
||||||
return False
|
|
||||||
|
|
||||||
|
|
||||||
class SagittariusJoyDemo:
|
|
||||||
''' 手柄控制的例程
|
|
||||||
|
|
||||||
控制方式
|
|
||||||
|
|
||||||
左摇杆前/后: end-effect x 加/减
|
|
||||||
左摇杆左/右：腰部旋转 左摆/右摆
|
|
||||||
左摇杆摁下: 无
|
|
||||||
|
|
||||||
右摇杆前/后: end-effect pitch 往下看/往上看
|
|
||||||
右摇杆左/右: end-effect y 加/减
|
|
||||||
右摇杆摁下: 特殊设置 摆正夹爪
|
|
||||||
|
|
||||||
LB(L1): 高度减少
|
|
||||||
RB(R1): 高度增加
|
|
||||||
|
|
||||||
BACK(SHARE): 逆时针翻滚
|
|
||||||
START(OPTION): 顺时针翻滚
|
|
||||||
|
|
||||||
LT(L2): 夹爪减少间距
|
|
||||||
RT(R2): 夹爪增加间距
|
|
||||||
|
|
||||||
方向键上/下: 速度增加/减少
|
|
||||||
方向键上/下: 无
|
|
||||||
|
|
||||||
B(cycle): 预设姿态 home
|
|
||||||
A(cross): 预设姿态 sleep
|
|
||||||
|
|
||||||
长按 power 1秒: 扭矩输出切换
|
|
||||||
'''
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
self.waist_step = 0.01 # 一号舵机步长
|
|
||||||
self.rotate_step = 0.01 # 旋转矩阵步长
|
|
||||||
self.translate_step = 0.002 # 变换矩阵步长
|
|
||||||
self.gripper_pressure_step = 0.001 # 夹爪步长
|
|
||||||
self.current_step_scale = 2 # 步长放大倍数
|
|
||||||
self.current_torque_status = True # 扭矩输出标志位
|
|
||||||
self.current_gripper_spacing = 0 # 夹爪默认值
|
|
||||||
self.joy_msg = arm_joy()
|
|
||||||
self.joy_mutex = threading.Lock()
|
|
||||||
self.rate = rospy.Rate(15)
|
|
||||||
self.armbot = SagittariusCtrlTool()
|
|
||||||
self.T_sy = np.identity(4) # yaw
|
|
||||||
self.T_yb = np.identity(4)
|
|
||||||
self.update_T_yb()
|
|
||||||
rospy.Subscriber("commands/joy_processed",
|
|
||||||
arm_joy, self.joy_control_cb)
|
|
||||||
|
|
||||||
def update_speed(self, step_scale):
|
|
||||||
''' 速度更新
|
|
||||||
|
|
||||||
通过更新步长的放大倍速来修改速度
|
|
||||||
step_scale: 步长倍速, 默认 2
|
|
||||||
'''
|
|
||||||
self.current_step_scale = step_scale
|
|
||||||
rospy.loginfo("current step scale is %.2f." % self.current_step_scale)
|
|
||||||
|
|
||||||
def update_T_yb(self):
|
|
||||||
''' 更新 end-effect 位姿
|
|
||||||
|
|
||||||
分离姿态 yaw 为 T_sy, IK运算前需点乘
|
|
||||||
'''
|
|
||||||
T_sb = self.armbot.get_ee_posture_command()
|
|
||||||
rpy = tf_transformations.euler_from_matrix(T_sb[:3, :3])
|
|
||||||
# rpy = ang.rotationMatrixToEulerAngles(T_sb[:3, :3])
|
|
||||||
self.T_sy[:2, :2] = AngleManipulation.yawToRotationMatrix(rpy[2])
|
|
||||||
self.T_yb = np.dot(AngleManipulation.transInv(self.T_sy), T_sb)
|
|
||||||
|
|
||||||
def update_gripper_spacing(self, gripper_spacing):
|
|
||||||
''' 设置夹爪距离
|
|
||||||
|
|
||||||
gripper_spacing 夹爪距离, 单位为 m
|
|
||||||
'''
|
|
||||||
self.current_gripper_spacing = gripper_spacing
|
|
||||||
self.armbot.set_gripper(self.current_gripper_spacing)
|
|
||||||
rospy.loginfo("Gripper spacing is at %.2f." %
|
|
||||||
(self.current_gripper_spacing))
|
|
||||||
|
|
||||||
def joy_control_cb(self, msg):
|
|
||||||
''' 获取手柄按键的回调函数
|
|
||||||
|
|
||||||
'''
|
|
||||||
with self.joy_mutex:
|
|
||||||
self.joy_msg = copy.deepcopy(msg)
|
|
||||||
|
|
||||||
# Check the speed_cmd
|
|
||||||
if (msg.speed_cmd == arm_joy.SPEED_INC and self.current_step_scale < 4):
|
|
||||||
self.update_speed(self.current_step_scale + 0.1)
|
|
||||||
elif (msg.speed_cmd == arm_joy.SPEED_DEC and self.current_step_scale > 0.1):
|
|
||||||
self.update_speed(self.current_step_scale - 0.1)
|
|
||||||
|
|
||||||
# Check the torque_cmd
|
|
||||||
if (msg.torque_cmd == arm_joy.TORQUE_ON):
|
|
||||||
self.armbot.set_torque(True)
|
|
||||||
self.current_torque_status = True
|
|
||||||
elif (msg.torque_cmd == arm_joy.TORQUE_OFF):
|
|
||||||
self.armbot.set_torque(False)
|
|
||||||
self.current_torque_status = False
|
|
||||||
|
|
||||||
# @brief Main control loop to manipulate the arm
|
|
||||||
def controller(self):
|
|
||||||
''' 控制机械臂的函数
|
|
||||||
|
|
||||||
主函数循环回调
|
|
||||||
'''
|
|
||||||
waist_step = self.waist_step * self.current_step_scale
|
|
||||||
translate_step = self.translate_step * self.current_step_scale
|
|
||||||
rotate_step = self.rotate_step * self.current_step_scale
|
|
||||||
|
|
||||||
if (self.current_torque_status == False):
|
|
||||||
return
|
|
||||||
|
|
||||||
with self.joy_mutex:
|
|
||||||
msg = copy.deepcopy(self.joy_msg)
|
|
||||||
|
|
||||||
# Check the gripper_cmd
|
|
||||||
if (msg.gripper_spacing_cmd == arm_joy.GRIPPER_SPACING_INC and self.current_gripper_spacing > -0.03):
|
|
||||||
self.update_gripper_spacing(
|
|
||||||
self.current_gripper_spacing - self.gripper_pressure_step)
|
|
||||||
elif (msg.gripper_spacing_cmd == arm_joy.GRIPPER_SPACING_DEC and self.current_gripper_spacing < 0):
|
|
||||||
self.update_gripper_spacing(
|
|
||||||
self.current_gripper_spacing + self.gripper_pressure_step)
|
|
||||||
|
|
||||||
# Check the pose_cmd
|
|
||||||
if (msg.pose_cmd != 0):
|
|
||||||
if (msg.pose_cmd == arm_joy.HOME_POSE):
|
|
||||||
self.armbot.set_all_joint([0, 0, 0, 0, 0, 0])
|
|
||||||
elif (msg.pose_cmd == arm_joy.SLEEP_POSE):
|
|
||||||
self.armbot.set_all_joint([0, 1.4, -1.48, 0, -0.4851, 0])
|
|
||||||
elif (msg.pose_cmd == arm_joy.UP_POSE):
|
|
||||||
pass
|
|
||||||
self.update_T_yb()
|
|
||||||
return
|
|
||||||
|
|
||||||
# Check the waist_cmd
|
|
||||||
if (msg.ee_yaw_cmd != 0):
|
|
||||||
waist_position = self.armbot.get_single_joint(1)
|
|
||||||
if (msg.ee_yaw_cmd == arm_joy.EE_YAW_LEFT):
|
|
||||||
waist_position -= waist_step
|
|
||||||
if (waist_position >= self.armbot.upper_joint_limits[0]):
|
|
||||||
waist_position = self.armbot.upper_joint_limits[0]
|
|
||||||
self.armbot.set_single_joint(0, waist_position)
|
|
||||||
elif (msg.ee_yaw_cmd == arm_joy.EE_YAW_RIGHT):
|
|
||||||
waist_position += waist_step
|
|
||||||
if (waist_position <= self.armbot.lower_joint_limits[0]):
|
|
||||||
waist_position = self.armbot.lower_joint_limits[0]
|
|
||||||
self.armbot.set_single_joint(0, waist_position)
|
|
||||||
self.update_T_yb()
|
|
||||||
|
|
||||||
# Check the position and orientation cmd
|
|
||||||
else:
|
|
||||||
position_changed = msg.ee_x_cmd + msg.ee_y_cmd + msg.ee_z_cmd
|
|
||||||
orientation_changed = msg.ee_roll_cmd + msg.ee_pitch_cmd + msg.ee_yaw_cmd
|
|
||||||
|
|
||||||
if (position_changed + orientation_changed == 0
|
|
||||||
and msg.reset_cmd != arm_joy.ORIENTATION_RESET
|
|
||||||
and msg.reset_cmd != arm_joy.POSITION_RESET):
|
|
||||||
return
|
|
||||||
|
|
||||||
# Copy the most recent T_yb transform into a temporary variable
|
|
||||||
T_yb = np.array(self.T_yb)
|
|
||||||
|
|
||||||
# Is position has changed
|
|
||||||
if (position_changed):
|
|
||||||
# check ee_x_cmd
|
|
||||||
if (msg.ee_x_cmd == arm_joy.EE_X_INC):
|
|
||||||
T_yb[0, 3] += translate_step
|
|
||||||
elif (msg.ee_x_cmd == arm_joy.EE_X_DEC):
|
|
||||||
T_yb[0, 3] -= translate_step
|
|
||||||
|
|
||||||
# check ee_y_cmd
|
|
||||||
if (msg.ee_y_cmd == arm_joy.EE_Y_INC and T_yb[0, 3] > 0.3):
|
|
||||||
T_yb[1, 3] += translate_step
|
|
||||||
elif (msg.ee_y_cmd == arm_joy.EE_Y_DEC and T_yb[0, 3] > 0.3):
|
|
||||||
T_yb[1, 3] -= translate_step
|
|
||||||
|
|
||||||
# check ee_z_cmd
|
|
||||||
if (msg.ee_z_cmd == arm_joy.EE_Z_INC):
|
|
||||||
T_yb[2, 3] += translate_step
|
|
||||||
elif (msg.ee_z_cmd == arm_joy.EE_Z_DEC):
|
|
||||||
T_yb[2, 3] -= translate_step
|
|
||||||
|
|
||||||
# Is position need reset
|
|
||||||
elif (msg.reset_cmd == arm_joy.POSITION_RESET):
|
|
||||||
return
|
|
||||||
|
|
||||||
# Is orientation has changed
|
|
||||||
if (orientation_changed != 0):
|
|
||||||
roll, pitch, yaw = tf_transformations.euler_from_matrix(
|
|
||||||
T_yb[:3, :3])
|
|
||||||
|
|
||||||
# check ee_roll_cmd
|
|
||||||
if (msg.ee_roll_cmd == arm_joy.EE_ROLL_CW):
|
|
||||||
roll += rotate_step
|
|
||||||
elif (msg.ee_roll_cmd == arm_joy.EE_ROLL_CCW):
|
|
||||||
roll -= rotate_step
|
|
||||||
|
|
||||||
# check ee_pitch_cmd
|
|
||||||
if (msg.ee_pitch_cmd == arm_joy.EE_PITCH_DOWN):
|
|
||||||
pitch += rotate_step
|
|
||||||
elif (msg.ee_pitch_cmd == arm_joy.EE_PITCH_UP):
|
|
||||||
pitch -= rotate_step
|
|
||||||
|
|
||||||
# check ee_yaw_cmd
|
|
||||||
# if (msg.ee_yaw_cmd == arm_joy.EE_YAW_LEFT):
|
|
||||||
# yaw += rotate_step
|
|
||||||
# elif (msg.ee_yaw_cmd == arm_joy.EE_YAW_RIGHT):
|
|
||||||
# yaw -= rotate_step
|
|
||||||
|
|
||||||
T_yb[:3, :3] = tf_transformations.euler_matrix(roll, pitch, yaw)[
|
|
||||||
:3, :3]
|
|
||||||
|
|
||||||
# Is orientation need reset
|
|
||||||
elif (msg.reset_cmd == arm_joy.ORIENTATION_RESET):
|
|
||||||
T_yb[1, 3] = 0
|
|
||||||
roll, pitch, yaw = tf_transformations.euler_from_matrix(
|
|
||||||
T_yb[:3, :3])
|
|
||||||
T_yb[:3, :3] = tf_transformations.euler_matrix(0, pitch, 0)[
|
|
||||||
:3, :3]
|
|
||||||
|
|
||||||
# Get desired transformation matrix of the end-effector w.r.t. the base frame
|
|
||||||
T_sd = np.dot(self.T_sy, T_yb)
|
|
||||||
success = self.armbot.set_ee_posture(T_sd)
|
|
||||||
if (success):
|
|
||||||
self.T_yb = np.array(T_yb)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
rospy.init_node('sagittarius_joy_demo')
|
|
||||||
arm = SagittariusJoyDemo()
|
|
||||||
rospy.sleep(5)
|
|
||||||
|
|
||||||
rospy.loginfo("*********** 控制方式 **************")
|
|
||||||
rospy.loginfo("左摇杆前/后: end-effect x 加/减")
|
|
||||||
rospy.loginfo("左摇杆左/右：腰部旋转 左摆/右摆")
|
|
||||||
rospy.loginfo("左摇杆摁下: 无")
|
|
||||||
rospy.loginfo("")
|
|
||||||
rospy.loginfo("右摇杆前/后: end-effect pitch 往下看/往上看")
|
|
||||||
rospy.loginfo("右摇杆左/右: end-effect y 加/减")
|
|
||||||
rospy.loginfo("右摇杆摁下: 特殊设置 摆正夹爪")
|
|
||||||
rospy.loginfo("")
|
|
||||||
rospy.loginfo("LB(L1): 高度减少")
|
|
||||||
rospy.loginfo("RB(R1): 高度增加")
|
|
||||||
rospy.loginfo("")
|
|
||||||
rospy.loginfo("BACK(SHARE): 逆时针翻滚")
|
|
||||||
rospy.loginfo("START(OPTION): 顺时针翻滚")
|
|
||||||
rospy.loginfo("")
|
|
||||||
rospy.loginfo("LT(L2): 夹爪减少间距")
|
|
||||||
rospy.loginfo("RT(R2): 夹爪增加间距")
|
|
||||||
rospy.loginfo("")
|
|
||||||
rospy.loginfo("方向键上/下: 速度增加/减少")
|
|
||||||
rospy.loginfo("方向键上/下: 无")
|
|
||||||
rospy.loginfo("")
|
|
||||||
rospy.loginfo("B(cycle): 预设姿态 home")
|
|
||||||
rospy.loginfo("A(cross): 预设姿态 sleep")
|
|
||||||
rospy.loginfo("")
|
|
||||||
rospy.loginfo("长按 power 1秒: 扭矩输出切换")
|
|
||||||
rospy.loginfo("*********************************")
|
|
||||||
|
|
||||||
while not rospy.is_shutdown():
|
|
||||||
arm.controller()
|
|
||||||
arm.rate.sleep()
|
|
||||||
|
|
@ -1,192 +0,0 @@
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <sensor_msgs/Joy.h>
|
|
||||||
#include "sagittarius_joy/arm_joy.h"
|
|
||||||
|
|
||||||
// Xbox 360 Controller button mappings
|
|
||||||
static const std::map<std::string, int> xbox360 = {{"SLEEP_POSE", 0}, // buttons start here
|
|
||||||
{"HOME_POSE", 1},
|
|
||||||
{"SUPER_KEY", 2},
|
|
||||||
{"UP_POSE", 3},
|
|
||||||
{"EE_Z_DEC", 4},
|
|
||||||
{"EE_Z_INC", 5},
|
|
||||||
{"EE_ROLL_CCW", 6},
|
|
||||||
{"EE_ROLL_CW", 7},
|
|
||||||
{"TORQUE_ONOFF", 8},
|
|
||||||
{"POSITION_RESET", 9},
|
|
||||||
{"ORIENTATION_RESET", 10},
|
|
||||||
{"EE_YAW", 0}, // axes start here
|
|
||||||
{"EE_X", 1},
|
|
||||||
{"GRIPPER_SPACING_INC", 2},
|
|
||||||
{"EE_Y", 3},
|
|
||||||
{"EE_PITCH", 4},
|
|
||||||
{"GRIPPER_SPACING_DEC", 5},
|
|
||||||
{"TORQUE", 6},
|
|
||||||
{"SPEED", 7}};
|
|
||||||
|
|
||||||
// PS4 Controller button mappings
|
|
||||||
static const std::map<std::string, int> ps4 = {{"SLEEP_POSE", 0}, // buttons start here
|
|
||||||
{"HOME_POSE", 1},
|
|
||||||
{"UP_POSE", 2},
|
|
||||||
{"SUPER_KEY", 3},
|
|
||||||
{"EE_Z_DEC", 4},
|
|
||||||
{"EE_Z_INC", 5},
|
|
||||||
{"null_1", 6},
|
|
||||||
{"null_2", 7},
|
|
||||||
{"EE_ROLL_CCW", 8},
|
|
||||||
{"EE_ROLL_CW", 9},
|
|
||||||
{"TORQUE_ONOFF", 10},
|
|
||||||
{"POSITION_RESET", 11},
|
|
||||||
{"ORIENTATION_RESET", 12},
|
|
||||||
{"EE_YAW", 0}, // axes start here
|
|
||||||
{"EE_X", 1},
|
|
||||||
{"GRIPPER_SPACING_INC", 2},
|
|
||||||
{"EE_Y", 3},
|
|
||||||
{"EE_PITCH", 4},
|
|
||||||
{"GRIPPER_SPACING_DEC", 5},
|
|
||||||
{"TORQUE", 6},
|
|
||||||
{"SPEED", 7}};
|
|
||||||
|
|
||||||
ros::Publisher pub_joy_cmd; // ROS Publisher to publish ArmJoy messages
|
|
||||||
sagittarius_joy::arm_joy prev_joy_cmd; // Keep track of the previously commanded ArmJoy message so that only unique messages are published
|
|
||||||
std::map<std::string, int> cntlr; // Holds the controller button mappings
|
|
||||||
std::string controller_type; // Holds the name of the controller received from the ROS Parameter server
|
|
||||||
double threshold; // Joystick sensitivity threshold
|
|
||||||
|
|
||||||
/// @brief Joystick callback to create custom ArmJoy messages to control the Arm
|
|
||||||
/// @param msg - raw sensor_msgs::Joy data
|
|
||||||
void joy_state_cb(const sensor_msgs::Joy &msg)
|
|
||||||
{
|
|
||||||
static double time_start;
|
|
||||||
static bool timer_started = false;
|
|
||||||
static bool torque_cmd_last_state = true;
|
|
||||||
sagittarius_joy::arm_joy joy_cmd;
|
|
||||||
|
|
||||||
// Check the ee_x_cmd
|
|
||||||
if (msg.axes.at(cntlr["EE_X"]) >= threshold)
|
|
||||||
joy_cmd.ee_x_cmd = sagittarius_joy::arm_joy::EE_X_INC;
|
|
||||||
else if (msg.axes.at(cntlr["EE_X"]) <= -threshold)
|
|
||||||
joy_cmd.ee_x_cmd = sagittarius_joy::arm_joy::EE_X_DEC;
|
|
||||||
|
|
||||||
// Check the ee_y_cmd
|
|
||||||
if (msg.axes.at(cntlr["EE_Y"]) >= threshold)
|
|
||||||
joy_cmd.ee_y_cmd = sagittarius_joy::arm_joy::EE_Y_INC;
|
|
||||||
else if (msg.axes.at(cntlr["EE_Y"]) <= -threshold)
|
|
||||||
joy_cmd.ee_y_cmd = sagittarius_joy::arm_joy::EE_Y_DEC;
|
|
||||||
|
|
||||||
// Check the ee_z_cmd
|
|
||||||
if (msg.buttons.at(cntlr["EE_Z_INC"]) == 1)
|
|
||||||
joy_cmd.ee_z_cmd = sagittarius_joy::arm_joy::EE_Z_INC;
|
|
||||||
else if (msg.buttons.at(cntlr["EE_Z_DEC"]) == 1)
|
|
||||||
joy_cmd.ee_z_cmd = sagittarius_joy::arm_joy::EE_Z_DEC;
|
|
||||||
|
|
||||||
// Check the ee_roll_cmd
|
|
||||||
if (msg.buttons.at(cntlr["EE_ROLL_CW"]) == 1)
|
|
||||||
joy_cmd.ee_roll_cmd = sagittarius_joy::arm_joy::EE_ROLL_CW;
|
|
||||||
else if (msg.buttons.at(cntlr["EE_ROLL_CCW"]) == 1)
|
|
||||||
joy_cmd.ee_roll_cmd = sagittarius_joy::arm_joy::EE_ROLL_CCW;
|
|
||||||
|
|
||||||
// Check the ee_pitch_cmd
|
|
||||||
if (msg.axes.at(cntlr["EE_PITCH"]) >= threshold)
|
|
||||||
joy_cmd.ee_pitch_cmd = sagittarius_joy::arm_joy::EE_PITCH_DOWN;
|
|
||||||
else if (msg.axes.at(cntlr["EE_PITCH"]) <= -threshold)
|
|
||||||
joy_cmd.ee_pitch_cmd = sagittarius_joy::arm_joy::EE_PITCH_UP;
|
|
||||||
|
|
||||||
// Check the ee_yaw_cmd
|
|
||||||
if (msg.axes.at(cntlr["EE_YAW"]) >= threshold)
|
|
||||||
joy_cmd.ee_yaw_cmd = sagittarius_joy::arm_joy::EE_YAW_LEFT;
|
|
||||||
else if (msg.axes.at(cntlr["EE_YAW"]) <= -threshold)
|
|
||||||
joy_cmd.ee_yaw_cmd = sagittarius_joy::arm_joy::EE_YAW_RIGHT;
|
|
||||||
|
|
||||||
// Check the pose_cmd
|
|
||||||
if (msg.buttons.at(cntlr["HOME_POSE"]) == 1)
|
|
||||||
joy_cmd.pose_cmd = sagittarius_joy::arm_joy::HOME_POSE;
|
|
||||||
else if (msg.buttons.at(cntlr["SLEEP_POSE"]) == 1)
|
|
||||||
joy_cmd.pose_cmd = sagittarius_joy::arm_joy::SLEEP_POSE;
|
|
||||||
else if (msg.buttons.at(cntlr["UP_POSE"]) == 1)
|
|
||||||
joy_cmd.pose_cmd = sagittarius_joy::arm_joy::UP_POSE;
|
|
||||||
|
|
||||||
// Check the reset_cmd
|
|
||||||
if (msg.buttons.at(cntlr["POSITION_RESET"]) == 1)
|
|
||||||
joy_cmd.reset_cmd = sagittarius_joy::arm_joy::POSITION_RESET;
|
|
||||||
else if (msg.buttons.at(cntlr["ORIENTATION_RESET"]) == 1)
|
|
||||||
joy_cmd.reset_cmd = sagittarius_joy::arm_joy::ORIENTATION_RESET;
|
|
||||||
|
|
||||||
// Check the gripper_cmd
|
|
||||||
if (msg.axes.at(cntlr["GRIPPER_SPACING_INC"]) <= -0.2)
|
|
||||||
joy_cmd.gripper_spacing_cmd = sagittarius_joy::arm_joy::GRIPPER_SPACING_INC;
|
|
||||||
else if (msg.axes.at(cntlr["GRIPPER_SPACING_DEC"]) <= -0.2)
|
|
||||||
joy_cmd.gripper_spacing_cmd = sagittarius_joy::arm_joy::GRIPPER_SPACING_DEC;
|
|
||||||
|
|
||||||
// Check the speed_cmd
|
|
||||||
if (msg.axes.at(cntlr["SPEED"]) == 1)
|
|
||||||
joy_cmd.speed_cmd = sagittarius_joy::arm_joy::SPEED_INC;
|
|
||||||
else if (msg.axes.at(cntlr["SPEED"]) == -1)
|
|
||||||
joy_cmd.speed_cmd = sagittarius_joy::arm_joy::SPEED_DEC;
|
|
||||||
|
|
||||||
// Check the torque_cmd
|
|
||||||
if (msg.buttons.at(cntlr["TORQUE_ONOFF"]) == 1)
|
|
||||||
{
|
|
||||||
time_start = ros::Time::now().toSec();
|
|
||||||
timer_started = true;
|
|
||||||
}
|
|
||||||
else if (msg.buttons.at(cntlr["TORQUE_ONOFF"]) == 0)
|
|
||||||
{
|
|
||||||
if (timer_started && ros::Time::now().toSec() - time_start > 0)
|
|
||||||
if (torque_cmd_last_state == true)
|
|
||||||
{
|
|
||||||
joy_cmd.torque_cmd = sagittarius_joy::arm_joy::TORQUE_OFF;
|
|
||||||
torque_cmd_last_state = false;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
joy_cmd.torque_cmd = sagittarius_joy::arm_joy::TORQUE_ON;
|
|
||||||
torque_cmd_last_state = true;
|
|
||||||
}
|
|
||||||
timer_started = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check the torque_lev_cmd
|
|
||||||
if (msg.axes.at(cntlr["TORQUE"]) == 1)
|
|
||||||
joy_cmd.torque_lev_cmd = sagittarius_joy::arm_joy::TORQUE_INC;
|
|
||||||
else if (msg.axes.at(cntlr["TORQUE"]) == -1)
|
|
||||||
joy_cmd.torque_lev_cmd = sagittarius_joy::arm_joy::TORQUE_DEC;
|
|
||||||
|
|
||||||
// Check the super_cmd
|
|
||||||
if (msg.buttons.at(cntlr["SUPER_KEY"]) == 1)
|
|
||||||
joy_cmd.super_cmd = sagittarius_joy::arm_joy::SUPER_KEY;
|
|
||||||
|
|
||||||
// Only publish a ArmJoy message if any of the following fields have changed.
|
|
||||||
if (!(prev_joy_cmd.ee_x_cmd == joy_cmd.ee_x_cmd &&
|
|
||||||
prev_joy_cmd.ee_y_cmd == joy_cmd.ee_y_cmd &&
|
|
||||||
prev_joy_cmd.ee_z_cmd == joy_cmd.ee_z_cmd &&
|
|
||||||
prev_joy_cmd.ee_roll_cmd == joy_cmd.ee_roll_cmd &&
|
|
||||||
prev_joy_cmd.ee_pitch_cmd == joy_cmd.ee_pitch_cmd &&
|
|
||||||
prev_joy_cmd.ee_yaw_cmd == joy_cmd.ee_yaw_cmd &&
|
|
||||||
prev_joy_cmd.pose_cmd == joy_cmd.pose_cmd &&
|
|
||||||
prev_joy_cmd.reset_cmd == joy_cmd.reset_cmd &&
|
|
||||||
prev_joy_cmd.speed_cmd == joy_cmd.speed_cmd &&
|
|
||||||
prev_joy_cmd.gripper_spacing_cmd == joy_cmd.gripper_spacing_cmd &&
|
|
||||||
prev_joy_cmd.torque_cmd == joy_cmd.torque_cmd &&
|
|
||||||
prev_joy_cmd.torque_lev_cmd == joy_cmd.torque_lev_cmd &&
|
|
||||||
prev_joy_cmd.super_cmd == joy_cmd.super_cmd))
|
|
||||||
pub_joy_cmd.publish(joy_cmd);
|
|
||||||
prev_joy_cmd = joy_cmd;
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "sagittarius_joy");
|
|
||||||
ros::NodeHandle n;
|
|
||||||
ros::param::get("~threshold", threshold);
|
|
||||||
ros::param::get("~controller", controller_type);
|
|
||||||
if (controller_type == "xbox360")
|
|
||||||
cntlr = xbox360;
|
|
||||||
else if (controller_type == "ps4")
|
|
||||||
cntlr = ps4;
|
|
||||||
// else
|
|
||||||
// cntlr = ps4;
|
|
||||||
ros::Subscriber sub_joy_raw = n.subscribe("commands/joy_raw", 10, joy_state_cb);
|
|
||||||
pub_joy_cmd = n.advertise<sagittarius_joy::arm_joy>("commands/joy_processed", 10);
|
|
||||||
ros::spin();
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
@ -1,53 +0,0 @@
|
||||||
cmake_minimum_required(VERSION 2.8.3)
|
|
||||||
project(sagittarius_puppet_control)
|
|
||||||
|
|
||||||
## Compile as C++11, supported in ROS Kinetic and newer
|
|
||||||
add_compile_options(-std=c++11)
|
|
||||||
|
|
||||||
## 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
|
|
||||||
sdk_sagittarius_arm
|
|
||||||
rosbag
|
|
||||||
roscpp
|
|
||||||
tf2_ros
|
|
||||||
rviz
|
|
||||||
sensor_msgs
|
|
||||||
std_msgs
|
|
||||||
std_srvs
|
|
||||||
)
|
|
||||||
|
|
||||||
###################################
|
|
||||||
## catkin specific configuration ##
|
|
||||||
###################################
|
|
||||||
## The catkin_package macro generates cmake config files for your package
|
|
||||||
## Declare things to be passed to dependent projects
|
|
||||||
catkin_package(
|
|
||||||
CATKIN_DEPENDS rosbag roscpp tf2_ros rviz sensor_msgs std_msgs std_srvs
|
|
||||||
)
|
|
||||||
|
|
||||||
###########
|
|
||||||
## Build ##
|
|
||||||
###########
|
|
||||||
|
|
||||||
## Specify additional locations of header files
|
|
||||||
## Your package locations should be listed before other locations
|
|
||||||
include_directories(
|
|
||||||
${catkin_INCLUDE_DIRS}
|
|
||||||
)
|
|
||||||
|
|
||||||
## Declare a C++ executable
|
|
||||||
## Specify libraries to link a library or executable target against
|
|
||||||
## Add cmake target dependencies of the executable
|
|
||||||
###add_executable(puppet_control_node src/puppet_control_node.cpp)
|
|
||||||
###target_link_libraries(puppet_control_node ${catkin_LIBRARIES})
|
|
||||||
###add_dependencies(puppet_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
|
||||||
|
|
||||||
add_executable(puppet_control_single_node src/puppet_control_single_node.cpp)
|
|
||||||
target_link_libraries(puppet_control_single_node ${catkin_LIBRARIES})
|
|
||||||
add_dependencies(puppet_control_single_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
|
||||||
|
|
||||||
add_executable(puppet_control_node src/puppet_control_node.cpp)
|
|
||||||
target_link_libraries(puppet_control_node ${catkin_LIBRARIES})
|
|
||||||
add_dependencies(puppet_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
|
||||||
|
|
@ -1,2 +0,0 @@
|
||||||
sleep 5
|
|
||||||
rosbag play record1.bag
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,66 +0,0 @@
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="robot_model_master" default="sgr532"/>
|
|
||||||
<arg name="master_port" default="/dev/ttyACM0"/>
|
|
||||||
<arg name="robot_model_slaver" default="sgr532"/>
|
|
||||||
<arg name="slaver1_port" default="/dev/ttyACM1"/>
|
|
||||||
<arg name="rvizconfig" default="$(find sagittarius_puppet_control)/rviz/puppet_config.rviz" />
|
|
||||||
<arg name="arm_run" default="true"/>
|
|
||||||
<arg name="use_default_rviz" default="false"/>
|
|
||||||
|
|
||||||
<arg name="slaver_robot_num" default="1"/>
|
|
||||||
|
|
||||||
<arg name="robot_name_master" value="master/$(arg robot_model_master)"/>
|
|
||||||
<arg name="robot_name_slaver1" value="slaver1/$(arg robot_model_slaver)"/>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_master)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_master)"/>
|
|
||||||
<arg name="serialname" value="$(arg master_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver1_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="puppet_control_node"
|
|
||||||
pkg="sagittarius_puppet_control"
|
|
||||||
type="puppet_control_node"
|
|
||||||
respawn="false"
|
|
||||||
output="screen">
|
|
||||||
<param name="robot_name_master" value="$(arg robot_name_master)"/>
|
|
||||||
<param name="robot_name_slaver1" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<param name="slaver_robot_num" value="$(arg slaver_robot_num)"/>
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_1_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 -0.3 0 0 0 0 /world /$(arg robot_name_master)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_2_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0.3 0 0 0 0 /world /$(arg robot_name_slaver1)/base_link"/>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="rviz"
|
|
||||||
pkg="rviz"
|
|
||||||
type="rviz"
|
|
||||||
args="-d $(arg rvizconfig)"
|
|
||||||
required="true"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,81 +0,0 @@
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="robot_model_master" default="sgr532"/>
|
|
||||||
<arg name="master_port" default="/dev/ttyACM0"/>
|
|
||||||
<arg name="robot_model_slaver" default="sgr532"/>
|
|
||||||
<arg name="slaver1_port" default="/dev/ttyACM1"/>
|
|
||||||
<arg name="slaver2_port" default="/dev/ttyACM2"/>
|
|
||||||
<arg name="rvizconfig" default="$(find sagittarius_puppet_control)/rviz/puppet_config.rviz" />
|
|
||||||
<arg name="arm_run" default="true"/>
|
|
||||||
<arg name="use_default_rviz" default="false"/>
|
|
||||||
|
|
||||||
<arg name="slaver_robot_num" default="2"/>
|
|
||||||
|
|
||||||
<arg name="robot_name_master" value="master/$(arg robot_model_master)"/>
|
|
||||||
<arg name="robot_name_slaver1" value="slaver1/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver2" value="slaver2/$(arg robot_model_slaver)"/>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_master)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_master)"/>
|
|
||||||
<arg name="serialname" value="$(arg master_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver1_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver2)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver2_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
|
|
||||||
</include>
|
|
||||||
<node
|
|
||||||
name="puppet_control_node"
|
|
||||||
pkg="sagittarius_puppet_control"
|
|
||||||
type="puppet_control_node"
|
|
||||||
respawn="false"
|
|
||||||
output="screen">
|
|
||||||
<param name="robot_name_master" value="$(arg robot_name_master)"/>
|
|
||||||
<param name="robot_name_slaver1" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<param name="robot_name_slaver2" value="$(arg robot_name_slaver2)"/>
|
|
||||||
<param name="slaver_robot_num" value="$(arg slaver_robot_num)"/>
|
|
||||||
|
|
||||||
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_1_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 -0.3 0 0 0 0 /world /$(arg robot_name_master)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_2_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0.3 0 0 0 0 /world /$(arg robot_name_slaver1)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_3_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0.9 0 0 0 0 /world /$(arg robot_name_slaver2)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="rviz"
|
|
||||||
pkg="rviz"
|
|
||||||
type="rviz"
|
|
||||||
args="-d $(arg rvizconfig)"
|
|
||||||
required="true"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,99 +0,0 @@
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="robot_model_master" default="sgr532"/>
|
|
||||||
<arg name="master_port" default="/dev/ttyACM0"/>
|
|
||||||
<arg name="robot_model_slaver" default="sgr532"/>
|
|
||||||
<arg name="slaver1_port" default="/dev/ttyACM1"/>
|
|
||||||
<arg name="slaver2_port" default="/dev/ttyACM2"/>
|
|
||||||
<arg name="slaver3_port" default="/dev/ttyACM3"/>
|
|
||||||
|
|
||||||
<arg name="rvizconfig" default="$(find sagittarius_puppet_control)/rviz/puppet_config.rviz" />
|
|
||||||
<arg name="arm_run" default="true"/>
|
|
||||||
<arg name="use_default_rviz" default="false"/>
|
|
||||||
|
|
||||||
<arg name="slaver_robot_num" default="3"/>
|
|
||||||
|
|
||||||
<arg name="robot_name_master" value="master/$(arg robot_model_master)"/>
|
|
||||||
<arg name="robot_name_slaver1" value="slaver1/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver2" value="slaver2/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver3" value="slaver3/$(arg robot_model_slaver)"/>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_master)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_master)"/>
|
|
||||||
<arg name="serialname" value="$(arg master_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver1_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver2)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver2_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver3)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver3_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
<node
|
|
||||||
name="puppet_control_node"
|
|
||||||
pkg="sagittarius_puppet_control"
|
|
||||||
type="puppet_control_node"
|
|
||||||
respawn="false"
|
|
||||||
output="screen">
|
|
||||||
<param name="robot_name_master" value="$(arg robot_name_master)"/>
|
|
||||||
<param name="robot_name_slaver1" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<param name="robot_name_slaver2" value="$(arg robot_name_slaver2)"/>
|
|
||||||
<param name="robot_name_slaver3" value="$(arg robot_name_slaver3)"/>
|
|
||||||
|
|
||||||
<param name="slaver_robot_num" value="$(arg slaver_robot_num)"/>
|
|
||||||
|
|
||||||
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_1_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 -0.3 0 0 0 0 /world /$(arg robot_name_master)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_2_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0.3 0 0 0 0 /world /$(arg robot_name_slaver1)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_3_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0.9 0 0 0 0 /world /$(arg robot_name_slaver2)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_4_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 1.5 0 0 0 0 /world /$(arg robot_name_slaver3)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="rviz"
|
|
||||||
pkg="rviz"
|
|
||||||
type="rviz"
|
|
||||||
args="-d $(arg rvizconfig)"
|
|
||||||
required="true"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,117 +0,0 @@
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="robot_model_master" default="sgr532"/>
|
|
||||||
<arg name="master_port" default="/dev/ttyACM0"/>
|
|
||||||
<arg name="robot_model_slaver" default="sgr532"/>
|
|
||||||
<arg name="slaver1_port" default="/dev/ttyACM1"/>
|
|
||||||
<arg name="slaver2_port" default="/dev/ttyACM2"/>
|
|
||||||
<arg name="slaver3_port" default="/dev/ttyACM3"/>
|
|
||||||
<arg name="slaver4_port" default="/dev/ttyACM4"/>
|
|
||||||
|
|
||||||
<arg name="rvizconfig" default="$(find sagittarius_puppet_control)/rviz/puppet_config.rviz" />
|
|
||||||
<arg name="arm_run" default="true"/>
|
|
||||||
<arg name="use_default_rviz" default="false"/>
|
|
||||||
|
|
||||||
<arg name="slaver_robot_num" default="4"/>
|
|
||||||
|
|
||||||
<arg name="robot_name_master" value="master/$(arg robot_model_master)"/>
|
|
||||||
<arg name="robot_name_slaver1" value="slaver1/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver2" value="slaver2/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver3" value="slaver3/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver4" value="slaver4/$(arg robot_model_slaver)"/>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_master)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_master)"/>
|
|
||||||
<arg name="serialname" value="$(arg master_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver1_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver2)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver2_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver3)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver3_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver4)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver4_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="puppet_control_node"
|
|
||||||
pkg="sagittarius_puppet_control"
|
|
||||||
type="puppet_control_node"
|
|
||||||
respawn="false"
|
|
||||||
output="screen">
|
|
||||||
<param name="robot_name_master" value="$(arg robot_name_master)"/>
|
|
||||||
<param name="robot_name_slaver1" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<param name="robot_name_slaver2" value="$(arg robot_name_slaver2)"/>
|
|
||||||
<param name="robot_name_slaver3" value="$(arg robot_name_slaver3)"/>
|
|
||||||
<param name="robot_name_slaver4" value="$(arg robot_name_slaver4)"/>
|
|
||||||
|
|
||||||
<param name="slaver_robot_num" value="$(arg slaver_robot_num)"/>
|
|
||||||
|
|
||||||
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_1_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 -0.3 0 0 0 0 /world /$(arg robot_name_master)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_2_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0.3 0 0 0 0 /world /$(arg robot_name_slaver1)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_3_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0.9 0 0 0 0 /world /$(arg robot_name_slaver2)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_4_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 1.5 0 0 0 0 /world /$(arg robot_name_slaver3)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_5_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 2.1 0 0 0 0 /world /$(arg robot_name_slaver4)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="rviz"
|
|
||||||
pkg="rviz"
|
|
||||||
type="rviz"
|
|
||||||
args="-d $(arg rvizconfig)"
|
|
||||||
required="true"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,133 +0,0 @@
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="robot_model_master" default="sgr532"/>
|
|
||||||
<arg name="master_port" default="/dev/ttyACM0"/>
|
|
||||||
<arg name="robot_model_slaver" default="sgr532"/>
|
|
||||||
<arg name="slaver1_port" default="/dev/ttyACM1"/>
|
|
||||||
<arg name="slaver2_port" default="/dev/ttyACM2"/>
|
|
||||||
<arg name="slaver3_port" default="/dev/ttyACM3"/>
|
|
||||||
<arg name="slaver4_port" default="/dev/ttyACM4"/>
|
|
||||||
<arg name="slaver5_port" default="/dev/ttyACM5"/>
|
|
||||||
|
|
||||||
<arg name="rvizconfig" default="$(find sagittarius_puppet_control)/rviz/puppet_config.rviz" />
|
|
||||||
<arg name="arm_run" default="true"/>
|
|
||||||
<arg name="use_default_rviz" default="false"/>
|
|
||||||
|
|
||||||
<arg name="slaver_robot_num" default="5"/>
|
|
||||||
|
|
||||||
<arg name="robot_name_master" value="master/$(arg robot_model_master)"/>
|
|
||||||
<arg name="robot_name_slaver1" value="slaver1/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver2" value="slaver2/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver3" value="slaver3/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver4" value="slaver4/$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="robot_name_slaver5" value="slaver5/$(arg robot_model_slaver)"/>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_master)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_master)"/>
|
|
||||||
<arg name="serialname" value="$(arg master_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver1_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver2)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver2_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver3)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver3_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver4)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver4_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name_slaver5)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model_slaver)"/>
|
|
||||||
<arg name="serialname" value="$(arg slaver5_port)"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="use_world_frame" value="false"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="puppet_control_node"
|
|
||||||
pkg="sagittarius_puppet_control"
|
|
||||||
type="puppet_control_node"
|
|
||||||
respawn="false"
|
|
||||||
output="screen">
|
|
||||||
<param name="robot_name_master" value="$(arg robot_name_master)"/>
|
|
||||||
<param name="robot_name_slaver1" value="$(arg robot_name_slaver1)"/>
|
|
||||||
<param name="robot_name_slaver2" value="$(arg robot_name_slaver2)"/>
|
|
||||||
<param name="robot_name_slaver3" value="$(arg robot_name_slaver3)"/>
|
|
||||||
<param name="robot_name_slaver4" value="$(arg robot_name_slaver4)"/>
|
|
||||||
<param name="robot_name_slaver5" value="$(arg robot_name_slaver5)"/>
|
|
||||||
|
|
||||||
<param name="slaver_robot_num" value="$(arg slaver_robot_num)"/>
|
|
||||||
|
|
||||||
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_1_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 -0.3 0 0 0 0 /world /$(arg robot_name_master)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_2_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0.3 0 0 0 0 /world /$(arg robot_name_slaver1)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_3_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0.9 0 0 0 0 /world /$(arg robot_name_slaver2)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_4_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 1.5 0 0 0 0 /world /$(arg robot_name_slaver3)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_5_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 2.1 0 0 0 0 /world /$(arg robot_name_slaver4)/base_link"/>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_6_transform_broadcaster"
|
|
||||||
pkg="tf2_ros"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 2.7 0 0 0 0 /world /$(arg robot_name_slaver5)/base_link"/>
|
|
||||||
<node
|
|
||||||
name="rviz"
|
|
||||||
pkg="rviz"
|
|
||||||
type="rviz"
|
|
||||||
args="-d $(arg rvizconfig)"
|
|
||||||
required="true"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,31 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<launch>
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default="$(arg robot_name)"/>
|
|
||||||
<arg name="bag_name" default="record"/>
|
|
||||||
<arg name="arm_run" default="true"/>
|
|
||||||
<arg name="joint_pub_gui" default="false"/>
|
|
||||||
<arg name="use_joint_pub" default="true"/>
|
|
||||||
<include if="$(arg arm_run)" file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model)"/>
|
|
||||||
<arg name="use_joint_pub" value="$(arg use_joint_pub)"/>
|
|
||||||
<arg name="joint_pub_gui" value="$(arg joint_pub_gui)"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="puppet_control_single_node"
|
|
||||||
pkg="sagittarius_puppet_control"
|
|
||||||
type="puppet_control_single_node"
|
|
||||||
respawn="false"
|
|
||||||
output="screen"
|
|
||||||
ns="$(arg robot_name)">
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="rosbag_record_commands"
|
|
||||||
pkg="rosbag"
|
|
||||||
type="record"
|
|
||||||
args="record -O $(find sagittarius_puppet_control)/bag/$(arg bag_name) /$(arg robot_name)/joint/commands /$(arg robot_name)/gripper/command"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,13 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<launch>
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default="$(arg robot_name)"/>
|
|
||||||
<!---->
|
|
||||||
<include file="$(find sdk_sagittarius_arm)/launch/run_sagittarius.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model)"/>
|
|
||||||
</include>
|
|
||||||
<!--是否开始 -->
|
|
||||||
<node pkg="sagittarius_puppet_control" type="rosbag_play.sh" name="pstc" />
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,43 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<package format="2">
|
|
||||||
<name>sagittarius_puppet_control</name>
|
|
||||||
<version>0.0.0</version>
|
|
||||||
<description>The sagittarius_puppet_control package</description>
|
|
||||||
<maintainer email="help@nxrobo.com">help</maintainer>
|
|
||||||
<license>BSD</license>
|
|
||||||
|
|
||||||
<author email="litian.zhuang@nxrobo.com">litian.zhuang</author>
|
|
||||||
|
|
||||||
<!-- The *depend tags are used to specify dependencies -->
|
|
||||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
<build_depend>sdk_sagittarius_arm</build_depend>
|
|
||||||
<build_depend>rosbag</build_depend>
|
|
||||||
<build_depend>roscpp</build_depend>
|
|
||||||
<build_depend>tf2_ros</build_depend>
|
|
||||||
<build_depend>rviz</build_depend>
|
|
||||||
<build_depend>sensor_msgs</build_depend>
|
|
||||||
<build_depend>std_msgs</build_depend>
|
|
||||||
<build_depend>std_srvs</build_depend>
|
|
||||||
<build_export_depend>sdk_sagittarius_arm</build_export_depend>
|
|
||||||
<build_export_depend>rosbag</build_export_depend>
|
|
||||||
<build_export_depend>roscpp</build_export_depend>
|
|
||||||
<build_export_depend>tf2_ros</build_export_depend>
|
|
||||||
<build_export_depend>rviz</build_export_depend>
|
|
||||||
<build_export_depend>sensor_msgs</build_export_depend>
|
|
||||||
<build_export_depend>std_msgs</build_export_depend>
|
|
||||||
<build_export_depend>std_srvs</build_export_depend>
|
|
||||||
<exec_depend>sdk_sagittarius_arm</exec_depend>
|
|
||||||
<exec_depend>rosbag</exec_depend>
|
|
||||||
<exec_depend>roscpp</exec_depend>
|
|
||||||
<exec_depend>tf2_ros</exec_depend>
|
|
||||||
<exec_depend>rviz</exec_depend>
|
|
||||||
<exec_depend>sensor_msgs</exec_depend>
|
|
||||||
<exec_depend>std_msgs</exec_depend>
|
|
||||||
<exec_depend>std_srvs</exec_depend>
|
|
||||||
|
|
||||||
<!-- The export tag contains other, unspecified, tags -->
|
|
||||||
<export>
|
|
||||||
<!-- Other tools can request additional information be placed here -->
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
|
|
@ -1,375 +0,0 @@
|
||||||
Panels:
|
|
||||||
- Class: rviz/Displays
|
|
||||||
Help Height: 78
|
|
||||||
Name: Displays
|
|
||||||
Property Tree Widget:
|
|
||||||
Expanded:
|
|
||||||
- /TF1/Frames1
|
|
||||||
- /TF1/Tree1
|
|
||||||
- /RobotModel1
|
|
||||||
- /RobotModel2
|
|
||||||
Splitter Ratio: 0.4979674816131592
|
|
||||||
Tree Height: 539
|
|
||||||
- Class: rviz/Selection
|
|
||||||
Name: Selection
|
|
||||||
- Class: rviz/Tool Properties
|
|
||||||
Expanded:
|
|
||||||
- /2D Pose Estimate1
|
|
||||||
- /2D Nav Goal1
|
|
||||||
- /Publish Point1
|
|
||||||
Name: Tool Properties
|
|
||||||
Splitter Ratio: 0.5886790156364441
|
|
||||||
- Class: rviz/Views
|
|
||||||
Expanded:
|
|
||||||
- /Current View1
|
|
||||||
Name: Views
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
- Class: rviz/Time
|
|
||||||
Experimental: false
|
|
||||||
Name: Time
|
|
||||||
SyncMode: 0
|
|
||||||
SyncSource: ""
|
|
||||||
Preferences:
|
|
||||||
PromptSaveOnExit: true
|
|
||||||
Toolbars:
|
|
||||||
toolButtonStyle: 2
|
|
||||||
Visualization Manager:
|
|
||||||
Class: ""
|
|
||||||
Displays:
|
|
||||||
- Alpha: 0.5
|
|
||||||
Cell Size: 0.25
|
|
||||||
Class: rviz/Grid
|
|
||||||
Color: 160; 160; 164
|
|
||||||
Enabled: true
|
|
||||||
Line Style:
|
|
||||||
Line Width: 0.029999999329447746
|
|
||||||
Value: Lines
|
|
||||||
Name: Grid
|
|
||||||
Normal Cell Count: 0
|
|
||||||
Offset:
|
|
||||||
X: 0
|
|
||||||
Y: 0
|
|
||||||
Z: 0
|
|
||||||
Plane: XY
|
|
||||||
Plane Cell Count: 10
|
|
||||||
Reference Frame: <Fixed Frame>
|
|
||||||
Value: true
|
|
||||||
- Class: rviz/TF
|
|
||||||
Enabled: true
|
|
||||||
Frame Timeout: 15
|
|
||||||
Frames:
|
|
||||||
All Enabled: true
|
|
||||||
master/sgr532/ar_tag_link:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/base_link:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link1:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link2:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link3:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link4:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link5:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link6:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link_grasping_frame:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link_gripper_left:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link_gripper_right:
|
|
||||||
Value: true
|
|
||||||
master/sgr532/usb_cam_link:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/ar_tag_link:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/base_link:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link1:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link2:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link3:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link4:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link5:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link6:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link_grasping_frame:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link_gripper_left:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link_gripper_right:
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/usb_cam_link:
|
|
||||||
Value: true
|
|
||||||
world:
|
|
||||||
Value: true
|
|
||||||
Marker Alpha: 1
|
|
||||||
Marker Scale: 0.25
|
|
||||||
Name: TF
|
|
||||||
Show Arrows: true
|
|
||||||
Show Axes: true
|
|
||||||
Show Names: false
|
|
||||||
Tree:
|
|
||||||
world:
|
|
||||||
master/sgr532/base_link:
|
|
||||||
master/sgr532/link1:
|
|
||||||
master/sgr532/link2:
|
|
||||||
master/sgr532/link3:
|
|
||||||
master/sgr532/link4:
|
|
||||||
master/sgr532/link5:
|
|
||||||
master/sgr532/link6:
|
|
||||||
master/sgr532/ar_tag_link:
|
|
||||||
{}
|
|
||||||
master/sgr532/link_grasping_frame:
|
|
||||||
{}
|
|
||||||
master/sgr532/link_gripper_left:
|
|
||||||
{}
|
|
||||||
master/sgr532/link_gripper_right:
|
|
||||||
{}
|
|
||||||
master/sgr532/usb_cam_link:
|
|
||||||
{}
|
|
||||||
slaver1/sgr532/base_link:
|
|
||||||
slaver1/sgr532/link1:
|
|
||||||
slaver1/sgr532/link2:
|
|
||||||
slaver1/sgr532/link3:
|
|
||||||
slaver1/sgr532/link4:
|
|
||||||
slaver1/sgr532/link5:
|
|
||||||
slaver1/sgr532/link6:
|
|
||||||
slaver1/sgr532/ar_tag_link:
|
|
||||||
{}
|
|
||||||
slaver1/sgr532/link_grasping_frame:
|
|
||||||
{}
|
|
||||||
slaver1/sgr532/link_gripper_left:
|
|
||||||
{}
|
|
||||||
slaver1/sgr532/link_gripper_right:
|
|
||||||
{}
|
|
||||||
slaver1/sgr532/usb_cam_link:
|
|
||||||
{}
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
- Alpha: 1
|
|
||||||
Class: rviz/RobotModel
|
|
||||||
Collision Enabled: false
|
|
||||||
Enabled: true
|
|
||||||
Links:
|
|
||||||
All Links Enabled: true
|
|
||||||
Expand Joint Details: false
|
|
||||||
Expand Link Details: false
|
|
||||||
Expand Tree: false
|
|
||||||
Link Tree Style: Links in Alphabetic Order
|
|
||||||
slaver1/sgr532/ar_tag_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
slaver1/sgr532/base_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link1:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link2:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link3:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link4:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link5:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link6:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link_grasping_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
slaver1/sgr532/link_gripper_left:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/link_gripper_right:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
slaver1/sgr532/usb_cam_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
world:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: slaver1/sgr532/robot_description
|
|
||||||
TF Prefix: ""
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
Visual Enabled: true
|
|
||||||
- Alpha: 1
|
|
||||||
Class: rviz/RobotModel
|
|
||||||
Collision Enabled: false
|
|
||||||
Enabled: true
|
|
||||||
Links:
|
|
||||||
All Links Enabled: true
|
|
||||||
Expand Joint Details: false
|
|
||||||
Expand Link Details: false
|
|
||||||
Expand Tree: false
|
|
||||||
Link Tree Style: Links in Alphabetic Order
|
|
||||||
master/sgr532/ar_tag_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
master/sgr532/base_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link1:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link2:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link3:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link4:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link5:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link6:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link_grasping_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
master/sgr532/link_gripper_left:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
master/sgr532/link_gripper_right:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
master/sgr532/usb_cam_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
world:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: master/sgr532/robot_description
|
|
||||||
TF Prefix: ""
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
Visual Enabled: true
|
|
||||||
Enabled: true
|
|
||||||
Global Options:
|
|
||||||
Background Color: 40; 40; 121
|
|
||||||
Default Light: true
|
|
||||||
Fixed Frame: world
|
|
||||||
Frame Rate: 30
|
|
||||||
Name: root
|
|
||||||
Tools:
|
|
||||||
- Class: rviz/Interact
|
|
||||||
Hide Inactive Objects: true
|
|
||||||
- Class: rviz/MoveCamera
|
|
||||||
- Class: rviz/Select
|
|
||||||
- Class: rviz/FocusCamera
|
|
||||||
- Class: rviz/Measure
|
|
||||||
- Class: rviz/SetInitialPose
|
|
||||||
Theta std deviation: 0.2617993950843811
|
|
||||||
Topic: /initialpose
|
|
||||||
X std deviation: 0.5
|
|
||||||
Y std deviation: 0.5
|
|
||||||
- Class: rviz/SetGoal
|
|
||||||
Topic: /move_base_simple/goal
|
|
||||||
- Class: rviz/PublishPoint
|
|
||||||
Single click: true
|
|
||||||
Topic: /clicked_point
|
|
||||||
Value: true
|
|
||||||
Views:
|
|
||||||
Current:
|
|
||||||
Class: rviz/Orbit
|
|
||||||
Distance: 1.5
|
|
||||||
Enable Stereo Rendering:
|
|
||||||
Stereo Eye Separation: 0.05999999865889549
|
|
||||||
Stereo Focal Distance: 1
|
|
||||||
Swap Stereo Eyes: false
|
|
||||||
Value: false
|
|
||||||
Field of View: 0.7853981852531433
|
|
||||||
Focal Point:
|
|
||||||
X: 0
|
|
||||||
Y: 0
|
|
||||||
Z: 0
|
|
||||||
Focal Shape Fixed Size: false
|
|
||||||
Focal Shape Size: 0.05000000074505806
|
|
||||||
Invert Z Axis: false
|
|
||||||
Name: Current View
|
|
||||||
Near Clip Distance: 0.009999999776482582
|
|
||||||
Pitch: 0.5253986120223999
|
|
||||||
Target Frame: <Fixed Frame>
|
|
||||||
Yaw: 0.22039595246315002
|
|
||||||
Saved: ~
|
|
||||||
Window Geometry:
|
|
||||||
Displays:
|
|
||||||
collapsed: false
|
|
||||||
Height: 836
|
|
||||||
Hide Left Dock: false
|
|
||||||
Hide Right Dock: true
|
|
||||||
QMainWindow State: 000000ff00000000fd0000000400000000000001ee000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005580000003efc0100000002fb0000000800540069006d0065010000000000000558000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000364000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
|
||||||
Selection:
|
|
||||||
collapsed: false
|
|
||||||
Time:
|
|
||||||
collapsed: false
|
|
||||||
Tool Properties:
|
|
||||||
collapsed: false
|
|
||||||
Views:
|
|
||||||
collapsed: true
|
|
||||||
Width: 1368
|
|
||||||
X: 72
|
|
||||||
Y: 27
|
|
||||||
|
|
@ -1,7 +0,0 @@
|
||||||
#!/bin/bash
|
|
||||||
|
|
||||||
BASEPATH=$(cd `dirname $0`; pwd)
|
|
||||||
echo $BASEPATH
|
|
||||||
sleep 5
|
|
||||||
gnome-terminal -x bash -c "rosbag play $BASEPATH/../bag/record1.bag"
|
|
||||||
|
|
||||||
|
|
@ -1,110 +0,0 @@
|
||||||
/*
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
* Copyright (c) 2022, NXROBO.
|
|
||||||
* All rights reserved.
|
|
||||||
* Author: litian.zhuang <litian.zhuang@nxrobo.com>
|
|
||||||
*/
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <std_msgs/String.h>
|
|
||||||
|
|
||||||
#include <sensor_msgs/JointState.h>
|
|
||||||
#include <std_msgs/Float64.h>
|
|
||||||
#include <std_srvs/Empty.h>
|
|
||||||
#include <sdk_sagittarius_arm/ArmRadControl.h>
|
|
||||||
#include "sdk_sagittarius_arm/ArmInfo.h"
|
|
||||||
|
|
||||||
sensor_msgs::JointState joint_states;
|
|
||||||
|
|
||||||
/// @brief Joint state callback function
|
|
||||||
/// @param msg - updated joint states
|
|
||||||
void joint_state_cb(const sensor_msgs::JointState &msg)
|
|
||||||
{
|
|
||||||
joint_states = msg;
|
|
||||||
}
|
|
||||||
#define MAX_SLAVER_ROBOT_NUM 5
|
|
||||||
int main(int argc, char **argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "sagittarius_puppet_control");
|
|
||||||
ros::NodeHandle n;
|
|
||||||
int slaver_robot_num = 1;
|
|
||||||
int i;
|
|
||||||
ros::Publisher pub_positions[MAX_SLAVER_ROBOT_NUM];
|
|
||||||
ros::Publisher pub_gripper[MAX_SLAVER_ROBOT_NUM];
|
|
||||||
std::string robot_name_master, robot_name_slaver1,robot_name_slaver[MAX_SLAVER_ROBOT_NUM];
|
|
||||||
ros::param::get("~robot_name_master", robot_name_master);
|
|
||||||
ros::param::get("~slaver_robot_num", slaver_robot_num);
|
|
||||||
if(slaver_robot_num > MAX_SLAVER_ROBOT_NUM)
|
|
||||||
{
|
|
||||||
ROS_ERROR("arm number must be not more than 5");
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
for(i=0; i<slaver_robot_num; i++)
|
|
||||||
{
|
|
||||||
ros::param::get("~robot_name_slaver"+std::to_string(i+1), robot_name_slaver[i]);
|
|
||||||
pub_positions[i] = n.advertise<sdk_sagittarius_arm::ArmRadControl>(robot_name_slaver[i] + "/joint/commands", 100);
|
|
||||||
pub_gripper[i] = n.advertise<std_msgs::Float64>(robot_name_slaver[i] + "/gripper/command", 100);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
// 定阅主机械臂的关节角度
|
|
||||||
ros::Subscriber sub_positions = n.subscribe(robot_name_master + "/joint_states", 100, joint_state_cb);
|
|
||||||
ros::Publisher pub_torque = n.advertise<std_msgs::String>(robot_name_master + "/control_torque", 1);
|
|
||||||
// 释放主机械臂的锁舵功能
|
|
||||||
ros::ServiceClient srv_robot_info = n.serviceClient<sdk_sagittarius_arm::ArmInfo>(robot_name_master + "/get_robot_info");
|
|
||||||
ros::Rate loop_rate(100);
|
|
||||||
bool success;
|
|
||||||
|
|
||||||
// 等待SDK节点开始订阅后，才开始发送。
|
|
||||||
while ((pub_positions[slaver_robot_num-1].getNumSubscribers() < 1 || joint_states.position.size() < 1) && ros::ok())
|
|
||||||
{
|
|
||||||
ros::spinOnce();
|
|
||||||
loop_rate.sleep();
|
|
||||||
}
|
|
||||||
while ((pub_torque.getNumSubscribers() < 1) && ros::ok())
|
|
||||||
{
|
|
||||||
ros::spinOnce();
|
|
||||||
loop_rate.sleep();
|
|
||||||
}
|
|
||||||
std_msgs::String msg;
|
|
||||||
msg.data = "close";
|
|
||||||
pub_torque.publish(msg);
|
|
||||||
|
|
||||||
|
|
||||||
// 获取机械臂的关节数
|
|
||||||
sdk_sagittarius_arm::ArmInfo robot_info_srv;
|
|
||||||
success = srv_robot_info.call(robot_info_srv);
|
|
||||||
if (!success)
|
|
||||||
{
|
|
||||||
ROS_ERROR("Could not get robot info.");
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
size_t cntr = 0;
|
|
||||||
while (ros::ok())
|
|
||||||
{
|
|
||||||
sdk_sagittarius_arm::ArmRadControl pos_msg;
|
|
||||||
for (size_t i{0}; i < robot_info_srv.response.num_joints; i++)
|
|
||||||
{
|
|
||||||
pos_msg.rad.push_back(joint_states.position.at(i));
|
|
||||||
}
|
|
||||||
for(i=0; i<slaver_robot_num; i++)
|
|
||||||
{
|
|
||||||
pub_positions[i].publish(pos_msg);
|
|
||||||
}
|
|
||||||
if (cntr == 5)
|
|
||||||
{
|
|
||||||
std_msgs::Float64 gpr_msg;
|
|
||||||
gpr_msg.data = joint_states.position.at(joint_states.position.size()-2);
|
|
||||||
for(i=0; i<slaver_robot_num; i++)
|
|
||||||
{
|
|
||||||
pub_gripper[i].publish(gpr_msg);
|
|
||||||
}
|
|
||||||
cntr = 0;
|
|
||||||
}
|
|
||||||
cntr++;
|
|
||||||
ros::spinOnce();
|
|
||||||
loop_rate.sleep();
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
@ -1,85 +0,0 @@
|
||||||
/*
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
* Copyright (c) 2021, NXROBO.
|
|
||||||
* All rights reserved.
|
|
||||||
* Author: litian.zhuang <litian.zhuang@nxrobo.com>
|
|
||||||
*/
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include "sdk_sagittarius_arm/ArmRadControl.h"
|
|
||||||
#include "sdk_sagittarius_arm/ArmInfo.h"
|
|
||||||
#include <sensor_msgs/JointState.h>
|
|
||||||
#include <std_msgs/Float64.h>
|
|
||||||
#include <std_msgs/String.h>
|
|
||||||
#include <std_srvs/Empty.h>
|
|
||||||
#include <string>
|
|
||||||
sensor_msgs::JointState joint_states;
|
|
||||||
|
|
||||||
/// @brief Joint state callback function
|
|
||||||
/// @param msg - updated joint states
|
|
||||||
void joint_state_cb(const sensor_msgs::JointState &msg)
|
|
||||||
{
|
|
||||||
joint_states = msg;
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "sagittarius_puppet_control_single");
|
|
||||||
ros::NodeHandle n;
|
|
||||||
|
|
||||||
// 定阅主机械臂的关节角度
|
|
||||||
ros::Subscriber sub_positions = n.subscribe("joint_states", 100, joint_state_cb);
|
|
||||||
ros::Publisher pub_positions = n.advertise<sdk_sagittarius_arm::ArmRadControl>("joint/commands", 100);
|
|
||||||
ros::Publisher pub_gripper = n.advertise<std_msgs::Float64>("gripper/command", 100);
|
|
||||||
ros::ServiceClient srv_robot_info = n.serviceClient<sdk_sagittarius_arm::ArmInfo>("get_robot_info");
|
|
||||||
// 释放主机械臂的锁舵功能
|
|
||||||
ros::Publisher pub_torque = n.advertise<std_msgs::String>("control_torque", 1);
|
|
||||||
ros::Rate loop_rate(100);
|
|
||||||
bool success;
|
|
||||||
|
|
||||||
// 等待SDK节点开始订阅后，才开始发送。
|
|
||||||
while ((pub_positions.getNumSubscribers() < 1 || joint_states.position.size() < 1) && ros::ok())
|
|
||||||
{
|
|
||||||
ros::spinOnce();
|
|
||||||
loop_rate.sleep();
|
|
||||||
}
|
|
||||||
while ((pub_torque.getNumSubscribers() < 1) && ros::ok())
|
|
||||||
{
|
|
||||||
ros::spinOnce();
|
|
||||||
loop_rate.sleep();
|
|
||||||
}
|
|
||||||
std_msgs::String msg;
|
|
||||||
msg.data = "close";
|
|
||||||
pub_torque.publish(msg);
|
|
||||||
|
|
||||||
// 获取机械臂的关节数
|
|
||||||
sdk_sagittarius_arm::ArmInfo robot_info_srv;
|
|
||||||
success = srv_robot_info.call(robot_info_srv);
|
|
||||||
if (!success)
|
|
||||||
{
|
|
||||||
ROS_ERROR("Could not get robot info.");
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
size_t cntr = 0;
|
|
||||||
while (ros::ok())
|
|
||||||
{
|
|
||||||
sdk_sagittarius_arm::ArmRadControl pos_msg;
|
|
||||||
for (size_t i{0}; i < robot_info_srv.response.num_joints; i++)
|
|
||||||
{
|
|
||||||
pos_msg.rad.push_back(joint_states.position.at(i));
|
|
||||||
}
|
|
||||||
pub_positions.publish(pos_msg);
|
|
||||||
|
|
||||||
if (cntr == 10)
|
|
||||||
{
|
|
||||||
std_msgs::Float64 gpr_msg;
|
|
||||||
gpr_msg.data = joint_states.position.at(joint_states.position.size()-1);
|
|
||||||
pub_gripper.publish(gpr_msg);
|
|
||||||
cntr = 0;
|
|
||||||
}
|
|
||||||
cntr++;
|
|
||||||
ros::spinOnce();
|
|
||||||
loop_rate.sleep();
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
@ -1,14 +0,0 @@
|
||||||
cmake_minimum_required(VERSION 2.8.3)
|
|
||||||
|
|
||||||
project(sagittarius_descriptions)
|
|
||||||
|
|
||||||
find_package(catkin REQUIRED)
|
|
||||||
|
|
||||||
catkin_package()
|
|
||||||
|
|
||||||
find_package(roslaunch)
|
|
||||||
|
|
||||||
foreach(dir config launch meshes urdf)
|
|
||||||
install(DIRECTORY ${dir}/
|
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
|
|
||||||
endforeach(dir)
|
|
||||||
|
|
@ -1 +0,0 @@
|
||||||
controller_joint_names: ['', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint_gripper_left', 'joint_gripper_right', ]
|
|
||||||
|
|
@ -1,54 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default="$(arg robot_name)"/>
|
|
||||||
<arg name="use_world_frame" default="true"/>
|
|
||||||
<arg name="external_urdf_loc" default=""/>
|
|
||||||
<arg name="load_gazebo_configs" default="false"/>
|
|
||||||
<arg name="joint_pub_gui" default="true"/>
|
|
||||||
<arg name="use_joint_pub" default="false"/>
|
|
||||||
<arg name="use_default_rviz" default="true"/>
|
|
||||||
<arg name="rvizconfig" default="$(find sagittarius_descriptions)/rviz/description.rviz" />
|
|
||||||
<arg name="model" default="$(find sagittarius_descriptions)/urdf/$(arg robot_model).urdf.xacro
|
|
||||||
robot_name:=$(arg robot_name)
|
|
||||||
use_world_frame:=$(arg use_world_frame)
|
|
||||||
external_urdf_loc:=$(arg external_urdf_loc)
|
|
||||||
load_gazebo_configs:=$(arg load_gazebo_configs)"/>
|
|
||||||
|
|
||||||
<param name="$(arg robot_name)/robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
<node if="$(arg joint_pub_gui)"
|
|
||||||
name="joint_state_publisher_gui"
|
|
||||||
pkg="joint_state_publisher_gui"
|
|
||||||
type="joint_state_publisher_gui"
|
|
||||||
ns="$(arg robot_name)">
|
|
||||||
<rosparam param="source_list">[sagittarius_joint_states]</rosparam>
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<node if="$(arg use_joint_pub)"
|
|
||||||
name="joint_state_publisher"
|
|
||||||
pkg="joint_state_publisher"
|
|
||||||
type="joint_state_publisher"
|
|
||||||
ns="$(arg robot_name)">
|
|
||||||
<rosparam param="source_list">[sagittarius_joint_states]</rosparam>
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="robot_state_publisher"
|
|
||||||
pkg="robot_state_publisher"
|
|
||||||
type="robot_state_publisher"
|
|
||||||
ns="$(arg robot_name)"/>
|
|
||||||
|
|
||||||
<node if="$(arg use_default_rviz)"
|
|
||||||
name="rviz"
|
|
||||||
pkg="rviz"
|
|
||||||
type="rviz"
|
|
||||||
args="-d $(arg rvizconfig)"
|
|
||||||
ns="$(arg robot_name)"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,20 +0,0 @@
|
||||||
<launch>
|
|
||||||
<arg
|
|
||||||
name="model" />
|
|
||||||
<param
|
|
||||||
name="robot_description"
|
|
||||||
textfile="$(find sagittarius_descriptions)/urdf/sagittarius_descriptions.urdf" />
|
|
||||||
<node
|
|
||||||
name="joint_state_publisher_gui"
|
|
||||||
pkg="joint_state_publisher_gui"
|
|
||||||
type="joint_state_publisher_gui" />
|
|
||||||
<node
|
|
||||||
name="robot_state_publisher"
|
|
||||||
pkg="robot_state_publisher"
|
|
||||||
type="robot_state_publisher" />
|
|
||||||
<node
|
|
||||||
name="rviz"
|
|
||||||
pkg="rviz"
|
|
||||||
type="rviz"
|
|
||||||
args="-d $(find sagittarius_descriptions)/urdf.rviz" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,20 +0,0 @@
|
||||||
<launch>
|
|
||||||
<include
|
|
||||||
file="$(find gazebo_ros)/launch/empty_world.launch" />
|
|
||||||
<node
|
|
||||||
name="tf_footprint_base"
|
|
||||||
pkg="tf"
|
|
||||||
type="static_transform_publisher"
|
|
||||||
args="0 0 0 0 0 0 base_link base_footprint 40" />
|
|
||||||
<node
|
|
||||||
name="spawn_model"
|
|
||||||
pkg="gazebo_ros"
|
|
||||||
type="spawn_model"
|
|
||||||
args="-file $(find sagittarius_descriptions)/urdf/sagittarius_descriptions.urdf -urdf -model sagittarius_descriptions"
|
|
||||||
output="screen" />
|
|
||||||
<node
|
|
||||||
name="fake_joint_calibration"
|
|
||||||
pkg="rostopic"
|
|
||||||
type="rostopic"
|
|
||||||
args="pub /calibrated std_msgs/Bool true" />
|
|
||||||
</launch>
|
|
||||||
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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,21 +0,0 @@
|
||||||
<package format="2">
|
|
||||||
<name>sagittarius_descriptions</name>
|
|
||||||
<version>1.0.0</version>
|
|
||||||
<description>
|
|
||||||
<p>URDF Description package for sagittarius_descriptions</p>
|
|
||||||
<p>This package contains configuration data, 3D models and launch files
|
|
||||||
for sagittarius_descriptions robot</p>
|
|
||||||
</description>
|
|
||||||
<author>litian.zhuang</author>
|
|
||||||
<maintainer email="litian.zhuang@nxrobo.com" />
|
|
||||||
<license>BSD</license>
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
|
||||||
<depend>roslaunch</depend>
|
|
||||||
<depend>robot_state_publisher</depend>
|
|
||||||
<depend>rviz</depend>
|
|
||||||
<depend>joint_state_publisher_gui</depend>
|
|
||||||
<depend>gazebo</depend>
|
|
||||||
<export>
|
|
||||||
<architecture_independent />
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
|
|
@ -1,192 +0,0 @@
|
||||||
Panels:
|
|
||||||
- Class: rviz/Displays
|
|
||||||
Help Height: 78
|
|
||||||
Name: Displays
|
|
||||||
Property Tree Widget:
|
|
||||||
Expanded:
|
|
||||||
- /RobotModel1/Status1
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
Tree Height: 539
|
|
||||||
- Class: rviz/Selection
|
|
||||||
Name: Selection
|
|
||||||
- Class: rviz/Tool Properties
|
|
||||||
Expanded:
|
|
||||||
- /2D Pose Estimate1
|
|
||||||
- /2D Nav Goal1
|
|
||||||
- /Publish Point1
|
|
||||||
Name: Tool Properties
|
|
||||||
Splitter Ratio: 0.5886790156364441
|
|
||||||
- Class: rviz/Views
|
|
||||||
Expanded:
|
|
||||||
- /Current View1
|
|
||||||
Name: Views
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
- Class: rviz/Time
|
|
||||||
Experimental: false
|
|
||||||
Name: Time
|
|
||||||
SyncMode: 0
|
|
||||||
SyncSource: ""
|
|
||||||
Preferences:
|
|
||||||
PromptSaveOnExit: true
|
|
||||||
Toolbars:
|
|
||||||
toolButtonStyle: 2
|
|
||||||
Visualization Manager:
|
|
||||||
Class: ""
|
|
||||||
Displays:
|
|
||||||
- Alpha: 0.5
|
|
||||||
Cell Size: 1
|
|
||||||
Class: rviz/Grid
|
|
||||||
Color: 160; 160; 164
|
|
||||||
Enabled: true
|
|
||||||
Line Style:
|
|
||||||
Line Width: 0.029999999329447746
|
|
||||||
Value: Lines
|
|
||||||
Name: Grid
|
|
||||||
Normal Cell Count: 0
|
|
||||||
Offset:
|
|
||||||
X: 0
|
|
||||||
Y: 0
|
|
||||||
Z: 0
|
|
||||||
Plane: XY
|
|
||||||
Plane Cell Count: 10
|
|
||||||
Reference Frame: world
|
|
||||||
Value: true
|
|
||||||
- Alpha: 1
|
|
||||||
Class: rviz/RobotModel
|
|
||||||
Collision Enabled: false
|
|
||||||
Enabled: true
|
|
||||||
Links:
|
|
||||||
All Links Enabled: true
|
|
||||||
Expand Joint Details: false
|
|
||||||
Expand Link Details: false
|
|
||||||
Expand Tree: false
|
|
||||||
Link Tree Style: Links in Alphabetic Order
|
|
||||||
sgr532/ar_tag_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
sgr532/base_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link1:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link2:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link3:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link4:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link5:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link6:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link_grasping_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
sgr532/link_gripper_left:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
sgr532/link_gripper_right:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
world:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: /sgr532/robot_description
|
|
||||||
TF Prefix: ""
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
Visual Enabled: true
|
|
||||||
Enabled: true
|
|
||||||
Global Options:
|
|
||||||
Background Color: 48; 48; 48
|
|
||||||
Default Light: true
|
|
||||||
Fixed Frame: sgr532/base_link
|
|
||||||
Frame Rate: 30
|
|
||||||
Name: root
|
|
||||||
Tools:
|
|
||||||
- Class: rviz/Interact
|
|
||||||
Hide Inactive Objects: true
|
|
||||||
- Class: rviz/MoveCamera
|
|
||||||
- Class: rviz/Select
|
|
||||||
- Class: rviz/FocusCamera
|
|
||||||
- Class: rviz/Measure
|
|
||||||
- Class: rviz/SetInitialPose
|
|
||||||
Theta std deviation: 0.2617993950843811
|
|
||||||
Topic: /initialpose
|
|
||||||
X std deviation: 0.5
|
|
||||||
Y std deviation: 0.5
|
|
||||||
- Class: rviz/SetGoal
|
|
||||||
Topic: /move_base_simple/goal
|
|
||||||
- Class: rviz/PublishPoint
|
|
||||||
Single click: true
|
|
||||||
Topic: /clicked_point
|
|
||||||
Value: true
|
|
||||||
Views:
|
|
||||||
Current:
|
|
||||||
Class: rviz/Orbit
|
|
||||||
Distance: 2.4508087635040283
|
|
||||||
Enable Stereo Rendering:
|
|
||||||
Stereo Eye Separation: 0.05999999865889549
|
|
||||||
Stereo Focal Distance: 1
|
|
||||||
Swap Stereo Eyes: false
|
|
||||||
Value: false
|
|
||||||
Field of View: 0.7853981852531433
|
|
||||||
Focal Point:
|
|
||||||
X: 0
|
|
||||||
Y: 0
|
|
||||||
Z: 0
|
|
||||||
Focal Shape Fixed Size: true
|
|
||||||
Focal Shape Size: 0.05000000074505806
|
|
||||||
Invert Z Axis: false
|
|
||||||
Name: Current View
|
|
||||||
Near Clip Distance: 0.009999999776482582
|
|
||||||
Pitch: 0.2503983974456787
|
|
||||||
Target Frame: <Fixed Frame>
|
|
||||||
Yaw: 1.577213168144226
|
|
||||||
Saved: ~
|
|
||||||
Window Geometry:
|
|
||||||
Displays:
|
|
||||||
collapsed: false
|
|
||||||
Height: 836
|
|
||||||
Hide Left Dock: false
|
|
||||||
Hide Right Dock: false
|
|
||||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005580000003efc0100000002fb0000000800540069006d0065010000000000000558000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e7000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
|
||||||
Selection:
|
|
||||||
collapsed: false
|
|
||||||
Time:
|
|
||||||
collapsed: false
|
|
||||||
Tool Properties:
|
|
||||||
collapsed: false
|
|
||||||
Views:
|
|
||||||
collapsed: false
|
|
||||||
Width: 1368
|
|
||||||
X: 72
|
|
||||||
Y: 27
|
|
||||||
|
|
@ -1,180 +0,0 @@
|
||||||
Panels:
|
|
||||||
- Class: rviz/Displays
|
|
||||||
Help Height: 78
|
|
||||||
Name: Displays
|
|
||||||
Property Tree Widget:
|
|
||||||
Expanded:
|
|
||||||
- /Global Options1
|
|
||||||
- /Status1
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
Tree Height: 557
|
|
||||||
- Class: rviz/Selection
|
|
||||||
Name: Selection
|
|
||||||
- Class: rviz/Tool Properties
|
|
||||||
Expanded:
|
|
||||||
- /2D Pose Estimate1
|
|
||||||
- /2D Nav Goal1
|
|
||||||
- /Publish Point1
|
|
||||||
Name: Tool Properties
|
|
||||||
Splitter Ratio: 0.588679016
|
|
||||||
- Class: rviz/Views
|
|
||||||
Expanded:
|
|
||||||
- /Current View1
|
|
||||||
Name: Views
|
|
||||||
Splitter Ratio: 0.5
|
|
||||||
- Class: rviz/Time
|
|
||||||
Experimental: false
|
|
||||||
Name: Time
|
|
||||||
SyncMode: 0
|
|
||||||
SyncSource: ""
|
|
||||||
Toolbars:
|
|
||||||
toolButtonStyle: 2
|
|
||||||
Visualization Manager:
|
|
||||||
Class: ""
|
|
||||||
Displays:
|
|
||||||
- Alpha: 0.5
|
|
||||||
Cell Size: 1
|
|
||||||
Class: rviz/Grid
|
|
||||||
Color: 160; 160; 164
|
|
||||||
Enabled: true
|
|
||||||
Line Style:
|
|
||||||
Line Width: 0.0299999993
|
|
||||||
Value: Lines
|
|
||||||
Name: Grid
|
|
||||||
Normal Cell Count: 0
|
|
||||||
Offset:
|
|
||||||
X: 0
|
|
||||||
Y: 0
|
|
||||||
Z: 0
|
|
||||||
Plane: XY
|
|
||||||
Plane Cell Count: 10
|
|
||||||
Reference Frame: <Fixed Frame>
|
|
||||||
Value: true
|
|
||||||
- Alpha: 1
|
|
||||||
Class: rviz/RobotModel
|
|
||||||
Collision Enabled: false
|
|
||||||
Enabled: true
|
|
||||||
Links:
|
|
||||||
All Links Enabled: true
|
|
||||||
Expand Joint Details: false
|
|
||||||
Expand Link Details: false
|
|
||||||
Expand Tree: false
|
|
||||||
Link Tree Style: Links in Alphabetic Order
|
|
||||||
link1:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
link2:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
link3:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
link4:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
link5:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
link6:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
link_grasping_frame:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
link_gripper_left:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
link_gripper_right:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
base_link:
|
|
||||||
Alpha: 1
|
|
||||||
Show Axes: false
|
|
||||||
Show Trail: false
|
|
||||||
Value: true
|
|
||||||
Name: RobotModel
|
|
||||||
Robot Description: robot_description
|
|
||||||
TF Prefix: ""
|
|
||||||
Update Interval: 0
|
|
||||||
Value: true
|
|
||||||
Visual Enabled: true
|
|
||||||
Enabled: true
|
|
||||||
Global Options:
|
|
||||||
Background Color: 48; 48; 48
|
|
||||||
Default Light: true
|
|
||||||
Fixed Frame: base_link
|
|
||||||
Frame Rate: 30
|
|
||||||
Name: root
|
|
||||||
Tools:
|
|
||||||
- Class: rviz/Interact
|
|
||||||
Hide Inactive Objects: true
|
|
||||||
- Class: rviz/MoveCamera
|
|
||||||
- Class: rviz/Select
|
|
||||||
- Class: rviz/FocusCamera
|
|
||||||
- Class: rviz/Measure
|
|
||||||
- Class: rviz/SetInitialPose
|
|
||||||
Topic: /initialpose
|
|
||||||
- Class: rviz/SetGoal
|
|
||||||
Topic: /move_base_simple/goal
|
|
||||||
- Class: rviz/PublishPoint
|
|
||||||
Single click: true
|
|
||||||
Topic: /clicked_point
|
|
||||||
Value: true
|
|
||||||
Views:
|
|
||||||
Current:
|
|
||||||
Class: rviz/Orbit
|
|
||||||
Distance: 10
|
|
||||||
Enable Stereo Rendering:
|
|
||||||
Stereo Eye Separation: 0.0599999987
|
|
||||||
Stereo Focal Distance: 1
|
|
||||||
Swap Stereo Eyes: false
|
|
||||||
Value: false
|
|
||||||
Focal Point:
|
|
||||||
X: 0
|
|
||||||
Y: 0
|
|
||||||
Z: 0
|
|
||||||
Focal Shape Fixed Size: true
|
|
||||||
Focal Shape Size: 0.0500000007
|
|
||||||
Invert Z Axis: false
|
|
||||||
Name: Current View
|
|
||||||
Near Clip Distance: 0.00999999978
|
|
||||||
Pitch: 0.785398006
|
|
||||||
Target Frame: <Fixed Frame>
|
|
||||||
Value: Orbit (rviz)
|
|
||||||
Yaw: 0.785398006
|
|
||||||
Saved: ~
|
|
||||||
Window Geometry:
|
|
||||||
Displays:
|
|
||||||
collapsed: false
|
|
||||||
Height: 846
|
|
||||||
Hide Left Dock: false
|
|
||||||
Hide Right Dock: true
|
|
||||||
QMainWindow State: 000000ff00000000fd000000040000000000000171000002bffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002b000002bf000000da00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002b000002bf000000b200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000035200fffffffb0000000800540069006d0065010000000000000450000000000000000000000339000002bf00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
|
||||||
Selection:
|
|
||||||
collapsed: false
|
|
||||||
Time:
|
|
||||||
collapsed: false
|
|
||||||
Tool Properties:
|
|
||||||
collapsed: false
|
|
||||||
Views:
|
|
||||||
collapsed: true
|
|
||||||
Width: 1200
|
|
||||||
X: 73
|
|
||||||
Y: 60
|
|
||||||
|
|
@ -1,11 +0,0 @@
|
||||||
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
|
|
||||||
sagittarius_base_link,0.00793179544277737,2.48115460577673E-19,0.0312047945759253,0,0,0,0.940146705073746,0.000950195835062344,3.14825974312405E-20,-0.000200082473207336,0.00267650790326047,-1.01412991222915E-19,0.002902663900239,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/sagittarius_base_link.STL,1,1,1,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/sagittarius_base_link.STL,,A_ASM_5_ASM^sagittarius_descriptions-1/1_5^A_ASM_5_ASM_sagittarius_descriptions-1,坐标系9,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
|
|
||||||
link1,-7.74579608786574E-10,3.23354786979222E-12,0.0344870497564283,0,0,0,0.123745079127031,8.15779591743416E-05,-4.25325094802094E-15,-2.22201794150093E-12,6.7607001964135E-05,-1.59127137655435E-14,4.41297902395382E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link1.STL,1,1,1,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link1.STL,,1^sagittarius_descriptions-1/2_2^1_sagittarius_descriptions-1,坐标系1,基准轴1,joint1,revolute,0.045,0,0.07,0,0,0,sagittarius_base_link,0,0,-1,0,0,-2,2,,,,,,,,
|
|
||||||
link2,0.0119015312584793,-1.03639845764692E-08,0.150755903867182,0,0,0,0.156160822791157,0.000429266751802994,-2.27426165787001E-11,-5.58862834004722E-05,0.000444676097748043,-2.41993274420629E-10,7.916162292894E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link2.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link2.STL,,2^sagittarius_descriptions-1,坐标系2,基准轴2,joint2,revolute,0,-0.0001,0.055,0,0,0,link1,0,1,0,0,0,-1.4,1.57,,,,,,,,
|
|
||||||
link3,0.054357321065004,-2.67256405253992E-08,0.00064594073830132,0,0,0,0.0366487313200473,9.28701176876329E-06,-2.23878504272996E-11,6.34310578103811E-07,2.0727596617309E-05,-1.71647322080315E-11,2.42416271089979E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link3.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link3.STL,,3^sagittarius_descriptions-1,坐标系3,基准轴3,joint3,revolute,0.033,-0.0001,0.179,0,0,0,link2,0,1,0,0,0,-1.8,1.57,,,,,,,,
|
|
||||||
link4,0.0444029485674779,-0.00527865310158246,-1.43738021485262E-10,0,0,0,0.124950088011432,3.52699895078887E-05,-1.14870103744551E-05,2.45948329713632E-14,0.00010928557067047,-3.42525315676607E-12,0.000121451732690361,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link4.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link4.STL,,4^sagittarius_descriptions-1,坐标系4,基准轴4,joint4,revolute,0.0845,0,0,0,0,0,link3,1,0,0,0,0,-2.62,2.62,,,,,,,,
|
|
||||||
link5,0.0391172635125616,1.22102848665318E-15,0.011359922796391,0,0,0,0.0564710099602834,1.77667699918498E-05,3.77572698012408E-19,-6.79618041712231E-07,1.89643063051294E-05,1.78381670331667E-19,1.66130972969903E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link5.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link5.STL,,5^sagittarius_descriptions-1,坐标系5,基准轴5,joint5,revolute,0.08435,0.00065,0,0,0,0,link4,0,1,0,0,0,-1.57,1.57,,,,,,,,
|
|
||||||
link6,0.0330748503556987,-1.47583477394259E-10,0.00874151659437283,0,0,0,0.0790875358325134,4.00727396068743E-05,1.5026227935429E-13,6.77337873337555E-06,4.37063090227749E-05,-1.73263828329941E-13,5.69755186998711E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link6.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link6.STL,,6^sagittarius_descriptions-1,坐标系6,基准轴6,joint6,revolute,0.061387,0,0,0,0,0,link5,-1,0,0,0,0,-3.1,3.1,,,,,,,,
|
|
||||||
link_grasping_frame,-0.29292863789122,-0.000855248417215765,-0.182862893125548,0,0,0,1.55422651132493,0.0246807389934657,6.12977506319177E-05,-0.0174568851774044,0.0450160124726729,0.000114187269419605,0.0218809326193524,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_grasping_frame.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_grasping_frame.STL,,10^sagittarius_descriptions-1,坐标系10,基准轴10,joint_grasping_frame,fixed,0.0557,0,0,0,0,0,link6,0,0,0,0,0,0,0,,,,,,,,
|
|
||||||
link_gripper_left,0.0168448613850675,0.00562743947549268,-3.34157867051821E-05,0,0,0,0.0185082696416839,1.65620668352765E-06,3.12039082733024E-07,-1.06123823321138E-09,7.56731403094351E-06,5.54920898904154E-11,6.37610666327876E-06,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_gripper_left.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_gripper_left.STL,,7^sagittarius_descriptions-1,坐标系7,基准轴7,joint_gripper_left,prismatic,0.0695,0.033,0,0,0,0,link6,0,1,0,0,0,-0.025,0,,,,,,,,
|
|
||||||
link_gripper_right,0.0168448612901284,-0.00562743946967042,3.34158424380449E-05,0,0,0,0.0185082695670405,1.65620670013429E-06,-3.12039085367371E-07,1.06121397059487E-09,7.56731399585561E-06,5.54960746810554E-11,6.37610660879392E-06,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_gripper_right.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_gripper_right.STL,,8^sagittarius_descriptions-1,坐标系8,基准轴8,joint_gripper_right,prismatic,0.0695,-0.033,0,0,0,0,link6,0,-1,0,0,0,-0.025,0,,,,,,,,
|
|
||||||
|
|
|
@ -1,672 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<robot name="sgr532" xmlns:xacro="http://ros.org/wiki/xacro">
|
|
||||||
<xacro:arg name="robot_name" default = "sgr532"/>
|
|
||||||
|
|
||||||
<xacro:arg name="load_gazebo_configs" default="false"/>
|
|
||||||
<xacro:arg name="use_world_frame" default="false"/>
|
|
||||||
|
|
||||||
<xacro:arg name="external_urdf_loc" default=""/>
|
|
||||||
<xacro:property name="urdf_loc" value="$(arg external_urdf_loc)"/>
|
|
||||||
|
|
||||||
<xacro:if value="$(arg load_gazebo_configs)">
|
|
||||||
<xacro:include filename="$(find sagittarius_gazebo)/config/sagittarius_texture.gazebo"/>
|
|
||||||
</xacro:if>
|
|
||||||
|
|
||||||
<xacro:if value="$(arg use_world_frame)">
|
|
||||||
<link name="world"/>
|
|
||||||
<joint name="fixed" type="fixed">
|
|
||||||
<parent link="world"/>
|
|
||||||
<child link="$(arg robot_name)/base_link"/>
|
|
||||||
</joint>
|
|
||||||
</xacro:if>
|
|
||||||
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/base_link">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="0.00793179544277737 2.48115460577673E-19 0.0312047945759253"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="0.940146705073746" />
|
|
||||||
<inertia
|
|
||||||
ixx="0.000950195835062344"
|
|
||||||
ixy="3.14825974312405E-20"
|
|
||||||
ixz="-0.000200082473207336"
|
|
||||||
iyy="0.00267650790326047"
|
|
||||||
iyz="-1.01412991222915E-19"
|
|
||||||
izz="0.002902663900239" />
|
|
||||||
</inertial>
|
|
||||||
<visual>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/sagittarius_base_link.STL" />
|
|
||||||
</geometry>
|
|
||||||
<material
|
|
||||||
name="">
|
|
||||||
<color
|
|
||||||
rgba="1 1 1 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/sagittarius_base_link.STL" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/link1">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="-7.74579608786574E-10 3.23354786979222E-12 0.0344870497564283"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="0.123745079127031" />
|
|
||||||
<inertia
|
|
||||||
ixx="8.15779591743416E-05"
|
|
||||||
ixy="-4.25325094802094E-15"
|
|
||||||
ixz="-2.22201794150093E-12"
|
|
||||||
iyy="6.7607001964135E-05"
|
|
||||||
iyz="-1.59127137655435E-14"
|
|
||||||
izz="4.41297902395382E-05" />
|
|
||||||
</inertial>
|
|
||||||
<visual>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link1.STL" />
|
|
||||||
</geometry>
|
|
||||||
<material
|
|
||||||
name="">
|
|
||||||
<color
|
|
||||||
rgba="1 1 1 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link1.STL" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint1"
|
|
||||||
type="revolute">
|
|
||||||
<origin
|
|
||||||
xyz="0.045 0 0.07"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/base_link" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/link1" />
|
|
||||||
<axis
|
|
||||||
xyz="0 0 -1" />
|
|
||||||
<limit
|
|
||||||
lower="-2"
|
|
||||||
upper="2"
|
|
||||||
effort="100"
|
|
||||||
velocity="3.14" />
|
|
||||||
</joint>
|
|
||||||
<transmission name="trans_joint1">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<joint name="joint1">
|
|
||||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
<actuator name="joint1_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
</transmission>
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/link2">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="0.0119015312584793 -1.03639845764692E-08 0.150755903867182"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="0.156160822791157" />
|
|
||||||
<inertia
|
|
||||||
ixx="0.000429266751802994"
|
|
||||||
ixy="-2.27426165787001E-11"
|
|
||||||
ixz="-5.58862834004722E-05"
|
|
||||||
iyy="0.000444676097748043"
|
|
||||||
iyz="-2.41993274420629E-10"
|
|
||||||
izz="7.916162292894E-05" />
|
|
||||||
</inertial>
|
|
||||||
<visual>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link2.STL" />
|
|
||||||
</geometry>
|
|
||||||
<material
|
|
||||||
name="">
|
|
||||||
<color
|
|
||||||
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link2.STL" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint2"
|
|
||||||
type="revolute">
|
|
||||||
<origin
|
|
||||||
xyz="0 -0.0001 0.055"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/link1" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/link2" />
|
|
||||||
<axis
|
|
||||||
xyz="0 -1 0" />
|
|
||||||
<limit
|
|
||||||
lower="-1.57"
|
|
||||||
upper="1.4"
|
|
||||||
effort="100"
|
|
||||||
velocity="3.14" />
|
|
||||||
</joint>
|
|
||||||
<transmission name="trans_joint2">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<joint name="joint2">
|
|
||||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
<actuator name="joint2_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
</transmission>
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/link3">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="0.054357321065004 -2.67256405253992E-08 0.00064594073830132"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="0.0366487313200473" />
|
|
||||||
<inertia
|
|
||||||
ixx="9.28701176876329E-06"
|
|
||||||
ixy="-2.23878504272996E-11"
|
|
||||||
ixz="6.34310578103811E-07"
|
|
||||||
iyy="2.0727596617309E-05"
|
|
||||||
iyz="-1.71647322080315E-11"
|
|
||||||
izz="2.42416271089979E-05" />
|
|
||||||
</inertial>
|
|
||||||
<visual>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link3.STL" />
|
|
||||||
</geometry>
|
|
||||||
<material
|
|
||||||
name="">
|
|
||||||
<color
|
|
||||||
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link3.STL" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint3"
|
|
||||||
type="revolute">
|
|
||||||
<origin
|
|
||||||
xyz="0.033 -0.0001 0.179"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/link2" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/link3" />
|
|
||||||
<axis
|
|
||||||
xyz="0 -1 0" />
|
|
||||||
<limit
|
|
||||||
lower="-1.48"
|
|
||||||
upper="1.8"
|
|
||||||
effort="100"
|
|
||||||
velocity="3.14" />
|
|
||||||
</joint>
|
|
||||||
<transmission name="trans_joint3">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<joint name="joint3">
|
|
||||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
<actuator name="joint3_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
</transmission>
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/link4">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="0.0444029485674779 -0.00527865310158246 -1.43738021485262E-10"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="0.124950088011432" />
|
|
||||||
<inertia
|
|
||||||
ixx="3.52699895078887E-05"
|
|
||||||
ixy="-1.14870103744551E-05"
|
|
||||||
ixz="2.45948329713632E-14"
|
|
||||||
iyy="0.00010928557067047"
|
|
||||||
iyz="-3.42525315676607E-12"
|
|
||||||
izz="0.000121451732690361" />
|
|
||||||
</inertial>
|
|
||||||
<visual>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link4.STL" />
|
|
||||||
</geometry>
|
|
||||||
<material
|
|
||||||
name="">
|
|
||||||
<color
|
|
||||||
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link4.STL" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint4"
|
|
||||||
type="revolute">
|
|
||||||
<origin
|
|
||||||
xyz="0.0845 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/link3" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/link4" />
|
|
||||||
<axis
|
|
||||||
xyz="-1 0 0" />
|
|
||||||
<limit
|
|
||||||
lower="-2.90"
|
|
||||||
upper="2.90"
|
|
||||||
effort="100"
|
|
||||||
velocity="3.14" />
|
|
||||||
</joint>
|
|
||||||
<transmission name="trans_joint4">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<joint name="joint4">
|
|
||||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
<actuator name="joint4_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
</transmission>
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/link5">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="0.0391172635125616 1.22102848665318E-15 0.011359922796391"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="0.0564710099602834" />
|
|
||||||
<inertia
|
|
||||||
ixx="1.77667699918498E-05"
|
|
||||||
ixy="3.77572698012408E-19"
|
|
||||||
ixz="-6.79618041712231E-07"
|
|
||||||
iyy="1.89643063051294E-05"
|
|
||||||
iyz="1.78381670331667E-19"
|
|
||||||
izz="1.66130972969903E-05" />
|
|
||||||
</inertial>
|
|
||||||
<visual>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link5.STL" />
|
|
||||||
</geometry>
|
|
||||||
<material
|
|
||||||
name="">
|
|
||||||
<color
|
|
||||||
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link5.STL" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint5"
|
|
||||||
type="revolute">
|
|
||||||
<origin
|
|
||||||
xyz="0.08435 0.00065 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/link4" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/link5" />
|
|
||||||
<axis
|
|
||||||
xyz="0 -1 0" />
|
|
||||||
<limit
|
|
||||||
lower="-1.8"
|
|
||||||
upper="1.60"
|
|
||||||
effort="100"
|
|
||||||
velocity="3.14" />
|
|
||||||
</joint>
|
|
||||||
<transmission name="trans_joint5">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<joint name="joint5">
|
|
||||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
<actuator name="joint5_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
</transmission>
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/link6">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="0.0330748503556987 -1.47583477394259E-10 0.00874151659437283"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="0.0790875358325134" />
|
|
||||||
<inertia
|
|
||||||
ixx="4.00727396068743E-05"
|
|
||||||
ixy="1.5026227935429E-13"
|
|
||||||
ixz="6.77337873337555E-06"
|
|
||||||
iyy="4.37063090227749E-05"
|
|
||||||
iyz="-1.73263828329941E-13"
|
|
||||||
izz="5.69755186998711E-05" />
|
|
||||||
</inertial>
|
|
||||||
<visual>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link6.STL" />
|
|
||||||
</geometry>
|
|
||||||
<material
|
|
||||||
name="">
|
|
||||||
<color
|
|
||||||
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link6.STL" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint6"
|
|
||||||
type="revolute">
|
|
||||||
<origin
|
|
||||||
xyz="0.061387 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/link5" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/link6" />
|
|
||||||
<axis
|
|
||||||
xyz="-1 0 0" />
|
|
||||||
<limit
|
|
||||||
lower="-3.1"
|
|
||||||
upper="3.1"
|
|
||||||
effort="100"
|
|
||||||
velocity="3.14" />
|
|
||||||
</joint>
|
|
||||||
<transmission name="trans_joint6">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<joint name="joint6">
|
|
||||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
<actuator name="joint6_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
</transmission>
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/ar_tag_link">
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint_ar_tag_frame"
|
|
||||||
type="fixed">
|
|
||||||
<origin
|
|
||||||
xyz="-0.018 0 0.036"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/link6" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/ar_tag_link" />
|
|
||||||
<axis
|
|
||||||
xyz="0 0 0" />
|
|
||||||
<limit
|
|
||||||
lower="0"
|
|
||||||
upper="0"
|
|
||||||
effort="100"
|
|
||||||
velocity="3.14" />
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/link_grasping_frame">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="-0.29292863789122 -0.000855248417215765 -0.182862893125548"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="1.55422651132493" />
|
|
||||||
<inertia
|
|
||||||
ixx="0.0246807389934657"
|
|
||||||
ixy="6.12977506319177E-05"
|
|
||||||
ixz="-0.0174568851774044"
|
|
||||||
iyy="0.0450160124726729"
|
|
||||||
iyz="0.000114187269419605"
|
|
||||||
izz="0.0218809326193524" />
|
|
||||||
</inertial>
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint_grasping_frame"
|
|
||||||
type="fixed">
|
|
||||||
<origin
|
|
||||||
xyz="0.0557 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/link6" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/link_grasping_frame" />
|
|
||||||
<axis
|
|
||||||
xyz="0 0 0" />
|
|
||||||
<limit
|
|
||||||
lower="0"
|
|
||||||
upper="0"
|
|
||||||
effort="100"
|
|
||||||
velocity="3.10" />
|
|
||||||
</joint>
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/link_gripper_left">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="0.0168448613850675 0.00562743947549268 -3.34157867051821E-05"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="0.0185082696416839" />
|
|
||||||
<inertia
|
|
||||||
ixx="1.65620668352765E-06"
|
|
||||||
ixy="3.12039082733024E-07"
|
|
||||||
ixz="-1.06123823321138E-09"
|
|
||||||
iyy="7.56731403094351E-06"
|
|
||||||
iyz="5.54920898904154E-11"
|
|
||||||
izz="6.37610666327876E-06" />
|
|
||||||
</inertial>
|
|
||||||
<visual>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link_gripper_left.STL" />
|
|
||||||
</geometry>
|
|
||||||
<material
|
|
||||||
name="">
|
|
||||||
<color
|
|
||||||
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link_gripper_left.STL" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint_gripper_left"
|
|
||||||
type="prismatic">
|
|
||||||
<origin
|
|
||||||
xyz="0.0695 0.033 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/link6" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/link_gripper_left" />
|
|
||||||
<axis
|
|
||||||
xyz="0 1 0" />
|
|
||||||
<limit
|
|
||||||
lower="-0.034"
|
|
||||||
upper="0"
|
|
||||||
effort="10"
|
|
||||||
velocity="1.14" />
|
|
||||||
</joint>
|
|
||||||
<transmission name="trans_joint_gripper_left">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<joint name="joint_gripper_left">
|
|
||||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
<actuator name="joint_gripper_left_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
</transmission>
|
|
||||||
<link
|
|
||||||
name="$(arg robot_name)/link_gripper_right">
|
|
||||||
<inertial>
|
|
||||||
<origin
|
|
||||||
xyz="0.0168448612901284 -0.00562743946967042 3.34158424380449E-05"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<mass
|
|
||||||
value="0.0185082695670405" />
|
|
||||||
<inertia
|
|
||||||
ixx="1.65620670013429E-06"
|
|
||||||
ixy="-3.12039085367371E-07"
|
|
||||||
ixz="1.06121397059487E-09"
|
|
||||||
iyy="7.56731399585561E-06"
|
|
||||||
iyz="5.54960746810554E-11"
|
|
||||||
izz="6.37610660879392E-06" />
|
|
||||||
</inertial>
|
|
||||||
<visual>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link_gripper_right.STL" />
|
|
||||||
</geometry>
|
|
||||||
<material
|
|
||||||
name="">
|
|
||||||
<color
|
|
||||||
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<origin
|
|
||||||
xyz="0 0 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh
|
|
||||||
filename="package://sagittarius_descriptions/meshes/link_gripper_right.STL" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
<joint
|
|
||||||
name="joint_gripper_right"
|
|
||||||
type="prismatic">
|
|
||||||
<origin
|
|
||||||
xyz="0.0695 -0.033 0"
|
|
||||||
rpy="0 0 0" />
|
|
||||||
<parent
|
|
||||||
link="$(arg robot_name)/link6" />
|
|
||||||
<child
|
|
||||||
link="$(arg robot_name)/link_gripper_right" />
|
|
||||||
<axis
|
|
||||||
xyz="0 -1 0" />
|
|
||||||
<limit
|
|
||||||
lower="-0.034"
|
|
||||||
upper="0"
|
|
||||||
effort="10"
|
|
||||||
velocity="1.10" />
|
|
||||||
</joint>
|
|
||||||
<transmission name="trans_joint_gripper_right">
|
|
||||||
<type>transmission_interface/SimpleTransmission</type>
|
|
||||||
<joint name="joint_gripper_right">
|
|
||||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
|
||||||
</joint>
|
|
||||||
<actuator name="joint_gripper_right_motor">
|
|
||||||
<mechanicalReduction>1</mechanicalReduction>
|
|
||||||
</actuator>
|
|
||||||
</transmission>
|
|
||||||
|
|
||||||
<link name="$(arg robot_name)/usb_cam_link"></link>
|
|
||||||
<joint name="joint_usb_cam_frame" type="fixed">
|
|
||||||
<origin xyz="0.05 0 0.05825" rpy="-1.57 0 -1.57" />
|
|
||||||
<parent link="$(arg robot_name)/link6" />
|
|
||||||
<child link="$(arg robot_name)/usb_cam_link" />
|
|
||||||
<axis xyz="0 0 0" />
|
|
||||||
<limit lower="0" upper="0" effort="0" velocity="0" />
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<xacro:if value="${urdf_loc != ''}">
|
|
||||||
<xacro:include filename="${urdf_loc}"/>
|
|
||||||
</xacro:if>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
</robot>
|
|
||||||
|
|
@ -1,9 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<robot name="sgr532" xmlns:xacro="http://ros.org/wiki/xacro">
|
|
||||||
<link name="base_footprint"/>
|
|
||||||
<joint name="sagittarius_base_link_joint" type="fixed">
|
|
||||||
<origin xyz="0.044 0 0.138" rpy="0 0 0"/>
|
|
||||||
<parent link="base_footprint"/>
|
|
||||||
<child link="$(arg robot_name)/base_link"/>
|
|
||||||
</joint>
|
|
||||||
</robot>
|
|
||||||
|
|
@ -1,34 +0,0 @@
|
||||||
cmake_minimum_required(VERSION 2.8.3)
|
|
||||||
project(sagittarius_gazebo)
|
|
||||||
|
|
||||||
## 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
|
|
||||||
controller_manager
|
|
||||||
effort_controllers
|
|
||||||
gazebo_ros
|
|
||||||
gazebo_ros_control
|
|
||||||
sagittarius_descriptions
|
|
||||||
joint_state_controller
|
|
||||||
joint_trajectory_controller
|
|
||||||
)
|
|
||||||
|
|
||||||
###################################
|
|
||||||
## catkin specific configuration ##
|
|
||||||
###################################
|
|
||||||
## The catkin_package macro generates cmake config files for your package
|
|
||||||
## Declare things to be passed to dependent projects
|
|
||||||
catkin_package(
|
|
||||||
CATKIN_DEPENDS controller_manager effort_controllers gazebo_ros gazebo_ros_control sagittarius_descriptions joint_state_controller joint_trajectory_controller
|
|
||||||
)
|
|
||||||
|
|
||||||
###########
|
|
||||||
## Build ##
|
|
||||||
###########
|
|
||||||
|
|
||||||
## Specify additional locations of header files
|
|
||||||
## Your package locations should be listed before other locations
|
|
||||||
include_directories(
|
|
||||||
${catkin_INCLUDE_DIRS}
|
|
||||||
)
|
|
||||||
|
|
@ -1,18 +0,0 @@
|
||||||
planning_time_limit: 10.0
|
|
||||||
max_iterations: 200
|
|
||||||
max_iterations_after_collision_free: 5
|
|
||||||
smoothness_cost_weight: 0.1
|
|
||||||
obstacle_cost_weight: 1.0
|
|
||||||
learning_rate: 0.01
|
|
||||||
smoothness_cost_velocity: 0.0
|
|
||||||
smoothness_cost_acceleration: 1.0
|
|
||||||
smoothness_cost_jerk: 0.0
|
|
||||||
ridge_factor: 0.01
|
|
||||||
use_pseudo_inverse: false
|
|
||||||
pseudo_inverse_ridge_factor: 1e-4
|
|
||||||
joint_update_limit: 0.1
|
|
||||||
collision_clearence: 0.2
|
|
||||||
collision_threshold: 0.07
|
|
||||||
use_stochastic_descent: true
|
|
||||||
enable_failure_recovery: true
|
|
||||||
max_recovery_attempts: 5
|
|
||||||
|
|
@ -1,9 +0,0 @@
|
||||||
controller_list:
|
|
||||||
- name: "sagittarius_arm_controller"
|
|
||||||
action_ns: follow_joint_trajectory
|
|
||||||
type: FollowJointTrajectory
|
|
||||||
joints: [joint1, joint2, joint3, joint4, joint5, joint6]
|
|
||||||
- name: "sagittarius_gripper_controller"
|
|
||||||
action_ns: follow_joint_trajectory
|
|
||||||
type: FollowJointTrajectory
|
|
||||||
joints: [joint_gripper_right, joint_gripper_left]
|
|
||||||
|
|
@ -1,53 +0,0 @@
|
||||||
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
|
|
||||||
|
|
||||||
# For beginners, we downscale velocity and acceleration limits.
|
|
||||||
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
|
|
||||||
default_velocity_scaling_factor: 0.1
|
|
||||||
default_acceleration_scaling_factor: 0.1
|
|
||||||
|
|
||||||
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
|
|
||||||
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
|
|
||||||
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
|
|
||||||
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
|
|
||||||
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
|
|
||||||
joint_limits:
|
|
||||||
joint1:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 3.14
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 1.57
|
|
||||||
joint2:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 3.14
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 1.57
|
|
||||||
joint3:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 3.14
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 1.57
|
|
||||||
joint4:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 3.14
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 1.57
|
|
||||||
joint5:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 3.14
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 1.57
|
|
||||||
joint6:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 3.14
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 1.57
|
|
||||||
joint_gripper_left:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 3.14
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 1.57
|
|
||||||
joint_gripper_right:
|
|
||||||
has_velocity_limits: true
|
|
||||||
max_velocity: 3.14
|
|
||||||
has_acceleration_limits: true
|
|
||||||
max_acceleration: 1.57
|
|
||||||
|
|
@ -1,23 +0,0 @@
|
||||||
# position-only-ik and orientation-only-ik don't work unless position_only_ik
|
|
||||||
# is set to true for the KDL and trac_ik plugins
|
|
||||||
|
|
||||||
# sagittarius_arm:
|
|
||||||
# kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
|
||||||
# kinematics_solver_search_resolution: 0.005
|
|
||||||
# kinematics_solver_timeout: 0.005
|
|
||||||
# kinematics_solver_attempts: 3
|
|
||||||
# position_only_ik: true
|
|
||||||
|
|
||||||
sagittarius_arm:
|
|
||||||
kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
|
|
||||||
kinematics_solver_timeout: 0.005
|
|
||||||
solve_type: Speed
|
|
||||||
position_only_ik: false
|
|
||||||
|
|
||||||
# Position-only-ik and orientation-only-ik work automatically for the LMA plugin
|
|
||||||
|
|
||||||
# sagittarius_arm:
|
|
||||||
# kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin
|
|
||||||
# kinematics_solver_search_resolution: 0.005
|
|
||||||
# kinematics_solver_timeout: 0.005
|
|
||||||
# kinematics_solver_attempts: 3
|
|
||||||
|
|
@ -1,174 +0,0 @@
|
||||||
planner_configs:
|
|
||||||
SBL:
|
|
||||||
type: geometric::SBL
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
EST:
|
|
||||||
type: geometric::EST
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
|
|
||||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
|
||||||
LBKPIECE:
|
|
||||||
type: geometric::LBKPIECE
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
|
|
||||||
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
|
|
||||||
BKPIECE:
|
|
||||||
type: geometric::BKPIECE
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
|
|
||||||
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
|
|
||||||
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
|
|
||||||
KPIECE:
|
|
||||||
type: geometric::KPIECE
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
|
||||||
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
|
|
||||||
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
|
|
||||||
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
|
|
||||||
RRT:
|
|
||||||
type: geometric::RRT
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
|
|
||||||
RRTConnect:
|
|
||||||
type: geometric::RRTConnect
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
RRTstar:
|
|
||||||
type: geometric::RRTstar
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
|
|
||||||
delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
|
|
||||||
TRRT:
|
|
||||||
type: geometric::TRRT
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
|
|
||||||
max_states_failed: 10 # when to start increasing temp. default: 10
|
|
||||||
temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
|
|
||||||
min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
|
|
||||||
init_temperature: 10e-6 # initial temperature. default: 10e-6
|
|
||||||
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
|
|
||||||
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
|
|
||||||
k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup()
|
|
||||||
PRM:
|
|
||||||
type: geometric::PRM
|
|
||||||
max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
|
|
||||||
PRMstar:
|
|
||||||
type: geometric::PRMstar
|
|
||||||
FMT:
|
|
||||||
type: geometric::FMT
|
|
||||||
num_samples: 1000 # number of states that the planner should sample. default: 1000
|
|
||||||
radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1
|
|
||||||
nearest_k: 1 # use Knearest strategy. default: 1
|
|
||||||
cache_cc: 1 # use collision checking cache. default: 1
|
|
||||||
heuristics: 0 # activate cost to go heuristics. default: 0
|
|
||||||
extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
|
|
||||||
BFMT:
|
|
||||||
type: geometric::BFMT
|
|
||||||
num_samples: 1000 # number of states that the planner should sample. default: 1000
|
|
||||||
radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0
|
|
||||||
nearest_k: 1 # use the Knearest strategy. default: 1
|
|
||||||
balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
|
|
||||||
optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
|
|
||||||
heuristics: 1 # activates cost to go heuristics. default: 1
|
|
||||||
cache_cc: 1 # use the collision checking cache. default: 1
|
|
||||||
extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
|
|
||||||
PDST:
|
|
||||||
type: geometric::PDST
|
|
||||||
STRIDE:
|
|
||||||
type: geometric::STRIDE
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
|
||||||
use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
|
|
||||||
degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
|
|
||||||
max_degree: 18 # max degree of a node in the GNAT. default: 12
|
|
||||||
min_degree: 12 # min degree of a node in the GNAT. default: 12
|
|
||||||
max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6
|
|
||||||
estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0
|
|
||||||
min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2
|
|
||||||
BiTRRT:
|
|
||||||
type: geometric::BiTRRT
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1
|
|
||||||
init_temperature: 100 # initial temperature. default: 100
|
|
||||||
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
|
|
||||||
frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
|
|
||||||
cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
|
|
||||||
LBTRRT:
|
|
||||||
type: geometric::LBTRRT
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
|
||||||
epsilon: 0.4 # optimality approximation factor. default: 0.4
|
|
||||||
BiEST:
|
|
||||||
type: geometric::BiEST
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
ProjEST:
|
|
||||||
type: geometric::ProjEST
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
|
||||||
LazyPRM:
|
|
||||||
type: geometric::LazyPRM
|
|
||||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
|
||||||
LazyPRMstar:
|
|
||||||
type: geometric::LazyPRMstar
|
|
||||||
SPARS:
|
|
||||||
type: geometric::SPARS
|
|
||||||
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
|
|
||||||
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
|
|
||||||
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
|
|
||||||
max_failures: 1000 # maximum consecutive failure limit. default: 1000
|
|
||||||
SPARStwo:
|
|
||||||
type: geometric::SPARStwo
|
|
||||||
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
|
|
||||||
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
|
|
||||||
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
|
|
||||||
max_failures: 5000 # maximum consecutive failure limit. default: 5000
|
|
||||||
sagittarius_arm:
|
|
||||||
default_planner_config: RRTConnect
|
|
||||||
planner_configs:
|
|
||||||
- SBL
|
|
||||||
- EST
|
|
||||||
- LBKPIECE
|
|
||||||
- BKPIECE
|
|
||||||
- KPIECE
|
|
||||||
- RRT
|
|
||||||
- RRTConnect
|
|
||||||
- RRTstar
|
|
||||||
- TRRT
|
|
||||||
- PRM
|
|
||||||
- PRMstar
|
|
||||||
- FMT
|
|
||||||
- BFMT
|
|
||||||
- PDST
|
|
||||||
- STRIDE
|
|
||||||
- BiTRRT
|
|
||||||
- LBTRRT
|
|
||||||
- BiEST
|
|
||||||
- ProjEST
|
|
||||||
- LazyPRM
|
|
||||||
- LazyPRMstar
|
|
||||||
- SPARS
|
|
||||||
- SPARStwo
|
|
||||||
sagittarius_gripper:
|
|
||||||
default_planner_config: RRTConnect
|
|
||||||
planner_configs:
|
|
||||||
- SBL
|
|
||||||
- EST
|
|
||||||
- LBKPIECE
|
|
||||||
- BKPIECE
|
|
||||||
- KPIECE
|
|
||||||
- RRT
|
|
||||||
- RRTConnect
|
|
||||||
- RRTstar
|
|
||||||
- TRRT
|
|
||||||
- PRM
|
|
||||||
- PRMstar
|
|
||||||
- FMT
|
|
||||||
- BFMT
|
|
||||||
- PDST
|
|
||||||
- STRIDE
|
|
||||||
- BiTRRT
|
|
||||||
- LBTRRT
|
|
||||||
- BiEST
|
|
||||||
- ProjEST
|
|
||||||
- LazyPRM
|
|
||||||
- LazyPRMstar
|
|
||||||
- SPARS
|
|
||||||
- SPARStwo
|
|
||||||
|
|
@ -1,54 +0,0 @@
|
||||||
<?xml version="1.0"?>
|
|
||||||
<robot xmlns:xacro="http://ros.org/wiki/xacro">
|
|
||||||
|
|
||||||
<gazebo>
|
|
||||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
|
||||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
|
||||||
<legacyModeNS>true</legacyModeNS>
|
|
||||||
</plugin>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/base_link">
|
|
||||||
<material>Custom/sagittarius</material>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/link1">
|
|
||||||
<material>Custom/sagittarius</material>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/link2">
|
|
||||||
<material>Custom/sagittarius</material>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/link3">
|
|
||||||
<material>Custom/sagittarius</material>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/link4">
|
|
||||||
<material>Custom/sagittarius</material>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/link5">
|
|
||||||
<material>Custom/sagittarius</material>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/link6">
|
|
||||||
<material>Custom/sagittarius</material>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/link_gripper_left">
|
|
||||||
<material>Custom/sagittarius</material>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/link_gripper_right">
|
|
||||||
<material>Custom/sagittarius</material>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
<gazebo reference="$(arg robot_name)/ar_tag_link">
|
|
||||||
<disableFixedJointLumping>true</disableFixedJointLumping>
|
|
||||||
</gazebo>
|
|
||||||
<gazebo reference="$(arg robot_name)/link_grasping_frame">
|
|
||||||
<disableFixedJointLumping>true</disableFixedJointLumping>
|
|
||||||
</gazebo>
|
|
||||||
|
|
||||||
</robot>
|
|
||||||
|
|
@ -1,52 +0,0 @@
|
||||||
# Publish all joint states
|
|
||||||
# Creates the /joint_states topic necessary in ROS
|
|
||||||
joint_state_controller:
|
|
||||||
type: joint_state_controller/JointStateController
|
|
||||||
publish_rate: 100
|
|
||||||
|
|
||||||
sagittarius_arm_controller:
|
|
||||||
type: effort_controllers/JointTrajectoryController
|
|
||||||
joints:
|
|
||||||
- joint1
|
|
||||||
- joint2
|
|
||||||
- joint3
|
|
||||||
- joint4
|
|
||||||
- joint5
|
|
||||||
- joint6
|
|
||||||
gains: # Required because we're controlling an effort interface
|
|
||||||
joint1: {p: 500.0, d: 1.0, i: 5.0}
|
|
||||||
joint2: {p: 800.0, d: 1.0, i: 10.0}
|
|
||||||
joint3: {p: 800.0, d: 1.0, i: 10.0}
|
|
||||||
joint4: {p: 200.0, d: 0, i: 0}
|
|
||||||
joint5: {p: 100.0, d: 1.0, i: 3.0}
|
|
||||||
joint6: {p: 20.0, d: 0, i: 0}
|
|
||||||
# joint4: {p: 5.0, d: 0, i: 0}
|
|
||||||
constraints:
|
|
||||||
goal_time: 0.2
|
|
||||||
joint1:
|
|
||||||
goal: 0.1
|
|
||||||
trajectory: 0.2
|
|
||||||
joint2:
|
|
||||||
goal: 0.2
|
|
||||||
trajectory: 0.2
|
|
||||||
joint3:
|
|
||||||
goal: 0.2
|
|
||||||
trajectory: 0.2
|
|
||||||
joint4:
|
|
||||||
goal: 0.2
|
|
||||||
trajectory: 0.2
|
|
||||||
joint5:
|
|
||||||
goal: 0.2
|
|
||||||
trajectory: 0.2
|
|
||||||
joint6:
|
|
||||||
goal: 0.2
|
|
||||||
trajectory: 0.2
|
|
||||||
|
|
||||||
sagittarius_gripper_controller:
|
|
||||||
type: effort_controllers/JointTrajectoryController
|
|
||||||
joints:
|
|
||||||
- joint_gripper_left
|
|
||||||
- joint_gripper_right
|
|
||||||
gains:
|
|
||||||
joint_gripper_left: {p: 100.0, d: 1.0, i: 0.0}
|
|
||||||
joint_gripper_right: {p: 100.0, d: 1.0, i: 0.0}
|
|
||||||
|
|
@ -1,84 +0,0 @@
|
||||||
<?xml version="1.0" encoding="UTF-8"?>
|
|
||||||
<!--This does not replace URDF, and is not an extension of URDF.
|
|
||||||
This is a format for representing semantic information about the robot structure.
|
|
||||||
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
|
|
||||||
-->
|
|
||||||
<robot name="sgr532" xmlns:xacro="http://ros.org/wiki/xacro">
|
|
||||||
<xacro:arg name="robot_name" default = "sgr532"/>
|
|
||||||
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
|
|
||||||
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
|
|
||||||
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
|
|
||||||
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
|
|
||||||
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
|
|
||||||
<group name="sagittarius_arm">
|
|
||||||
<chain base_link="$(arg robot_name)/base_link" tip_link="$(arg robot_name)/link_grasping_frame"/>
|
|
||||||
</group>
|
|
||||||
<group name="sagittarius_gripper">
|
|
||||||
<link name="$(arg robot_name)/link_gripper_left"/>
|
|
||||||
<link name="$(arg robot_name)/link_gripper_right"/>
|
|
||||||
<joint name="joint_gripper_left"/>
|
|
||||||
<joint name="joint_gripper_right"/>
|
|
||||||
</group>
|
|
||||||
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
|
|
||||||
<group_state name="home" group="sagittarius_arm">
|
|
||||||
<joint name="joint1" value="0"/>
|
|
||||||
<joint name="joint2" value="0"/>
|
|
||||||
<joint name="joint3" value="0"/>
|
|
||||||
<joint name="joint4" value="0"/>
|
|
||||||
<joint name="joint5" value="0"/>
|
|
||||||
<joint name="joint6" value="0"/>
|
|
||||||
</group_state>
|
|
||||||
<group_state name="up" group="sagittarius_arm">
|
|
||||||
<joint name="joint1" value="0"/>
|
|
||||||
<joint name="joint2" value="0"/>
|
|
||||||
<joint name="joint3" value="1.57"/>
|
|
||||||
<joint name="joint4" value="0"/>
|
|
||||||
<joint name="joint5" value="0"/>
|
|
||||||
<joint name="joint6" value="0"/>
|
|
||||||
</group_state>
|
|
||||||
<group_state name="sleep" group="sagittarius_arm">
|
|
||||||
<joint name="joint1" value="0"/>
|
|
||||||
<joint name="joint2" value="1.4"/>
|
|
||||||
<joint name="joint3" value="-1.47"/>
|
|
||||||
<joint name="joint4" value="0"/>
|
|
||||||
<joint name="joint5" value="-0.4851"/>
|
|
||||||
<joint name="joint6" value="0"/>
|
|
||||||
</group_state>
|
|
||||||
<group_state name="open" group="sagittarius_gripper">
|
|
||||||
<joint name="joint_gripper_left" value="0"/>
|
|
||||||
<joint name="joint_gripper_right" value="0"/>
|
|
||||||
</group_state>
|
|
||||||
<group_state name="close" group="sagittarius_gripper">
|
|
||||||
<joint name="joint_gripper_left" value="-0.028"/>
|
|
||||||
<joint name="joint_gripper_right" value="-0.028"/>
|
|
||||||
</group_state>
|
|
||||||
<group_state name="middle" group="sagittarius_gripper">
|
|
||||||
<joint name="joint_gripper_left" value="-0.014"/>
|
|
||||||
<joint name="joint_gripper_right" value="-0.014"/>
|
|
||||||
</group_state>
|
|
||||||
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
|
|
||||||
<end_effector name="gripper_end" parent_link="$(arg robot_name)/link_grasping_frame" group="sagittarius_gripper"/>
|
|
||||||
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
|
|
||||||
<virtual_joint name="fixed" type="fixed" parent_frame="world" child_link="$(arg robot_name)/base_link"/>
|
|
||||||
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
|
||||||
<disable_collisions link1="$(arg robot_name)/base_link" link2="$(arg robot_name)/link1" reason="Adjacent"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/base_link" link2="$(arg robot_name)/link2" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/base_link" link2="$(arg robot_name)/link3" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link1" link2="$(arg robot_name)/link2" reason="Adjacent"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link1" link2="$(arg robot_name)/link3" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link2" link2="$(arg robot_name)/link3" reason="Adjacent"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link3" link2="$(arg robot_name)/link4" reason="Adjacent"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link3" link2="$(arg robot_name)/link5" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link3" link2="$(arg robot_name)/link6" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link3" link2="$(arg robot_name)/link_gripper_left" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link3" link2="$(arg robot_name)/link_gripper_right" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link4" link2="$(arg robot_name)/link5" reason="Adjacent"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link4" link2="$(arg robot_name)/link6" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link4" link2="$(arg robot_name)/link_gripper_left" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link4" link2="$(arg robot_name)/link_gripper_right" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link5" link2="$(arg robot_name)/link6" reason="Adjacent"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link5" link2="$(arg robot_name)/link_gripper_left" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link5" link2="$(arg robot_name)/link_gripper_right" reason="Never"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link6" link2="$(arg robot_name)/link_gripper_left" reason="Adjacent"/>
|
|
||||||
<disable_collisions link1="$(arg robot_name)/link6" link2="$(arg robot_name)/link_gripper_right" reason="Adjacent"/>
|
|
||||||
</robot>
|
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 483 KiB |
|
|
@ -1,10 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- CHOMP Plugin for MoveIt! -->
|
|
||||||
<arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
|
|
||||||
<arg name="start_state_max_bounds_error" value="0.1" />
|
|
||||||
|
|
||||||
<param name="planning_plugin" value="$(arg planning_plugin)" />
|
|
||||||
<param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
|
|
||||||
|
|
||||||
<rosparam command="load" file="$(find sagittarius_gazebo)/config/chomp_planning.yaml" />
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,39 +0,0 @@
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default="$(arg robot_name)"/>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
<arg name="use_pid_cntlrs" default="false"/>
|
|
||||||
<arg name="use_world_frame" default="true"/>
|
|
||||||
<arg name="external_urdf_loc" default=""/>
|
|
||||||
<arg name="external_srdf_loc" default=""/>
|
|
||||||
<arg name="rviz_frame" default="world"/>
|
|
||||||
<arg name="use_gazebo" default="true"/>
|
|
||||||
<arg name="use_actual" default="false"/>
|
|
||||||
<arg name="use_fake" default="false"/>
|
|
||||||
|
|
||||||
<include if="$(arg use_gazebo)" file="$(find sagittarius_gazebo)/launch/gazebo.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model)"/>
|
|
||||||
<arg name="use_world_frame" value="$(arg use_world_frame)"/>
|
|
||||||
<arg name="external_urdf_loc" value="$(arg external_urdf_loc)"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
<include file="$(find sagittarius_gazebo)/launch/move_group.launch" ns="$(arg robot_name)">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model)"/>
|
|
||||||
<arg name="external_srdf_loc" value="$(arg external_srdf_loc)"/>
|
|
||||||
<arg name="fake_execution" value="$(arg use_fake)"/>
|
|
||||||
<arg name="publish_monitored_planning_scene" value="true" />
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<include file="$(find sagittarius_gazebo)/launch/moveit_rviz.launch" ns="$(arg robot_name)">
|
|
||||||
<arg name="rviz_frame" value="$(arg rviz_frame)"/>
|
|
||||||
<arg name="config" value="true"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,61 +0,0 @@
|
||||||
<launch>
|
|
||||||
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default="$(arg robot_name)"/>
|
|
||||||
<arg name="use_world_frame" default="true"/>
|
|
||||||
<arg name="external_urdf_loc" default=""/>
|
|
||||||
<arg name="use_default_rviz" default="false"/>
|
|
||||||
<arg name="gui" default="true"/>
|
|
||||||
<arg name="debug" default="false"/>
|
|
||||||
<arg name="paused" default="true"/>
|
|
||||||
<arg name="recording" default="false"/>
|
|
||||||
<arg name="use_sim_time" default="true"/>
|
|
||||||
|
|
||||||
|
|
||||||
<env name="GAZEBO_RESOURCE_PATH" value="$(find sagittarius_gazebo)"/>
|
|
||||||
|
|
||||||
<rosparam file="$(find sagittarius_gazebo)/config/$(arg robot_model)_gazebo_controllers.yaml" command="load" ns="$(arg robot_name)"/>
|
|
||||||
|
|
||||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
|
||||||
<arg name="gui" value="$(arg gui)" />
|
|
||||||
<arg name="debug" value="$(arg debug)" />
|
|
||||||
<arg name="paused" value="$(arg paused)"/>
|
|
||||||
<arg name="recording" value="$(arg recording)"/>
|
|
||||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="controller_spawner"
|
|
||||||
pkg="controller_manager"
|
|
||||||
type="spawner"
|
|
||||||
respawn="false"
|
|
||||||
output="screen"
|
|
||||||
ns="$(arg robot_name)"
|
|
||||||
args="sagittarius_arm_controller sagittarius_gripper_controller joint_state_controller"/>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
<!-- If needed, broadcast static tf for robot root -->
|
|
||||||
<include file="$(find sagittarius_descriptions)/launch/description.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model)"/>
|
|
||||||
<arg name="joint_pub_gui" value="false"/>
|
|
||||||
<arg name="use_default_rviz" value="$(arg use_default_rviz)"/>
|
|
||||||
<arg name="external_urdf_loc" value="$(arg external_urdf_loc)"/>
|
|
||||||
<arg name="use_world_frame" value="$(arg use_world_frame)"/>
|
|
||||||
<arg name="load_gazebo_configs" value="true"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
|
|
||||||
<node
|
|
||||||
name="urdf_spawner"
|
|
||||||
pkg="gazebo_ros"
|
|
||||||
type="spawn_model"
|
|
||||||
respawn="false"
|
|
||||||
output="screen"
|
|
||||||
ns="$(arg robot_name)"
|
|
||||||
args="-urdf -model $(arg robot_model) -param robot_description"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,17 +0,0 @@
|
||||||
<launch>
|
|
||||||
<!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
|
|
||||||
|
|
||||||
<arg name="dev" default="/dev/input/js0" />
|
|
||||||
|
|
||||||
<!-- Launch joy node -->
|
|
||||||
<node pkg="joy" type="joy_node" name="joy">
|
|
||||||
<param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
|
|
||||||
<param name="deadzone" value="0.2" />
|
|
||||||
<param name="autorepeat_rate" value="40" />
|
|
||||||
<param name="coalesce_interval" value="0.025" />
|
|
||||||
</node>
|
|
||||||
|
|
||||||
<!-- Launch python interface -->
|
|
||||||
<node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
|
|
@ -1,84 +0,0 @@
|
||||||
<launch>
|
|
||||||
|
|
||||||
<!-- GDB Debug Option -->
|
|
||||||
<arg name="debug" default="false" />
|
|
||||||
<arg unless="$(arg debug)" name="launch_prefix" value="" />
|
|
||||||
<arg if="$(arg debug)" name="launch_prefix"
|
|
||||||
value="gdb -x $(find sagittarius_gazebo)/launch/gdb_settings.gdb --ex run --args" />
|
|
||||||
|
|
||||||
<!-- Verbose Mode Option -->
|
|
||||||
<arg name="info" default="$(arg debug)" />
|
|
||||||
<arg unless="$(arg info)" name="command_args" value="" />
|
|
||||||
<arg if="$(arg info)" name="command_args" value="--debug" />
|
|
||||||
|
|
||||||
<!-- move_group settings -->
|
|
||||||
<arg name="allow_trajectory_execution" default="true"/>
|
|
||||||
<arg name="fake_execution" default="false"/>
|
|
||||||
<arg name="max_safe_path_cost" default="1"/>
|
|
||||||
<arg name="jiggle_fraction" default="0.05" />
|
|
||||||
<arg name="publish_monitored_planning_scene" default="true"/>
|
|
||||||
|
|
||||||
<arg name="capabilities" default=""/>
|
|
||||||
<arg name="disable_capabilities" default=""/>
|
|
||||||
<arg name="robot_name" default="sgr532"/>
|
|
||||||
<arg name="robot_model" default=""/>
|
|
||||||
<arg name="external_srdf_loc" default=""/>
|
|
||||||
|
|
||||||
<include file="$(find sagittarius_gazebo)/launch/planning_context.launch">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name)"/>
|
|
||||||
<arg name="robot_model" value="$(arg robot_model)"/>
|
|
||||||
<arg name="external_srdf_loc" value="$(arg external_srdf_loc)"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<!-- load these non-default MoveGroup capabilities (space seperated) -->
|
|
||||||
<!--
|
|
||||||
<arg name="capabilities" value="
|
|
||||||
a_package/AwsomeMotionPlanningCapability
|
|
||||||
another_package/GraspPlanningPipeline
|
|
||||||
" />
|
|
||||||
-->
|
|
||||||
|
|
||||||
<!-- inhibit these default MoveGroup capabilities (space seperated) -->
|
|
||||||
<!--
|
|
||||||
<arg name="disable_capabilities" value="
|
|
||||||
move_group/MoveGroupKinematicsService
|
|
||||||
move_group/ClearOctomapService
|
|
||||||
" />
|
|
||||||
-->
|
|
||||||
|
|
||||||
<!-- Planning Functionality -->
|
|
||||||
<include ns="move_group" file="$(find sagittarius_gazebo)/launch/planning_pipeline.launch.xml">
|
|
||||||
<arg name="pipeline" value="ompl" />
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<!-- Trajectory Execution Functionality -->
|
|
||||||
<include ns="move_group" file="$(find sagittarius_gazebo)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
|
|
||||||
<arg name="fake_execution" value="$(arg fake_execution)"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<!-- Sensors Functionality -->
|
|
||||||
<include ns="move_group" file="$(find sagittarius_gazebo)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
|
|
||||||
<arg name="robot_name" value="$(arg robot_name)" />
|
|
||||||
<arg name="robot_model" value="$(arg robot_model)"/>
|
|
||||||
</include>
|
|
||||||
|
|
||||||
<!-- Start the actual move_group node/action server -->
|
|
||||||
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
|
|
||||||
<!-- Set the display variable, in case OpenGL code is used internally -->
|
|
||||||
<env name="DISPLAY" value="$(optenv DISPLAY :0)" />
|
|
||||||
|
|
||||||
<param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
|
|
||||||
<param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
|
|
||||||
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
|
|
||||||
<param name="capabilities" value="$(arg capabilities)"/>
|
|
||||||
<param name="disable_capabilities" value="$(arg disable_capabilities)"/>
|
|
||||||
|
|
||||||
|
|
||||||
<!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
|
|
||||||
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
|
|
||||||
<param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
|
|
||||||
<param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
|
|
||||||
<param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
|
|
||||||
</node>
|
|
||||||
|
|
||||||
</launch>
|
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue