Add the hector case for parallel distributed gazebo
This commit is contained in:
parent
46b83408d6
commit
c1ca3091ea
|
|
@ -0,0 +1,100 @@
|
|||
#!/bin/bash
|
||||
|
||||
launch_file=$1
|
||||
docker_ssh=$2
|
||||
|
||||
GAZEBO_IP=`ifconfig $3 |grep "inet "| cut -f 2 -d ":"|cut -f 1 -d " "`
|
||||
|
||||
group_count=0
|
||||
|
||||
file=envfile
|
||||
if [ -f $file ]
|
||||
then
|
||||
`rm -rf $file`
|
||||
fi
|
||||
`touch $file`
|
||||
|
||||
group="ns="
|
||||
port="htonsnumber"
|
||||
x="name=\"x\""
|
||||
y="name=\"y\""
|
||||
x_diff="name=\"X_DIFF\""
|
||||
y_diff="name=\"Y_DIFF\""
|
||||
|
||||
while read line
|
||||
do
|
||||
if [[ $line =~ $group ]]
|
||||
then
|
||||
let group_count=$group_count+1
|
||||
temp=${line#*\"}
|
||||
echo "GAZEBO_IP:$GAZEBO_IP" >> $file
|
||||
echo "NAMESPACE:${temp%\"*}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $port ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "SOCKET_PORT:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_X:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_Y:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "X_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "Y_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
done < $launch_file
|
||||
|
||||
i=0
|
||||
index=0
|
||||
while read line
|
||||
do
|
||||
case $(($i%7)) in
|
||||
0) gazebo_ip[$index]=$line;;
|
||||
1) namespace[$index]=$line;;
|
||||
2) socket_port[$index]=$line;;
|
||||
3) x_diff[$index]=$line;;
|
||||
4) y_diff[$index]=$line;;
|
||||
5) start_x[$index]=$line;;
|
||||
6) start_y[$index]=$line;let index++;;
|
||||
esac
|
||||
let i++
|
||||
done < $file
|
||||
|
||||
#`rm $file`
|
||||
|
||||
for((i=0;i<index;i++))
|
||||
do
|
||||
`sshpass -p '123456' ssh $2 "sudo docker stop ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1; sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
docker_ip[$i]=`sshpass -p '123456' ssh $2 "sudo docker inspect -f '{{range .NetworkSettings.Networks}}{{.IPAddress}}{{end}}' ${namespace[$i]#*:}"`
|
||||
|
||||
export ${namespace[$i]#*:}_ip=${docker_ip[$i]}
|
||||
echo "${namespace[$i]#*:} Done!!"
|
||||
done
|
||||
echo "All Done!!"
|
||||
|
||||
#source /home/zhangshuai/catkin_ws/devel/setup.bash
|
||||
source /home/airc1-02/Flight_control_Comprehensive/devel/setup.bash
|
||||
|
||||
`roslaunch hector_quadrotor_gazebo ${launch_file##*/}`
|
||||
|
|
@ -0,0 +1,98 @@
|
|||
#!/bin/bash
|
||||
|
||||
launch_file=$1
|
||||
docker_ssh=$2
|
||||
|
||||
GAZEBO_IP=`ifconfig $3 |grep "inet "| cut -f 2 -d ":"|cut -f 1 -d " "`
|
||||
|
||||
group_count=0
|
||||
|
||||
file=envfile
|
||||
if [ -f $file ]
|
||||
then
|
||||
`rm -rf $file`
|
||||
fi
|
||||
`touch $file`
|
||||
|
||||
group="ns="
|
||||
port="htonsnumber"
|
||||
x="name=\"x\""
|
||||
y="name=\"y\""
|
||||
x_diff="name=\"X_DIFF\""
|
||||
y_diff="name=\"Y_DIFF\""
|
||||
|
||||
while read line
|
||||
do
|
||||
if [[ $line =~ $group ]]
|
||||
then
|
||||
let group_count=$group_count+1
|
||||
temp=${line#*\"}
|
||||
echo "GAZEBO_IP:$GAZEBO_IP" >> $file
|
||||
echo "NAMESPACE:${temp%\"*}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $port ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "SOCKET_PORT:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_X:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_Y:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "X_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "Y_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
done < $launch_file
|
||||
|
||||
i=0
|
||||
index=0
|
||||
while read line
|
||||
do
|
||||
case $(($i%7)) in
|
||||
0) gazebo_ip[$index]=$line;;
|
||||
1) namespace[$index]=$line;;
|
||||
2) socket_port[$index]=$line;;
|
||||
3) x_diff[$index]=$line;;
|
||||
4) y_diff[$index]=$line;;
|
||||
5) start_x[$index]=$line;;
|
||||
6) start_y[$index]=$line;let index++;;
|
||||
esac
|
||||
let i++
|
||||
done < $file
|
||||
|
||||
#`rm $file`
|
||||
|
||||
for((i=0;i<index;i++))
|
||||
do
|
||||
`sshpass -p 'zhangmuhan' ssh $2 "sudo docker stop ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1; sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
docker_ip[$i]=`sshpass -p 'zhangmuhan' ssh $2 "sudo docker inspect -f '{{range .NetworkSettings.Networks}}{{.IPAddress}}{{end}}' ${namespace[$i]#*:}"`
|
||||
|
||||
export ${namespace[$i]#*:}_ip=${docker_ip[$i]}
|
||||
done
|
||||
|
||||
#source /home/zhangshuai/catkin_ws/devel/setup.bash
|
||||
source /home/ai01/Flight_control_Comprehensive/devel/setup.bash
|
||||
|
||||
`roslaunch hector_quadrotor_gazebo ${launch_file##*/}`
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
#!/bin/bash
|
||||
|
||||
launch_file=$1
|
||||
docker_ssh=$2
|
||||
|
||||
GAZEBO_IP=`ifconfig $3 |grep "inet "| cut -f 2 -d ":"|cut -f 1 -d " "`
|
||||
|
||||
group_count=0
|
||||
|
||||
file=envfile
|
||||
if [ -f $file ]
|
||||
then
|
||||
`rm -rf $file`
|
||||
fi
|
||||
`touch $file`
|
||||
|
||||
group="ns="
|
||||
port="htonsnumber"
|
||||
x="name=\"x\""
|
||||
y="name=\"y\""
|
||||
x_diff="name=\"X_DIFF\""
|
||||
y_diff="name=\"Y_DIFF\""
|
||||
|
||||
while read line
|
||||
do
|
||||
if [[ $line =~ $group ]]
|
||||
then
|
||||
let group_count=$group_count+1
|
||||
temp=${line#*\"}
|
||||
echo "GAZEBO_IP:$GAZEBO_IP" >> $file
|
||||
echo "NAMESPACE:${temp%\"*}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $port ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "SOCKET_PORT:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_X:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_Y:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "X_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "Y_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
done < $launch_file
|
||||
|
||||
i=0
|
||||
index=0
|
||||
while read line
|
||||
do
|
||||
case $(($i%7)) in
|
||||
0) gazebo_ip[$index]=$line;;
|
||||
1) namespace[$index]=$line;;
|
||||
2) socket_port[$index]=$line;;
|
||||
3) x_diff[$index]=$line;;
|
||||
4) y_diff[$index]=$line;;
|
||||
5) start_x[$index]=$line;;
|
||||
6) start_y[$index]=$line;let index++;;
|
||||
esac
|
||||
let i++
|
||||
done < $file
|
||||
|
||||
#`rm $file`
|
||||
|
||||
for((i=0;i<index;i++))
|
||||
do
|
||||
`sshpass -p '123456' ssh $2 "sudo docker stop ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1; sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
docker_ip[$i]=`sshpass -p '123456' ssh $2 "sudo docker inspect -f '{{range .NetworkSettings.Networks}}{{.IPAddress}}{{end}}' ${namespace[$i]#*:}"`
|
||||
|
||||
export ${namespace[$i]#*:}_ip=${docker_ip[$i]}
|
||||
echo "${namespace[$i]#*:} Done!!"
|
||||
done
|
||||
echo "All Done!!"
|
||||
|
||||
#source /home/zhangshuai/catkin_ws/devel/setup.bash
|
||||
#source /home/ai01/Flight_control/devel/setup.bash
|
||||
|
||||
#`roslaunch hector_quadrotor_gazebo ${launch_file##*/}`
|
||||
|
||||
#`roslaunch hector_quadrotor_gazebo /home/ai01/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/udp_sim_10plane_gazebo_new_split.launch`
|
||||
|
|
@ -0,0 +1,93 @@
|
|||
#!/bin/bash
|
||||
|
||||
launch_file=$1
|
||||
docker_ssh=$2
|
||||
|
||||
GAZEBO_IP=`ifconfig $3 |grep "inet "| cut -f 2 -d ":"|cut -f 1 -d " "`
|
||||
|
||||
group_count=0
|
||||
|
||||
file=envfile
|
||||
if [ -f $file ]
|
||||
then
|
||||
`rm -rf $file`
|
||||
fi
|
||||
`touch $file`
|
||||
|
||||
group="ns="
|
||||
port="htonsnumber"
|
||||
x="name=\"x\""
|
||||
y="name=\"y\""
|
||||
x_diff="name=\"X_DIFF\""
|
||||
y_diff="name=\"Y_DIFF\""
|
||||
|
||||
while read line
|
||||
do
|
||||
if [[ $line =~ $group ]]
|
||||
then
|
||||
let group_count=$group_count+1
|
||||
temp=${line#*\"}
|
||||
echo "GAZEBO_IP:$GAZEBO_IP" >> $file
|
||||
echo "NAMESPACE:${temp%\"*}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $port ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "SOCKET_PORT:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_X:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_Y:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "X_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "Y_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
done < $launch_file
|
||||
|
||||
i=0
|
||||
index=0
|
||||
while read line
|
||||
do
|
||||
case $(($i%7)) in
|
||||
0) gazebo_ip[$index]=$line;;
|
||||
1) namespace[$index]=$line;;
|
||||
2) socket_port[$index]=$line;;
|
||||
3) x_diff[$index]=$line;;
|
||||
4) y_diff[$index]=$line;;
|
||||
5) start_x[$index]=$line;;
|
||||
6) start_y[$index]=$line;let index++;;
|
||||
esac
|
||||
let i++
|
||||
done < $file
|
||||
|
||||
#`rm $file`
|
||||
|
||||
for((i=0;i<index;i++))
|
||||
do
|
||||
`sshpass -p '123456' ssh $2 "sudo docker stop ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
echo "${namespace[$i]#*:} Done!!"
|
||||
done
|
||||
echo "All Done!!"
|
||||
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
#!/bin/bash
|
||||
|
||||
launch_file=$1
|
||||
docker_ssh=$2
|
||||
|
||||
GAZEBO_IP=`ifconfig $3 |grep "inet "| cut -f 2 -d ":"|cut -f 1 -d " "`
|
||||
|
||||
group_count=0
|
||||
|
||||
file=envfile
|
||||
if [ -f $file ]
|
||||
then
|
||||
`rm -rf $file`
|
||||
fi
|
||||
`touch $file`
|
||||
|
||||
group="ns="
|
||||
port="htonsnumber"
|
||||
x="name=\"x\""
|
||||
y="name=\"y\""
|
||||
x_diff="name=\"X_DIFF\""
|
||||
y_diff="name=\"Y_DIFF\""
|
||||
|
||||
while read line
|
||||
do
|
||||
if [[ $line =~ $group ]]
|
||||
then
|
||||
let group_count=$group_count+1
|
||||
temp=${line#*\"}
|
||||
echo "GAZEBO_IP:$GAZEBO_IP" >> $file
|
||||
echo "NAMESPACE:${temp%\"*}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $port ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "SOCKET_PORT:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_X:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_Y:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "X_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "Y_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
done < $launch_file
|
||||
|
||||
i=0
|
||||
index=0
|
||||
while read line
|
||||
do
|
||||
case $(($i%7)) in
|
||||
0) gazebo_ip[$index]=$line;;
|
||||
1) namespace[$index]=$line;;
|
||||
2) socket_port[$index]=$line;;
|
||||
3) x_diff[$index]=$line;;
|
||||
4) y_diff[$index]=$line;;
|
||||
5) start_x[$index]=$line;;
|
||||
6) start_y[$index]=$line;let index++;;
|
||||
esac
|
||||
let i++
|
||||
done < $file
|
||||
|
||||
#`rm $file`
|
||||
|
||||
for((i=0;i<index;i++))
|
||||
do
|
||||
`sshpass -p 'zhangmuhan' ssh $2 "sudo docker stop ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1; sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
docker_ip[$i]=`sshpass -p '123456' ssh $2 "sudo docker inspect -f '{{range .NetworkSettings.Networks}}{{.IPAddress}}{{end}}' ${namespace[$i]#*:}"`
|
||||
|
||||
export ${namespace[$i]#*:}_ip=${docker_ip[$i]}
|
||||
echo "${namespace[$i]#*:} Done!!"
|
||||
done
|
||||
echo "All Done!!"
|
||||
|
||||
#source /home/zhangshuai/catkin_ws/devel/setup.bash
|
||||
#source /home/ai01/Flight_control/devel/setup.bash
|
||||
|
||||
#`roslaunch hector_quadrotor_gazebo ${launch_file##*/}`
|
||||
|
||||
#`roslaunch hector_quadrotor_gazebo /home/ai01/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo/launch/udp_sim_10plane_gazebo_new_split.launch`
|
||||
|
|
@ -0,0 +1,93 @@
|
|||
#!/bin/bash
|
||||
|
||||
launch_file=$1
|
||||
docker_ssh=$2
|
||||
|
||||
GAZEBO_IP=`ifconfig $3 |grep "inet "| cut -f 2 -d ":"|cut -f 1 -d " "`
|
||||
|
||||
group_count=0
|
||||
|
||||
file=envfile
|
||||
if [ -f $file ]
|
||||
then
|
||||
`rm -rf $file`
|
||||
fi
|
||||
`touch $file`
|
||||
|
||||
group="ns="
|
||||
port="htonsnumber"
|
||||
x="name=\"x\""
|
||||
y="name=\"y\""
|
||||
x_diff="name=\"X_DIFF\""
|
||||
y_diff="name=\"Y_DIFF\""
|
||||
|
||||
while read line
|
||||
do
|
||||
if [[ $line =~ $group ]]
|
||||
then
|
||||
let group_count=$group_count+1
|
||||
temp=${line#*\"}
|
||||
echo "GAZEBO_IP:$GAZEBO_IP" >> $file
|
||||
echo "NAMESPACE:${temp%\"*}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $port ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "SOCKET_PORT:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_X:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "START_Y:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $x_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "X_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
if [[ $line =~ $y_diff ]]
|
||||
then
|
||||
temp=${line%\"*}
|
||||
echo "Y_DIFF:${temp##*\"}" >> $file
|
||||
fi
|
||||
|
||||
done < $launch_file
|
||||
|
||||
i=0
|
||||
index=0
|
||||
while read line
|
||||
do
|
||||
case $(($i%7)) in
|
||||
0) gazebo_ip[$index]=$line;;
|
||||
1) namespace[$index]=$line;;
|
||||
2) socket_port[$index]=$line;;
|
||||
3) x_diff[$index]=$line;;
|
||||
4) y_diff[$index]=$line;;
|
||||
5) start_x[$index]=$line;;
|
||||
6) start_y[$index]=$line;let index++;;
|
||||
esac
|
||||
let i++
|
||||
done < $file
|
||||
|
||||
#`rm $file`
|
||||
|
||||
for((i=0;i<index;i++))
|
||||
do
|
||||
`sshpass -p 'zhangmuhan' ssh $2 "sudo docker stop ${namespace[$i]#*:} >/dev/null 2>&1; sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker rm ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker create --name ${namespace[$i]#*:} -e NAMESPACE=${namespace[$i]#*:} -e GAZEBO_IP=${gazebo_ip[$i]#*:} -e SOCKET_PORT=${socket_port[$i]#*:} -e START_X=${start_x[$i]#*:} -e START_Y=${start_y[$i]#*:} -e X_DIFF=${x_diff[$i]#*:} -e Y_DIFF=${y_diff[$i]#*:} $4 >/dev/null 2>&1"`
|
||||
# `sshpass -p '123123' ssh $2 "sudo docker start ${namespace[$i]#*:} >/dev/null 2>&1"`
|
||||
echo "${namespace[$i]#*:} Done!!"
|
||||
done
|
||||
echo "All Done!!"
|
||||
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
{
|
||||
"configurations": [
|
||||
{
|
||||
"browse": {
|
||||
"databaseFilename": "",
|
||||
"limitSymbolsToIncludedHeaders": true
|
||||
},
|
||||
"includePath": [
|
||||
"/home/zhangshuai/Flight_control/devel/include",
|
||||
"/opt/ros/kinetic/include",
|
||||
"/home/zhangshuai/Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include",
|
||||
"/home/zhangshuai/Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/include",
|
||||
"/home/zhangshuai/Flight_control/src/hector_quadrotor/hector_quadrotor_gazebo_plugins/include",
|
||||
"/home/zhangshuai/Flight_control/src/hector_quadrotor/hector_quadrotor_model/include",
|
||||
"/home/zhangshuai/Flight_control/src/hector_quadrotor/hector_quadrotor_pose_estimation/include",
|
||||
"/home/zhangshuai/Flight_control/src/hector_quadrotor/hector_uav_msgs/include",
|
||||
"/home/zhangshuai/Flight_control/src/rosflight_ws/rosflight/rosflight/include",
|
||||
"/home/zhangshuai/Flight_control/src/rosflight_ws/rosflight/rosflight_firmware/include",
|
||||
"/home/zhangshuai/Flight_control/src/rosflight_ws/rosflight_plugins/include",
|
||||
"/home/zhangshuai/Flight_control/src/rosflight_ws/rosflight/rosflight_sim/include",
|
||||
"/home/zhangshuai/Flight_control/src/rosflight_ws/rosflight/rosflight_utils/include",
|
||||
"/home/zhangshuai/Flight_control/src/rosflight_ws/rosplane/rosplane/include",
|
||||
"/home/zhangshuai/Flight_control/src/rosflight_ws/rosplane/rosplane_sim/include",
|
||||
"/usr/include"
|
||||
],
|
||||
"name": "Linux",
|
||||
"intelliSenseMode": "gcc-x64"
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
}
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
{
|
||||
"python.autoComplete.extraPaths": [
|
||||
"/home/zhangshuai/Flight_control/devel/lib/python2.7/dist-packages",
|
||||
"/opt/ros/kinetic/lib/python2.7/dist-packages"
|
||||
],
|
||||
"files.associations": {
|
||||
"*.ipp": "cpp",
|
||||
"cctype": "cpp",
|
||||
"clocale": "cpp",
|
||||
"cmath": "cpp",
|
||||
"csignal": "cpp",
|
||||
"cstdarg": "cpp",
|
||||
"cstddef": "cpp",
|
||||
"cstdio": "cpp",
|
||||
"cstdlib": "cpp",
|
||||
"cstring": "cpp",
|
||||
"ctime": "cpp",
|
||||
"cwchar": "cpp",
|
||||
"cwctype": "cpp",
|
||||
"atomic": "cpp",
|
||||
"strstream": "cpp",
|
||||
"*.tcc": "cpp",
|
||||
"chrono": "cpp",
|
||||
"codecvt": "cpp",
|
||||
"condition_variable": "cpp",
|
||||
"cstdint": "cpp",
|
||||
"future": "cpp",
|
||||
"initializer_list": "cpp",
|
||||
"iosfwd": "cpp",
|
||||
"mutex": "cpp",
|
||||
"ratio": "cpp",
|
||||
"system_error": "cpp",
|
||||
"thread": "cpp",
|
||||
"cinttypes": "cpp",
|
||||
"typeindex": "cpp"
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1 @@
|
|||
/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
################################################################################
|
||||
# CMake
|
||||
################################################################################
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(enemy_suv_description)
|
||||
|
||||
################################################################################
|
||||
# Packages
|
||||
################################################################################
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
urdf
|
||||
xacro
|
||||
)
|
||||
|
||||
################################################################################
|
||||
# Declare ROS messages, services and actions
|
||||
################################################################################
|
||||
|
||||
################################################################################
|
||||
# Declare ROS dynamic reconfigure parameters
|
||||
################################################################################
|
||||
|
||||
################################################################################
|
||||
# Catkin specific configuration
|
||||
################################################################################
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS urdf xacro
|
||||
)
|
||||
|
||||
################################################################################
|
||||
# Build
|
||||
################################################################################
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
################################################################################
|
||||
# Install
|
||||
################################################################################
|
||||
install(DIRECTORY meshes materials urdf launch
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
################################################################################
|
||||
# Test
|
||||
################################################################################
|
||||
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<arg name="suv_name" default="default_enemy"/>
|
||||
<arg name="model" default="$(find rosplane_sim)/xacro/$(arg suv_name)/$(arg suv_name).xacro"/>
|
||||
<arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
|
||||
<arg name="x" default="0.0"/>
|
||||
<arg name="y" default="0.0"/>
|
||||
<arg name="yaw" default="0.0"/>
|
||||
<arg name="z" default="0.1"/>
|
||||
<arg name="enable_logging" default="false"/>
|
||||
<arg name="enable_ground_truth" default="true"/>
|
||||
<arg name="log_file" default="$(arg suv_name)"/>
|
||||
<arg name="enable_wind" default="true"/>
|
||||
<arg name="robot_namespace" default="/"/>
|
||||
<arg name="gazebo_namespace" default=""/>
|
||||
|
||||
<!-- send the robot XML to param server -->
|
||||
<param name="robot_description" command="
|
||||
$(find xacro)/xacro.py '$(arg model)'
|
||||
suv_name:=$(arg suv_name)
|
||||
gazebo_namespace:=$(arg gazebo_namespace)/gazebo
|
||||
robot_namespace:=$(arg robot_namespace)"
|
||||
/>
|
||||
<param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
|
||||
|
||||
<!-- push robot_description to factory and spawn robot in gazebo -->
|
||||
<node name="spawn_$(arg suv_name)" pkg="gazebo_ros" type="spawn_model"
|
||||
args="-param robot_description
|
||||
-urdf
|
||||
-x $(arg x)
|
||||
-y $(arg y)
|
||||
-z $(arg z)
|
||||
-Y $(arg yaw)
|
||||
|
||||
-model $(arg suv_name)"
|
||||
respawn="false" output="screen">
|
||||
</node>
|
||||
</launch>
|
||||
<!-- -gazebo_namespace $(arg gazebo_namespace)/gazebo -->
|
||||
<!-- -robot_namespace $(arg robot_namespace) -->
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
<launch>
|
||||
<arg name="suv_name" default="enemy"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="paused" value="true"/>
|
||||
<arg name="gui" value="true"/>
|
||||
<arg name="verbose" value="true"/>
|
||||
<arg name="debug" value="false"/>
|
||||
<!-- <arg name="world_name" value="$(find enemy_suv_description)/worlds/kunming_airport.world"/> -->
|
||||
<arg name="world_name" value="$(find enemy_suv_description)/worlds/kunming/kunming.world"/>
|
||||
</include>
|
||||
|
||||
|
||||
<!-- <group ns="$(arg suv_name)"> -->
|
||||
<arg name="model" value="$(find enemy_suv_description)/urdf/enemy_suv.urdf.xacro"/>
|
||||
|
||||
<include file="$(find enemy_suv_description)/launch/spawn_enemy_suv.launch">
|
||||
<arg name="suv_name" value="$(arg suv_name)" />
|
||||
<arg name="tf_prefix" value="$(arg suv_name)" />
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="x" value="0.0" />
|
||||
<arg name="y" value="0.0" />
|
||||
<arg name="z" value="191.0" />
|
||||
</include>
|
||||
<!-- <node name="enemy_suv_teleop" pkg="enemy_suv_teleop" type="enemy_suv_teleop"/> -->
|
||||
<!-- </group> -->
|
||||
<node name="autopilot_pseudo" pkg="suv_move" type="suv_pseudo_controller" output="screen">
|
||||
<!-- <remap from="state" to="truth"/> -->
|
||||
</node>
|
||||
|
||||
<node name="pathfollower" pkg="suv_move" type="suv_path_follower" output="screen">
|
||||
<!-- <remap from="state" to="truth"/> -->
|
||||
</node>
|
||||
|
||||
<node name="pathmanager" pkg="suv_move" type="suv_path_manager" output="screen">
|
||||
<!-- <remap from="state" to="truth"/> -->
|
||||
</node>
|
||||
<node name="pathplanner" pkg="suv_move" type="suv_path_planner"/>
|
||||
|
||||
</launch>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 736 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 859 KiB |
|
|
@ -0,0 +1,30 @@
|
|||
# 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware
|
||||
# File Created: 02.10.2016 09:23:07
|
||||
|
||||
newmtl SUV_Body
|
||||
Ns 10.0000
|
||||
Ni 1.5000
|
||||
d 1.0000
|
||||
Tr 0.0000
|
||||
Tf 1.0000 1.0000 1.0000
|
||||
illum 2
|
||||
Ka 0.5882 0.5882 0.5882
|
||||
Kd 0.5882 0.5882 0.5882
|
||||
Ks 0.0000 0.0000 0.0000
|
||||
Ke 0.0000 0.0000 0.0000
|
||||
map_Ka suv.png
|
||||
map_Kd suv.png
|
||||
|
||||
newmtl SUV_Wheels
|
||||
Ns 10.0000
|
||||
Ni 1.5000
|
||||
d 1.0000
|
||||
Tr 0.0000
|
||||
Tf 1.0000 1.0000 1.0000
|
||||
illum 2
|
||||
Ka 0.5882 0.5882 0.5882
|
||||
Kd 0.5882 0.5882 0.5882
|
||||
Ks 0.0000 0.0000 0.0000
|
||||
Ke 0.0000 0.0000 0.0000
|
||||
map_Ka wheels_01.png
|
||||
map_Kd wheels_01.png
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,14 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>enemy_suv_description</name>
|
||||
<version>0.2.1</version>
|
||||
<description>
|
||||
3D models of the enemy_suv for simulation and visualization
|
||||
</description>
|
||||
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>urdf</depend>
|
||||
<depend>xacro</depend>
|
||||
<depend>launch</depend>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,138 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot name="enemy_suv"
|
||||
xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<xacro:arg name="laser_visual" default="false"/>
|
||||
<xacro:arg name="imu_visual" default="false"/>
|
||||
|
||||
<gazebo>
|
||||
<plugin name="enemy_suv_controller" filename="libgazebo_ros_diff_drive.so">
|
||||
<commandTopic>cmd_vel</commandTopic>
|
||||
<odometryTopic>odom</odometryTopic>
|
||||
<odometryFrame>odom</odometryFrame>
|
||||
<robotBaseFrame>base_link</robotBaseFrame>
|
||||
<publishWheelTF>false</publishWheelTF>
|
||||
<publishWheelJointState>true</publishWheelJointState>
|
||||
<legacyMode>false</legacyMode>
|
||||
<updateRate>100</updateRate>
|
||||
<leftJoint>wheel_left_joint</leftJoint>
|
||||
<rightJoint>wheel_right_joint</rightJoint>
|
||||
<wheelSeparation>2.160</wheelSeparation>
|
||||
<wheelDiameter>0.66</wheelDiameter>
|
||||
<wheelAcceleration>10</wheelAcceleration>
|
||||
<wheelTorque>100</wheelTorque>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<gazebo>
|
||||
<plugin name="enemy_suv_groundtruth_sim" filename="libgazebo_ros_p3d.so">
|
||||
<updateRate>100.0</updateRate>
|
||||
<bodyName>base_link</bodyName>
|
||||
<topicName>ground_truth/state</topicName>
|
||||
<gaussianNoise>0.0</gaussianNoise>
|
||||
<frameName>world</frameName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="wheel_left_link">
|
||||
<mu1>100</mu1>
|
||||
<mu2>100</mu2>
|
||||
<kp>500000.0</kp>
|
||||
<kd>10.0</kd>
|
||||
<minDepth>0.001</minDepth>
|
||||
<maxVel>0.1</maxVel>
|
||||
<fdir1>1 0 0</fdir1>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="wheel_right_link">
|
||||
<mu1>100</mu1>
|
||||
<mu2>100</mu2>
|
||||
<kp>500000.0</kp>
|
||||
<kd>10.0</kd>
|
||||
<minDepth>0.001</minDepth>
|
||||
<maxVel>0.1</maxVel>
|
||||
<fdir1>1 0 0</fdir1>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
</gazebo>
|
||||
|
||||
<!-- <gazebo reference="caster_back_link">
|
||||
<mu1>0.0</mu1>
|
||||
<mu2>0.0</mu2>
|
||||
<kp>1000000.0</kp>
|
||||
<kd>100.0</kd>
|
||||
<minDepth>0.001</minDepth>
|
||||
<maxVel>1.0</maxVel>
|
||||
<material>Gazebo/Grey</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo>
|
||||
<plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<bodyName>imu_link</bodyName>
|
||||
<topicName>imu</topicName>
|
||||
<serviceName>imu_service</serviceName>
|
||||
<gaussianNoise>0.0</gaussianNoise>
|
||||
<updateRate>100.0</updateRate>
|
||||
<imu>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<rate>
|
||||
<mean>0.0</mean>
|
||||
<stddev>2e-4</stddev>
|
||||
<bias_mean>0.0000075</bias_mean>
|
||||
<bias_stddev>0.0000008</bias_stddev>
|
||||
</rate>
|
||||
<accel>
|
||||
<mean>0.0</mean>
|
||||
<stddev>1.7e-2</stddev>
|
||||
<bias_mean>0.1</bias_mean>
|
||||
<bias_stddev>0.001</bias_stddev>
|
||||
</accel>
|
||||
</noise>
|
||||
</imu>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="imu_link">
|
||||
<sensor type="imu" name="imu">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>50</update_rate>
|
||||
<visualize>$(arg imu_visual)</visualize>
|
||||
</sensor>
|
||||
<material>Gazebo/Grey</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_scan">
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
<sensor type="ray" name="lds_lfcd_sensor">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<visualize>$(arg laser_visual)</visualize>
|
||||
<update_rate>1800</update_rate>
|
||||
<ray>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>360</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>0.0</min_angle>
|
||||
<max_angle>6.28</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>0.120</min>
|
||||
<max>3.5</max>
|
||||
<resolution>0.015</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</ray>
|
||||
<plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
|
||||
<topicName>scan</topicName>
|
||||
<frameName>base_scan</frameName>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo> -->
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,229 @@
|
|||
<?xml version="1.0" ?>
|
||||
<robot name="enemy_suv" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<xacro:property name="M_PI" value="3.1415926535897931" />
|
||||
<xacro:include filename="$(find enemy_suv_description)/urdf/enemy_suv.gazebo.xacro"/>
|
||||
|
||||
<!-- <link name="base_footprint"/>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link"/>
|
||||
<origin xyz="0.0 0.0 0.010" rpy="0 0 0"/>
|
||||
</joint> -->
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin xyz="0.0 0.0 0.0" rpy="0 0 -1.57079632679"/>
|
||||
<geometry>
|
||||
<mesh filename="package://enemy_suv_description/meshes/suv.obj" scale="0.06 0.06 0.06"/>
|
||||
</geometry>
|
||||
<!-- <material name="light_black"/> -->
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0.0 0.0 0.0" rpy="0 0 -1.57079632679"/>
|
||||
<geometry>
|
||||
<mesh filename="package://enemy_suv_description/meshes/suv.obj" scale="0.06 0.06 0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0.0 0 0" rpy="0 0 -1.57079632679"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
|
||||
iyy="0.01" iyz="0.0"
|
||||
izz="0.01" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="wheel_left_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_left_link"/>
|
||||
<origin xyz="2.0 1.08 0.32" rpy="${-M_PI*0.5} 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<link name="wheel_left_link">
|
||||
<!-- <visual>
|
||||
<origin xyz="0 0 0" rpy="${M_PI*0.5} 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="dark"/>
|
||||
</visual> -->
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.18" radius="0.33"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.01" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.001" iyz="0.0"
|
||||
izz="0.001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="wheel_right_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_right_link"/>
|
||||
<origin xyz="2.0 -1.08 0.32" rpy="${-M_PI*0.5} 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<link name="wheel_right_link">
|
||||
<!-- <visual>
|
||||
<origin xyz="0 0 0" rpy="${M_PI*0.5} 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="dark"/>
|
||||
</visual> -->
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.18" radius="0.33"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.01" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.001" iyz="0.0"
|
||||
izz="0.001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="wheel_left_back_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_left_back_link"/>
|
||||
<origin xyz="-2.0 1.08 0.32" rpy="${-M_PI*0.5} 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<link name="wheel_left_back_link">
|
||||
<!-- <visual>
|
||||
<origin xyz="0 0 0" rpy="${M_PI*0.5} 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="dark"/>
|
||||
</visual> -->
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.18" radius="0.33"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.01" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.001" iyz="0.0"
|
||||
izz="0.001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="wheel_right_back_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_right_back_link"/>
|
||||
<origin xyz="-2.0 -1.08 0.32" rpy="${-M_PI*0.5} 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
</joint>
|
||||
|
||||
<link name="wheel_right_back_link">
|
||||
<!-- <visual>
|
||||
<origin xyz="0 0 0" rpy="${M_PI*0.5} 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="dark"/>
|
||||
</visual> -->
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.18" radius="0.33"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.01" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.001" iyz="0.0"
|
||||
izz="0.001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- <joint name="caster_back_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="caster_back_link"/>
|
||||
<origin xyz="-0.081 0 -0.004" rpy="${-M_PI*0.5} 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="caster_back_link">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.030 0.010 0.020"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.01" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.001" iyz="0.0"
|
||||
izz="0.001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="imu_link"/>
|
||||
<origin xyz="-0.032 0 0.068" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="imu_link"/>
|
||||
|
||||
<joint name="scan_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="base_scan"/>
|
||||
<origin xyz="-0.032 0 0.171" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="base_scan">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="dark"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.0408" radius="0.070"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<inertial>
|
||||
<mass value="0.01" />
|
||||
<origin xyz="0 0 0" />
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.001" iyz="0.0"
|
||||
izz="0.001" />
|
||||
</inertial>
|
||||
</link> -->
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
<?xml version="1.0" ?>
|
||||
<sdf version="1.4">
|
||||
<world name="default">
|
||||
<!-- A global light source -->
|
||||
<include>
|
||||
<uri>model://sun</uri>
|
||||
</include>
|
||||
|
||||
<include>
|
||||
<uri>model://kunming</uri>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
|
||||
<scene>
|
||||
<ambient>0.68 0.68 0.68 1.0</ambient>
|
||||
<sky>
|
||||
<sunrise/>
|
||||
<clouds>
|
||||
<speed>0</speed>
|
||||
</clouds>
|
||||
</sky>
|
||||
</scene>
|
||||
|
||||
<!-- <actor name="animated_quadrotor_0">
|
||||
<include>
|
||||
<uri>model://quadrotor</uri>
|
||||
<pose>-1 -2 10 0 0 0</pose>
|
||||
</include>
|
||||
<script>
|
||||
<loop>true</loop>
|
||||
<auto_start>true</auto_start>
|
||||
<trajectory id="0" type="square">
|
||||
<waypoint>
|
||||
<time>0.0</time>
|
||||
<pose>-1 -2 10 0 0 0</pose>
|
||||
</waypoint>
|
||||
<waypoint>
|
||||
<time>1.0</time>
|
||||
<pose>-1 2 10 0 0 0</pose>
|
||||
</waypoint>
|
||||
<waypoint>
|
||||
<time>2.0</time>
|
||||
<pose>-1 -2 10 0 0 0</pose>
|
||||
</waypoint>
|
||||
</trajectory>
|
||||
</script>
|
||||
</actor> 不支持gazebo7版本-->
|
||||
|
||||
|
||||
<physics type="ode">
|
||||
<real_time_update_rate>1000</real_time_update_rate>
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
<!-- <ode>
|
||||
<solver>
|
||||
<type>quick</type>
|
||||
<iters>20</iters>
|
||||
<precon_iters>0</precon_iters>
|
||||
<sor>1.300000</sor>
|
||||
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
|
||||
</solver>
|
||||
<constraints>
|
||||
<cfm>0.00</cfm>
|
||||
<erp>0.2</erp>
|
||||
<contact_max_correcting_vel>10000.000000</contact_max_correcting_vel>
|
||||
<contact_surface_layer>0.01000</contact_surface_layer>
|
||||
</constraints>
|
||||
</ode> -->
|
||||
</physics>
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 14 MiB |
|
|
@ -0,0 +1,20 @@
|
|||
material kunming/Image
|
||||
{
|
||||
technique
|
||||
{
|
||||
pass
|
||||
{
|
||||
ambient .8 .8 .8 1.000000
|
||||
diffuse .5 .5 .5 1.000000
|
||||
specular 1 1 1 5.00000
|
||||
emissive 0.01 0.01 0.01 1.000000
|
||||
|
||||
texture_unit
|
||||
{
|
||||
texture km.jpg
|
||||
filtering trilinear
|
||||
scale 1 1
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
<?xml version="1.0" ?>
|
||||
<sdf version="1.4">
|
||||
<world name="default">
|
||||
<!-- A global light source -->
|
||||
<include>
|
||||
<uri>model://sun</uri>
|
||||
</include>
|
||||
|
||||
<!-- <include>
|
||||
<uri>model://kunming_airport</uri>
|
||||
<pose>65 45 250 0 0 0</pose>
|
||||
</include> -->
|
||||
|
||||
<scene>
|
||||
<ambient>0.68 0.68 0.68 1.0</ambient>
|
||||
<sky>
|
||||
<sunrise/>
|
||||
<clouds>
|
||||
<speed>0</speed>
|
||||
</clouds>
|
||||
</sky>
|
||||
</scene>
|
||||
|
||||
<model name="heightmap">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<heightmap>
|
||||
<uri>file:///home/zhangshuai/micros_back/src/micros/tools/simulator/turtlebot_simulator/turtlebot_gazebo/worlds/kunmingjichang.dem</uri>
|
||||
<!-- <uri>file://kunmingjichang.dem</uri> -->
|
||||
<!--<size>150 150 50</size>-->
|
||||
<!-- <size>1000 1000 1000</size> -->
|
||||
<pos>0 0 -1900</pos>
|
||||
</heightmap>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="visual_abcedf">
|
||||
<geometry>
|
||||
<heightmap>
|
||||
<uri>file:///home/zhangshuai/micros_back/src/micros/tools/simulator/turtlebot_simulator/turtlebot_gazebo/worlds/kunmingjichang.dem</uri>
|
||||
<!-- <uri>file://kunmingjichang.dem</uri> -->
|
||||
<!--<size>150 150 50</size>-->
|
||||
<!-- <size>1000 1000 1000</size> -->
|
||||
<pos>0 0 -1900</pos>
|
||||
</heightmap>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file:///home/zhangshuai/micros_back/src/micros/tools/simulator/turtlebot_simulator/turtlebot_gazebo/worlds/kunming.material</uri>
|
||||
<!-- <uri>file://kunming.material</uri> -->
|
||||
<name>kunming/Image</name>
|
||||
</script>
|
||||
</material>
|
||||
|
||||
</visual>
|
||||
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- <physics type="ode">
|
||||
<real_time_update_rate>50.0</real_time_update_rate>
|
||||
<max_step_size>0.02</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor> -->
|
||||
<!-- <ode>
|
||||
<solver>
|
||||
<type>quick</type>
|
||||
<iters>20</iters>
|
||||
<precon_iters>0</precon_iters>
|
||||
<sor>1.300000</sor>
|
||||
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
|
||||
</solver>
|
||||
<constraints>
|
||||
<cfm>0.00</cfm>
|
||||
<erp>0.2</erp>
|
||||
<contact_max_correcting_vel>10000.000000</contact_max_correcting_vel>
|
||||
<contact_surface_layer>0.01000</contact_surface_layer>
|
||||
</constraints>
|
||||
</ode> -->
|
||||
<!-- </physics> -->
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,78 @@
|
|||
<?xml version="1.0" ?>
|
||||
<sdf version="1.4">
|
||||
<world name="default">
|
||||
<!-- A global light source -->
|
||||
<include>
|
||||
<uri>model://sun</uri>
|
||||
</include>
|
||||
|
||||
<include>
|
||||
<uri>model://kunming_airport</uri>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
</include>
|
||||
|
||||
<include>
|
||||
<uri>model://suv</uri> <!-- SUV模型 -->
|
||||
<pose>602.050000 -729.260000 0 0 0 0.9</pose> <!-- 初始位姿（x,y,z,滚转，俯仰，偏航） -->
|
||||
<!-- <pose>745.208000 -553.204000 0 0 0 -1.57</pose> -->
|
||||
</include>
|
||||
|
||||
<scene>
|
||||
<ambient>0.68 0.68 0.68 1.0</ambient>
|
||||
<sky>
|
||||
<sunrise/>
|
||||
<clouds>
|
||||
<speed>0</speed>
|
||||
</clouds>
|
||||
</sky>
|
||||
</scene>
|
||||
|
||||
<!-- <actor name="animated_quadrotor_0">
|
||||
<include>
|
||||
<uri>model://quadrotor</uri>
|
||||
<pose>-1 -2 10 0 0 0</pose>
|
||||
</include>
|
||||
<script>
|
||||
<loop>true</loop>
|
||||
<auto_start>true</auto_start>
|
||||
<trajectory id="0" type="square">
|
||||
<waypoint>
|
||||
<time>0.0</time>
|
||||
<pose>-1 -2 10 0 0 0</pose>
|
||||
</waypoint>
|
||||
<waypoint>
|
||||
<time>1.0</time>
|
||||
<pose>-1 2 10 0 0 0</pose>
|
||||
</waypoint>
|
||||
<waypoint>
|
||||
<time>2.0</time>
|
||||
<pose>-1 -2 10 0 0 0</pose>
|
||||
</waypoint>
|
||||
</trajectory>
|
||||
</script>
|
||||
</actor> 不支持gazebo7版本-->
|
||||
|
||||
|
||||
<physics type="ode">
|
||||
<real_time_update_rate>1000</real_time_update_rate>
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
<!-- <ode>
|
||||
<solver>
|
||||
<type>quick</type>
|
||||
<iters>20</iters>
|
||||
<precon_iters>0</precon_iters>
|
||||
<sor>1.300000</sor>
|
||||
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
|
||||
</solver>
|
||||
<constraints>
|
||||
<cfm>0.00</cfm>
|
||||
<erp>0.2</erp>
|
||||
<contact_max_correcting_vel>10000.000000</contact_max_correcting_vel>
|
||||
<contact_surface_layer>0.01000</contact_surface_layer>
|
||||
</constraints>
|
||||
</ode> -->
|
||||
</physics>
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(enemy_suv_msgs)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
std_msgs
|
||||
message_generation
|
||||
)
|
||||
|
||||
add_message_files(
|
||||
FILES
|
||||
Controller_Commands.msg
|
||||
Current_Path.msg
|
||||
Goal_Info.msg
|
||||
Controller_Internals.msg
|
||||
State.msg
|
||||
State29.msg
|
||||
Up_Data_New.msg
|
||||
Down_Data_New.msg
|
||||
Formation_Type.msg
|
||||
Waypoint.msg
|
||||
)
|
||||
|
||||
generate_messages(
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS std_msgs
|
||||
)
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
# Controller commands output from the path follower, input to autopilot controller
|
||||
|
||||
# @warning Va_c, h_c and chi_c have always to be valid, the aux array is optional
|
||||
float32 Va_c # Commanded airspeed (m/s)
|
||||
float32 h_c # Commanded altitude (m)
|
||||
float32 chi_c # Commanded course (rad)
|
||||
float32 phi_ff # feed forward command for orbits (rad)
|
||||
float32[4] aux # Optional auxiliary commands
|
||||
bool aux_valid # Auxiliary commands valid
|
||||
bool takeoff
|
||||
bool landing
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
# Controller internals (inner loops, states) output from the autopilot controller, for now just for debuging and ploting
|
||||
|
||||
# @warning theta_c, phi_c and alt_zone have always to be valid, the aux array is optional
|
||||
float32 theta_c # Commanded pitch (rad)
|
||||
float32 phi_c # Commanded roll (rad)
|
||||
uint8 alt_zone # Zone in the altitude state machine
|
||||
float32[4] aux # Optional auxiliary commands
|
||||
bool aux_valid # Auxiliary commands valid
|
||||
|
||||
uint8 ZONE_TAKE_OFF = 0
|
||||
uint8 ZONE_CLIMB = 1
|
||||
uint8 ZONE_DESEND = 2
|
||||
uint8 ZONE_ALTITUDE_HOLD = 3
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
# Current path output from the path manager, input to path follower
|
||||
|
||||
# @warning Va_d have always to be valid,
|
||||
# r and q need to be valid if path_type == LINE_PATH
|
||||
# c, rho, and, lambda need to be valid if path_type == ORBIT_PATH
|
||||
uint8 path_type # Indicates strait line or orbital path
|
||||
float32 Va_d # Desired airspeed (m/s)
|
||||
float32[3] r # Vector to origin of straight line path (m)
|
||||
float32[3] q # Unit vector, desired direction of travel for line path
|
||||
float32[3] c # Center of orbital path (m)
|
||||
float32 rho # Radius of orbital path (m)
|
||||
float32 h_c # desire altitude (m) add by kobe
|
||||
int8 lambda # Direction of orbital path (clockwise is 1, counterclockwise is -1)
|
||||
|
||||
uint8 ORBIT_PATH = 0
|
||||
uint8 LINE_PATH = 1
|
||||
uint8 STAR_PATH = 2 #add by kobe
|
||||
|
||||
int8 CLOCKWISE = 1
|
||||
int8 COUNT_CLOCKWISE = -1
|
||||
|
||||
bool landing
|
||||
bool takeoff
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
int32 status_word
|
||||
float32 longitude
|
||||
float32 latitude
|
||||
float32 altitude
|
||||
float32 plane_speed #desired speed
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
# Vehicle state 'x_hat' output from the estimator or from simulator
|
||||
|
||||
Header header
|
||||
|
||||
# Original States
|
||||
# @warning roll, pitch and yaw have always to be valid, the quaternion is optional
|
||||
float32 formation_type # Airspeed (m/s)
|
||||
|
|
@ -0,0 +1,8 @@
|
|||
# Current path goal output from the path manager, add by kobe
|
||||
|
||||
float32 v_d # Desired airspeed (m/s)
|
||||
float32[2] xy # x-y-coordinate (m)
|
||||
float32[2] ll # latitude-longtitude
|
||||
int32 action # expected action move
|
||||
float32 altitude # desire altitude (m)
|
||||
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
# Vehicle state 'x_hat' output from the estimator or from simulator
|
||||
|
||||
Header header
|
||||
|
||||
# Original States
|
||||
# @warning roll, pitch and yaw have always to be valid, the quaternion is optional
|
||||
float32[3] position # north, east, down (m)
|
||||
float32 Va # Airspeed (m/s)
|
||||
float32 alpha # Angle of attack (rad)
|
||||
float32 beta # Slide slip angle (rad)
|
||||
float32 phi # Roll angle (rad)
|
||||
float32 theta # Pitch angle (rad)
|
||||
float32 psi # Yaw angle (rad)
|
||||
float32 chi # Course angle (rad)
|
||||
float32 p # Body frame rollrate (rad/s)
|
||||
float32 q # Body frame pitchrate (rad/s)
|
||||
float32 r # Body frame yawrate (rad/s)
|
||||
float32 Vg # Groundspeed (m/s)
|
||||
float32 wn # Windspeed north component (m/s)
|
||||
float32 we # Windspeed east component (m/s)
|
||||
|
||||
# add by kobe
|
||||
float32 Wd # latitude -90--90
|
||||
float32 Jd # longtitude -180--180
|
||||
float32 Vn # v-north speed or x
|
||||
float32 Ve # v-east or y
|
||||
float32 Vd # v-down or z
|
||||
|
||||
|
||||
# Additional States for convenience
|
||||
float32[4] quat # Quaternion (wxyz, NED)
|
||||
bool quat_valid # Quaternion valid
|
||||
float32 chi_deg # Wrapped course angle (deg)
|
||||
float32 psi_deg # Wrapped yaw angle (deg)
|
||||
float32 initial_lat # Initial/origin latitude (lat. deg)
|
||||
float32 initial_lon # Initial/origin longitude (lon. deg)
|
||||
float32 initial_alt # Initial/origin altitude (m)
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
# Vehicle state 'x_hat' output from the estimator or from simulator
|
||||
|
||||
Header header
|
||||
|
||||
# Original States
|
||||
# @warning roll, pitch and yaw have always to be valid, the quaternion is optional
|
||||
float32[3] position_gps # latitude (lat. deg), longitude (lon. deg), altitude (m)
|
||||
float32[3] attitude_angle # Roll angle (rad), Pitch angle (rad), Yaw angle (rad)
|
||||
float32[3] velocity # V_north (m/s), V_east (m/s), V_ground (m/s)
|
||||
float32[3] angular_velocity # Body frame rollrate (rad/s), Body frame pitchrate (rad/s), Body frame yawrate (rad/s)
|
||||
float32[3] acceleration # Body frame x (m/s^2), Body frame y (m/s^2), Body frame z (m/s^2)
|
||||
float32 electric_quantity # Quantity of electric charge
|
||||
int8 state_word # Control Status Word
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
int32 status_word
|
||||
float32 longitude
|
||||
float32 latitude
|
||||
float32 altitude # (m)
|
||||
float32 roll_angle #(rad)
|
||||
float32 pitch_angle
|
||||
float32 yaw_angle
|
||||
float32 angular_rate_x #Body frame rollrate (rad/s)
|
||||
float32 angular_rate_y #Body frame pitchrate (rad/s)
|
||||
float32 angular_rate_z #Body frame yawrate (rad/s)
|
||||
float32 north_direction_speed #(m/s)
|
||||
float32 east_direction_speed
|
||||
float32 ground_direction_speed
|
||||
float32 acceleration_x #(m/s^2)
|
||||
float32 acceleration_y
|
||||
float32 acceleration_z
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
# New waypoint, input to path manager
|
||||
|
||||
# @warning w and Va_d always have to be valid; the chi_d is optional.
|
||||
float32[3] w # Waypoint in local NED (m)
|
||||
|
||||
# add by kobe
|
||||
float32 lat # latitude
|
||||
float32 lon # longtitude
|
||||
|
||||
float32 chi_d # Desired course at this waypoint (rad)
|
||||
bool chi_valid # Desired course valid (dubin or fillet paths)
|
||||
float32 Va_d # Desired airspeed (m/s)
|
||||
bool set_current # Sets this waypoint to be executed now! Starts a new list
|
||||
bool clear_wp_list # Removes all waypoints and returns to origin. The rest of
|
||||
# this message will be ignored
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
# New waypoint, input to path manager
|
||||
|
||||
# @warning w and Va_d always have to be valid; the chi_d is optional.
|
||||
float32[3] w # Waypoint in local NED (m)
|
||||
|
||||
# add by kobe
|
||||
float32 lat # latitude
|
||||
float32 lon # longtitude
|
||||
|
||||
float32 chi_d # Desired course at this waypoint (rad)
|
||||
bool chi_valid # Desired course valid (dubin or fillet paths)
|
||||
float32 Va_d # Desired airspeed (m/s)
|
||||
bool set_current # Sets this waypoint to be executed now! Starts a new list
|
||||
bool clear_wp_list # Removes all waypoints and returns to origin. The rest of
|
||||
# this message will be ignored
|
||||
bool landing
|
||||
bool takeoff
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>enemy_suv_msgs</name>
|
||||
<version>0.0.0</version>
|
||||
<description>Message definitions for the ROSplane package</description>
|
||||
|
||||
<maintainer email="gary.ellingson@byu.edu">Gary Ellingson</maintainer>
|
||||
|
||||
<author email="gary.ellingson@byu.edu">Gary Ellingson</author>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
|
||||
|
||||
<export>
|
||||
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package hector_quadrotor_teleop
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.3.5 (2015-03-28)
|
||||
------------------
|
||||
|
||||
0.3.4 (2015-02-22)
|
||||
------------------
|
||||
* Add optional parameter to set the joystick device
|
||||
* Contributors: whoenig
|
||||
|
||||
0.3.3 (2014-09-01)
|
||||
------------------
|
||||
* added run dependency to joy package (joystick drivers)
|
||||
* Contributors: Johannes Meyer
|
||||
|
||||
0.3.2 (2014-03-30)
|
||||
------------------
|
||||
|
||||
0.3.1 (2013-12-26)
|
||||
------------------
|
||||
* added slow button (btn 6 on Logitech Gamepad)
|
||||
* Contributors: Johannes Meyer
|
||||
|
||||
0.3.0 (2013-09-11)
|
||||
------------------
|
||||
* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack
|
||||
* decreased z_velocity_max to 2.0 m/s in logitech_gamepad.launch
|
||||
|
|
@ -0,0 +1,89 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(enemy_suv_teleop)
|
||||
|
||||
## 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 sensor_msgs geometry_msgs hector_uav_msgs)
|
||||
include_directories(include ${catkin_INCLUDE_DIRS})
|
||||
|
||||
## 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()
|
||||
|
||||
|
||||
###################################
|
||||
## 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 you 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
|
||||
CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs hector_uav_msgs
|
||||
DEPENDS
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Declare a cpp library
|
||||
# add_library(@{name}
|
||||
# src/${PROJECT_NAME}/@name.cpp
|
||||
# )
|
||||
|
||||
## Declare a cpp executable
|
||||
add_executable(enemy_suv_teleop src/quadrotor_teleop.cpp)
|
||||
target_link_libraries(enemy_suv_teleop ${catkin_LIBRARIES})
|
||||
|
||||
## Add cmake target dependencies of the executable/library
|
||||
## as an example, message headers may need to be generated before nodes
|
||||
add_dependencies(enemy_suv_teleop ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
#############
|
||||
## 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
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
install(TARGETS enemy_suv_teleop
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_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}
|
||||
# )
|
||||
|
||||
install(DIRECTORY launch
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<arg name="joy_dev" default="/dev/input/js0" />
|
||||
<arg name="control_mode" default="twist" />
|
||||
|
||||
<node name="joy" pkg="joy" type="joy_node" output="screen" >
|
||||
<param name="dev" value="$(arg joy_dev)" />
|
||||
</node>
|
||||
|
||||
<!-- Note that axis IDs are those from the joystick message plus one, to be able to invert axes by specifiying either positive or negative axis numbers.-->
|
||||
<!-- Axis 2 from joy message thus has to be set as '3' or '-3'(inverted mode) below-->
|
||||
<node name="quadrotor_teleop" pkg="hector_quadrotor_teleop" type="quadrotor_teleop">
|
||||
<param name="control_mode" value="$(arg control_mode)" />
|
||||
<param name="x_axis" value="4"/>
|
||||
<param name="y_axis" value="3"/>
|
||||
<param name="z_axis" value="2"/>
|
||||
<param name="yaw_axis" value="1"/>
|
||||
<param name="x_velocity_max" value="5"/>
|
||||
<param name="y_velocity_max" value="5"/>
|
||||
<param name="z_velocity_max" value="2"/>
|
||||
<param name="slow_button" value="6"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<arg name="joy_dev" default="/dev/input/js0" />
|
||||
|
||||
<node name="joy" pkg="joy" type="joy_node" output="screen" >
|
||||
<param name="dev" value="$(arg joy_dev)" />
|
||||
</node>
|
||||
|
||||
<!-- Note that axis IDs are those from the joystick message plus one, to be able to invert axes by specifiying either positive or negative axis numbers.-->
|
||||
<!-- Axis 2 from joy message thus has to be set as '3' or '-3'(inverted mode) below-->
|
||||
<node name="quadrotor_teleop" pkg="hector_quadrotor_teleop" type="quadrotor_teleop">
|
||||
<param name="x_axis" value="5"/>
|
||||
<param name="y_axis" value="4"/>
|
||||
<param name="z_axis" value="2"/>
|
||||
<param name="yaw_axis" value="1"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
<package>
|
||||
<name>enemy_suv_teleop</name>
|
||||
<version>0.3.5</version>
|
||||
<description>hector_quadrotor_teleop enables quadrotor flying with a joystick by
|
||||
processing joy/Joy messages and translating them to geometry_msgs/Twist.</description>
|
||||
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://ros.org/wiki/hector_quadrotor_teleop</url>
|
||||
<url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
|
||||
|
||||
<author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
|
||||
|
||||
<!-- Dependencies which this package needs to build itself. -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<!-- Dependencies needed to compile this package. -->
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>hector_uav_msgs</build_depend>
|
||||
|
||||
<!-- Dependencies needed after this package is compiled. -->
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>hector_uav_msgs</run_depend>
|
||||
<run_depend>joy</run_depend>
|
||||
|
||||
<!-- Dependencies needed only for running tests. -->
|
||||
|
||||
</package>
|
||||
|
|
@ -0,0 +1,215 @@
|
|||
//=================================================================================================
|
||||
// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
|
||||
// All rights reserved.
|
||||
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of the Flight Systems and Automatic Control group,
|
||||
// TU Darmstadt, nor the names of its contributors may be used to
|
||||
// endorse or promote products derived from this software without
|
||||
// specific prior written permission.
|
||||
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
|
||||
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
//=================================================================================================
|
||||
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Joy.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <hector_uav_msgs/YawrateCommand.h>
|
||||
#include <hector_uav_msgs/ThrustCommand.h>
|
||||
#include <hector_uav_msgs/AttitudeCommand.h>
|
||||
|
||||
namespace hector_quadrotor
|
||||
{
|
||||
|
||||
class Teleop
|
||||
{
|
||||
private:
|
||||
ros::NodeHandle node_handle_;
|
||||
ros::Subscriber joy_subscriber_;
|
||||
|
||||
ros::Publisher velocity_publisher_, attitude_publisher_, yawrate_publisher_, thrust_publisher_;
|
||||
geometry_msgs::Twist velocity_;
|
||||
hector_uav_msgs::AttitudeCommand attitude_;
|
||||
hector_uav_msgs::ThrustCommand thrust_;
|
||||
hector_uav_msgs::YawrateCommand yawrate_;
|
||||
|
||||
struct Axis
|
||||
{
|
||||
int axis;
|
||||
double max;
|
||||
};
|
||||
|
||||
struct Button
|
||||
{
|
||||
int button;
|
||||
};
|
||||
|
||||
struct
|
||||
{
|
||||
Axis x;
|
||||
Axis y;
|
||||
Axis z;
|
||||
Axis yaw;
|
||||
} axes_;
|
||||
|
||||
struct
|
||||
{
|
||||
Button slow;
|
||||
} buttons_;
|
||||
|
||||
double slow_factor_;
|
||||
|
||||
public:
|
||||
Teleop()
|
||||
{
|
||||
ros::NodeHandle params("~");
|
||||
|
||||
params.param<int>("x_axis", axes_.x.axis, 4);
|
||||
params.param<int>("y_axis", axes_.y.axis, 3);
|
||||
params.param<int>("z_axis", axes_.z.axis, 2);
|
||||
params.param<int>("yaw_axis", axes_.yaw.axis, 1);
|
||||
|
||||
params.param<double>("yaw_velocity_max", axes_.yaw.max, 90.0 * M_PI / 180.0);
|
||||
params.param<int>("slow_button", buttons_.slow.button, 1);
|
||||
params.param<double>("slow_factor", slow_factor_, 0.2);
|
||||
|
||||
std::string control_mode_str;
|
||||
params.param<std::string>("control_mode", control_mode_str, "twist");
|
||||
|
||||
if (control_mode_str == "twist")
|
||||
{
|
||||
params.param<double>("x_velocity_max", axes_.x.max, 2.0);
|
||||
params.param<double>("y_velocity_max", axes_.y.max, 2.0);
|
||||
params.param<double>("z_velocity_max", axes_.z.max, 2.0);
|
||||
|
||||
joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1));
|
||||
velocity_publisher_ = node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
|
||||
}
|
||||
else if (control_mode_str == "attitude")
|
||||
{
|
||||
params.param<double>("x_roll_max", axes_.x.max, 0.35);
|
||||
params.param<double>("y_pitch_max", axes_.y.max, 0.35);
|
||||
params.param<double>("z_thrust_max", axes_.z.max, 25.0);
|
||||
joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyAttitudeCallback, this, _1));
|
||||
attitude_publisher_ = node_handle_.advertise<hector_uav_msgs::AttitudeCommand>("command/attitude", 10);
|
||||
yawrate_publisher_ = node_handle_.advertise<hector_uav_msgs::YawrateCommand>("command/yawrate", 10);
|
||||
thrust_publisher_ = node_handle_.advertise<hector_uav_msgs::ThrustCommand>("command/thrust", 10);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
~Teleop()
|
||||
{
|
||||
stop();
|
||||
}
|
||||
|
||||
void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy)
|
||||
{
|
||||
velocity_.linear.x = getAxis(joy, axes_.x);
|
||||
velocity_.linear.y = getAxis(joy, axes_.y);
|
||||
velocity_.linear.z = getAxis(joy, axes_.z);
|
||||
velocity_.angular.z = getAxis(joy, axes_.yaw);
|
||||
if (getButton(joy, buttons_.slow.button))
|
||||
{
|
||||
velocity_.linear.x *= slow_factor_;
|
||||
velocity_.linear.y *= slow_factor_;
|
||||
velocity_.linear.z *= slow_factor_;
|
||||
velocity_.angular.z *= slow_factor_;
|
||||
}
|
||||
velocity_publisher_.publish(velocity_);
|
||||
}
|
||||
|
||||
void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy)
|
||||
{
|
||||
attitude_.roll = getAxis(joy, axes_.x);
|
||||
attitude_.pitch = getAxis(joy, axes_.y);
|
||||
attitude_publisher_.publish(attitude_);
|
||||
|
||||
thrust_.thrust = getAxis(joy, axes_.z);
|
||||
thrust_publisher_.publish(thrust_);
|
||||
|
||||
yawrate_.turnrate = getAxis(joy, axes_.yaw);
|
||||
if (getButton(joy, buttons_.slow.button))
|
||||
{
|
||||
yawrate_.turnrate *= slow_factor_;
|
||||
}
|
||||
yawrate_publisher_.publish(yawrate_);
|
||||
}
|
||||
|
||||
sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis)
|
||||
{
|
||||
if (axis.axis == 0)
|
||||
{return 0;}
|
||||
sensor_msgs::Joy::_axes_type::value_type sign = 1.0;
|
||||
if (axis.axis < 0)
|
||||
{
|
||||
sign = -1.0;
|
||||
axis.axis = -axis.axis;
|
||||
}
|
||||
if ((size_t) axis.axis > joy->axes.size())
|
||||
{return 0;}
|
||||
return sign * joy->axes[axis.axis - 1] * axis.max;
|
||||
}
|
||||
|
||||
sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button)
|
||||
{
|
||||
if (button <= 0)
|
||||
{return 0;}
|
||||
if ((size_t) button > joy->buttons.size())
|
||||
{return 0;}
|
||||
return joy->buttons[button - 1];
|
||||
}
|
||||
|
||||
void stop()
|
||||
{
|
||||
if(velocity_publisher_.getNumSubscribers() > 0)
|
||||
{
|
||||
velocity_ = geometry_msgs::Twist();
|
||||
velocity_publisher_.publish(velocity_);
|
||||
}
|
||||
if(attitude_publisher_.getNumSubscribers() > 0)
|
||||
{
|
||||
attitude_ = hector_uav_msgs::AttitudeCommand();
|
||||
attitude_publisher_.publish(attitude_);
|
||||
}
|
||||
if(thrust_publisher_.getNumSubscribers() > 0)
|
||||
{
|
||||
thrust_ = hector_uav_msgs::ThrustCommand();
|
||||
thrust_publisher_.publish(thrust_);
|
||||
}
|
||||
if(yawrate_publisher_.getNumSubscribers() > 0)
|
||||
{
|
||||
yawrate_ = hector_uav_msgs::YawrateCommand();
|
||||
yawrate_publisher_.publish(yawrate_);
|
||||
}
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace hector_quadrotor
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "quadrotor_teleop");
|
||||
|
||||
hector_quadrotor::Teleop teleop;
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,89 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(enemy_suv_teleop_key)
|
||||
|
||||
## 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 geometry_msgs)
|
||||
include_directories(include ${catkin_INCLUDE_DIRS})
|
||||
|
||||
## 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()
|
||||
|
||||
|
||||
###################################
|
||||
## 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 you 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
|
||||
CATKIN_DEPENDS roscpp geometry_msgs
|
||||
DEPENDS
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Declare a cpp library
|
||||
# add_library(@{name}
|
||||
# src/${PROJECT_NAME}/@name.cpp
|
||||
# )
|
||||
|
||||
## Declare a cpp executable
|
||||
add_executable(enemy_suv_teleop_key src/enemy_suv_teleop_key.cpp)
|
||||
target_link_libraries(enemy_suv_teleop_key ${catkin_LIBRARIES})
|
||||
|
||||
## Add cmake target dependencies of the executable/library
|
||||
## as an example, message headers may need to be generated before nodes
|
||||
add_dependencies(enemy_suv_teleop_key ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
#############
|
||||
## 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
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
install(TARGETS enemy_suv_teleop_key
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_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}
|
||||
# )
|
||||
|
||||
install(DIRECTORY launch
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- key to control hector quadrotor -->
|
||||
<group ns="robot_5">
|
||||
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
|
||||
<!-- <param name="cmd_vel_robot" value="/robot_5/cmd_vel"/> -->
|
||||
</node>
|
||||
</group>
|
||||
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
|
||||
</node> -->
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- key to control hector quadrotor -->
|
||||
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
|
||||
</node>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- key to control hector quadrotor -->
|
||||
<group ns="robot_0">
|
||||
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
|
||||
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
|
||||
</node>
|
||||
</group>
|
||||
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
|
||||
</node> -->
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- key to control hector quadrotor -->
|
||||
<group ns="robot_2">
|
||||
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
|
||||
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
|
||||
</node>
|
||||
</group>
|
||||
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
|
||||
</node> -->
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- key to control hector quadrotor -->
|
||||
<group ns="robot_3">
|
||||
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
|
||||
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
|
||||
</node>
|
||||
</group>
|
||||
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
|
||||
</node> -->
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- key to control hector quadrotor -->
|
||||
<group ns="robot_4">
|
||||
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
|
||||
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
|
||||
</node>
|
||||
</group>
|
||||
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
|
||||
</node> -->
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<!-- key to control hector quadrotor -->
|
||||
<group ns="robot_5">
|
||||
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
|
||||
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
|
||||
</node>
|
||||
</group>
|
||||
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
|
||||
</node> -->
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
<package>
|
||||
<name>enemy_suv_teleop_key</name>
|
||||
<version>0.3.5</version>
|
||||
<description>hector_quadrotor_teleop enables quadrotor flying with key translating them to geometry_msgs/Twist.</description>
|
||||
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://ros.org/wiki/hector_quadrotor_teleop</url>
|
||||
<url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
|
||||
|
||||
<author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
|
||||
|
||||
<!-- Dependencies which this package needs to build itself. -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<!-- Dependencies needed to compile this package. -->
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
|
||||
<!-- Dependencies needed after this package is compiled. -->
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
|
||||
<!-- Dependencies needed only for running tests. -->
|
||||
|
||||
</package>
|
||||
|
|
@ -0,0 +1,382 @@
|
|||
//=================================================================================================
|
||||
// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
|
||||
// All rights reserved.
|
||||
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of the Flight Systems and Automatic Control group,
|
||||
// TU Darmstadt, nor the names of its contributors may be used to
|
||||
// endorse or promote products derived from this software without
|
||||
// specific prior written permission.
|
||||
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
|
||||
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
//=================================================================================================
|
||||
|
||||
#include <ros/ros.h>
|
||||
// #include <sensor_msgs/Joy.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
// #include <hector_uav_msgs/YawrateCommand.h>
|
||||
// #include <hector_uav_msgs/ThrustCommand.h>
|
||||
// #include <hector_uav_msgs/AttitudeCommand.h>
|
||||
#include <signal.h>
|
||||
#include <termios.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#define KEYCODE_W 0x77
|
||||
#define KEYCODE_A 0x61
|
||||
#define KEYCODE_S 0x73
|
||||
#define KEYCODE_D 0x64
|
||||
|
||||
#define KEYCODE_R 0x43
|
||||
#define KEYCODE_L 0x44
|
||||
#define KEYCODE_F 0x41
|
||||
#define KEYCODE_B 0x42
|
||||
#define KEYCODE_Q 0x71
|
||||
|
||||
int kfd = 0;
|
||||
struct termios cooked, raw;
|
||||
|
||||
void quit(int sig)
|
||||
{
|
||||
(void)sig;
|
||||
tcsetattr(kfd, TCSANOW, &cooked);
|
||||
ros::shutdown();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
namespace hector_quadrotor
|
||||
{
|
||||
|
||||
class Teleop
|
||||
{
|
||||
private:
|
||||
ros::NodeHandle node_handle_;
|
||||
// ros::Subscriber joy_subscriber_;
|
||||
|
||||
ros::Publisher velocity_publisher_; //, attitude_publisher_, yawrate_publisher_, thrust_publisher_;
|
||||
geometry_msgs::Twist velocity_;
|
||||
// hector_uav_msgs::AttitudeCommand attitude_;
|
||||
// hector_uav_msgs::ThrustCommand thrust_;
|
||||
// hector_uav_msgs::YawrateCommand yawrate_;
|
||||
|
||||
struct Axis
|
||||
{
|
||||
int axis;
|
||||
double max;
|
||||
};
|
||||
|
||||
// struct Button
|
||||
// {
|
||||
// int button;
|
||||
// };
|
||||
|
||||
struct
|
||||
{
|
||||
Axis x;
|
||||
Axis y;
|
||||
Axis z;
|
||||
Axis yaw;
|
||||
} axes_;
|
||||
|
||||
// struct
|
||||
// {
|
||||
// Button slow;
|
||||
// } buttons_;
|
||||
|
||||
// double slow_factor_;
|
||||
|
||||
public:
|
||||
Teleop()
|
||||
{
|
||||
ros::NodeHandle params("~");
|
||||
|
||||
// double cmd_vel_robot;
|
||||
// ros::param::get("~cmd_vel_robot",cmd_vel_robot);
|
||||
|
||||
params.param<int>("x_axis", axes_.x.axis, 4);
|
||||
params.param<int>("y_axis", axes_.y.axis, 3);
|
||||
params.param<int>("z_axis", axes_.z.axis, 2);
|
||||
params.param<int>("yaw_axis", axes_.yaw.axis, 1);
|
||||
|
||||
params.param<double>("yaw_velocity_max", axes_.yaw.max, 0.0); // 90.0 * M_PI / 180.0
|
||||
// params.param<int>("slow_button", buttons_.slow.button, 1);
|
||||
// params.param<double>("slow_factor", slow_factor_, 0.2);
|
||||
|
||||
std::string control_mode_str;
|
||||
params.param<std::string>("control_mode", control_mode_str, "twist");
|
||||
|
||||
if (control_mode_str == "twist")
|
||||
{
|
||||
params.param<double>("x_velocity_max", axes_.x.max, 0.0); //2.0
|
||||
params.param<double>("y_velocity_max", axes_.y.max, 0.0); //2.0
|
||||
params.param<double>("z_velocity_max", axes_.z.max, 0.0); //2.0
|
||||
|
||||
// joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1));
|
||||
velocity_publisher_ = node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
|
||||
}
|
||||
else if (control_mode_str == "attitude")
|
||||
{
|
||||
params.param<double>("x_roll_max", axes_.x.max, 0.35);
|
||||
params.param<double>("y_pitch_max", axes_.y.max, 0.35);
|
||||
params.param<double>("z_thrust_max", axes_.z.max, 25.0);
|
||||
// joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyAttitudeCallback, this, _1));
|
||||
// attitude_publisher_ = node_handle_.advertise<hector_uav_msgs::AttitudeCommand>("command/attitude", 10);
|
||||
// yawrate_publisher_ = node_handle_.advertise<hector_uav_msgs::YawrateCommand>("command/yawrate", 10);
|
||||
// thrust_publisher_ = node_handle_.advertise<hector_uav_msgs::ThrustCommand>("command/thrust", 10);
|
||||
}
|
||||
}
|
||||
|
||||
~Teleop()
|
||||
{
|
||||
stop();
|
||||
}
|
||||
|
||||
void keyLoop()
|
||||
{
|
||||
char c;
|
||||
bool dirty = false;
|
||||
|
||||
// get the console in raw mode
|
||||
tcgetattr(kfd, &cooked);
|
||||
memcpy(&raw, &cooked, sizeof(struct termios));
|
||||
raw.c_lflag &= ~(ICANON | ECHO);
|
||||
// Setting a new line, then end of file
|
||||
raw.c_cc[VEOL] = 1;
|
||||
raw.c_cc[VEOF] = 2;
|
||||
tcsetattr(kfd, TCSANOW, &raw);
|
||||
|
||||
puts("Reading from keyboard");
|
||||
puts("---------------------------");
|
||||
puts("Use arrow keys and w a s d to move the quadrotor.");
|
||||
|
||||
for (;;)
|
||||
{
|
||||
// get the next event from the keyboard
|
||||
if (read(kfd, &c, 1) < 0)
|
||||
{
|
||||
perror("read():");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
// linear_ = angular_ = 0;
|
||||
ROS_DEBUG("value: 0x%02X\n", c);
|
||||
|
||||
switch (c)
|
||||
{
|
||||
case KEYCODE_W:
|
||||
ROS_DEBUG("UP");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 2.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_S:
|
||||
ROS_DEBUG("DOWN");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = -2.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_A:
|
||||
ROS_DEBUG("TURN_LEFT");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 90.0 * M_PI / 180.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_D:
|
||||
ROS_DEBUG("TURN_RIGHT");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = -90.0 * M_PI / 180.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_L:
|
||||
ROS_DEBUG("LEFT");
|
||||
axes_.x.max = 20.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 1.0;//90.0 * M_PI / 180.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_R:
|
||||
ROS_DEBUG("RIGHT");
|
||||
axes_.x.max = 20.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = -1.0;//-90.0 * M_PI / 180.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_F:
|
||||
ROS_DEBUG("FORWARD");
|
||||
axes_.x.max = 20.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_B:
|
||||
ROS_DEBUG("BACKWARD");
|
||||
axes_.x.max = -20.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_Q:
|
||||
ROS_DEBUG("STOP");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
default:
|
||||
// max_tv = walk_vel_;
|
||||
// max_rv = yaw_rate_;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
dirty = false;
|
||||
}
|
||||
|
||||
// geometry_msgs::Twist twist;
|
||||
velocity_.linear.x = axes_.x.max;
|
||||
// velocity_.linear.y = axes_.y.max;
|
||||
// velocity_.linear.z = axes_.z.max;
|
||||
velocity_.angular.z = axes_.yaw.max;
|
||||
// twist.angular.z = a_scale_ * angular_;
|
||||
// twist.linear.x = l_scale_ * linear_;
|
||||
if (dirty == true)
|
||||
{
|
||||
velocity_publisher_.publish(velocity_);
|
||||
dirty = false;
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy)
|
||||
// {
|
||||
// velocity_.linear.x = getAxis(joy, axes_.x);
|
||||
// velocity_.linear.y = getAxis(joy, axes_.y);
|
||||
// velocity_.linear.z = getAxis(joy, axes_.z);
|
||||
// velocity_.angular.z = getAxis(joy, axes_.yaw);
|
||||
// if (getButton(joy, buttons_.slow.button))
|
||||
// {
|
||||
// velocity_.linear.x *= slow_factor_;
|
||||
// velocity_.linear.y *= slow_factor_;
|
||||
// velocity_.linear.z *= slow_factor_;
|
||||
// velocity_.angular.z *= slow_factor_;
|
||||
// }
|
||||
// velocity_publisher_.publish(velocity_);
|
||||
// }
|
||||
|
||||
// void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy)
|
||||
// {
|
||||
// attitude_.roll = getAxis(joy, axes_.x);
|
||||
// attitude_.pitch = getAxis(joy, axes_.y);
|
||||
// attitude_publisher_.publish(attitude_);
|
||||
|
||||
// thrust_.thrust = getAxis(joy, axes_.z);
|
||||
// thrust_publisher_.publish(thrust_);
|
||||
|
||||
// yawrate_.turnrate = getAxis(joy, axes_.yaw);
|
||||
// if (getButton(joy, buttons_.slow.button))
|
||||
// {
|
||||
// yawrate_.turnrate *= slow_factor_;
|
||||
// }
|
||||
// yawrate_publisher_.publish(yawrate_);
|
||||
// }
|
||||
|
||||
// sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis)
|
||||
// {
|
||||
// if (axis.axis == 0)
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
// sensor_msgs::Joy::_axes_type::value_type sign = 1.0;
|
||||
// if (axis.axis < 0)
|
||||
// {
|
||||
// sign = -1.0;
|
||||
// axis.axis = -axis.axis;
|
||||
// }
|
||||
// if ((size_t)axis.axis > joy->axes.size())
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
// return sign * joy->axes[axis.axis - 1] * axis.max;
|
||||
// }
|
||||
|
||||
// sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button)
|
||||
// {
|
||||
// if (button <= 0)
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
// if ((size_t)button > joy->buttons.size())
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
// return joy->buttons[button - 1];
|
||||
// }
|
||||
|
||||
void stop()
|
||||
{
|
||||
if (velocity_publisher_.getNumSubscribers() > 0)
|
||||
{
|
||||
velocity_ = geometry_msgs::Twist();
|
||||
velocity_publisher_.publish(velocity_);
|
||||
}
|
||||
// if (attitude_publisher_.getNumSubscribers() > 0)
|
||||
// {
|
||||
// attitude_ = hector_uav_msgs::AttitudeCommand();
|
||||
// attitude_publisher_.publish(attitude_);
|
||||
// }
|
||||
// if (thrust_publisher_.getNumSubscribers() > 0)
|
||||
// {
|
||||
// thrust_ = hector_uav_msgs::ThrustCommand();
|
||||
// thrust_publisher_.publish(thrust_);
|
||||
// }
|
||||
// if (yawrate_publisher_.getNumSubscribers() > 0)
|
||||
// {
|
||||
// yawrate_ = hector_uav_msgs::YawrateCommand();
|
||||
// yawrate_publisher_.publish(yawrate_);
|
||||
// }
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace hector_quadrotor
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "quadrotor_teleop_key");
|
||||
|
||||
hector_quadrotor::Teleop teleop;
|
||||
// ros::spin();
|
||||
signal(SIGINT, quit);
|
||||
|
||||
teleop.keyLoop();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,417 @@
|
|||
//=================================================================================================
|
||||
// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
|
||||
// All rights reserved.
|
||||
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions are met:
|
||||
// * Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of the Flight Systems and Automatic Control group,
|
||||
// TU Darmstadt, nor the names of its contributors may be used to
|
||||
// endorse or promote products derived from this software without
|
||||
// specific prior written permission.
|
||||
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
|
||||
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
//=================================================================================================
|
||||
|
||||
#include <ros/ros.h>
|
||||
// #include <sensor_msgs/Joy.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
// #include <hector_uav_msgs/YawrateCommand.h>
|
||||
// #include <hector_uav_msgs/ThrustCommand.h>
|
||||
// #include <hector_uav_msgs/AttitudeCommand.h>
|
||||
#include <signal.h>
|
||||
#include <termios.h>
|
||||
#include <stdio.h>
|
||||
#include <sys/poll.h>
|
||||
#include <boost/thread/thread.hpp>
|
||||
|
||||
#define KEYCODE_W 0x77
|
||||
#define KEYCODE_A 0x61
|
||||
#define KEYCODE_S 0x73
|
||||
#define KEYCODE_D 0x64
|
||||
|
||||
#define KEYCODE_R 0x43
|
||||
#define KEYCODE_L 0x44
|
||||
#define KEYCODE_F 0x41
|
||||
#define KEYCODE_B 0x42
|
||||
#define KEYCODE_Q 0x71
|
||||
|
||||
int kfd = 0;
|
||||
struct termios cooked, raw;
|
||||
|
||||
void quit(int sig)
|
||||
{
|
||||
(void)sig;
|
||||
tcsetattr(kfd, TCSANOW, &cooked);
|
||||
ros::shutdown();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
namespace hector_quadrotor
|
||||
{
|
||||
|
||||
class Teleop
|
||||
{
|
||||
private:
|
||||
ros::NodeHandle node_handle_;
|
||||
// ros::Subscriber joy_subscriber_;
|
||||
|
||||
ros::Publisher velocity_publisher_; //, attitude_publisher_, yawrate_publisher_, thrust_publisher_;
|
||||
geometry_msgs::Twist velocity_;
|
||||
// hector_uav_msgs::AttitudeCommand attitude_;
|
||||
// hector_uav_msgs::ThrustCommand thrust_;
|
||||
// hector_uav_msgs::YawrateCommand yawrate_;
|
||||
|
||||
struct Axis
|
||||
{
|
||||
int axis;
|
||||
double max;
|
||||
};
|
||||
|
||||
// struct Button
|
||||
// {
|
||||
// int button;
|
||||
// };
|
||||
|
||||
struct
|
||||
{
|
||||
Axis x;
|
||||
Axis y;
|
||||
Axis z;
|
||||
Axis yaw;
|
||||
} axes_;
|
||||
|
||||
// struct
|
||||
// {
|
||||
// Button slow;
|
||||
// } buttons_;
|
||||
|
||||
// double slow_factor_;
|
||||
|
||||
public:
|
||||
Teleop()
|
||||
{
|
||||
ros::NodeHandle params("~");
|
||||
|
||||
params.param<int>("x_axis", axes_.x.axis, 4);
|
||||
params.param<int>("y_axis", axes_.y.axis, 3);
|
||||
params.param<int>("z_axis", axes_.z.axis, 2);
|
||||
params.param<int>("yaw_axis", axes_.yaw.axis, 1);
|
||||
|
||||
params.param<double>("yaw_velocity_max", axes_.yaw.max, 0.0); // 90.0 * M_PI / 180.0
|
||||
// params.param<int>("slow_button", buttons_.slow.button, 1);
|
||||
// params.param<double>("slow_factor", slow_factor_, 0.2);
|
||||
|
||||
std::string control_mode_str;
|
||||
params.param<std::string>("control_mode", control_mode_str, "twist");
|
||||
|
||||
if (control_mode_str == "twist")
|
||||
{
|
||||
params.param<double>("x_velocity_max", axes_.x.max, 0.0); //2.0
|
||||
params.param<double>("y_velocity_max", axes_.y.max, 0.0); //2.0
|
||||
params.param<double>("z_velocity_max", axes_.z.max, 0.0); //2.0
|
||||
|
||||
// joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1));
|
||||
velocity_publisher_ = node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
|
||||
}
|
||||
else if (control_mode_str == "attitude")
|
||||
{
|
||||
params.param<double>("x_roll_max", axes_.x.max, 0.35);
|
||||
params.param<double>("y_pitch_max", axes_.y.max, 0.35);
|
||||
params.param<double>("z_thrust_max", axes_.z.max, 25.0);
|
||||
// joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyAttitudeCallback, this, _1));
|
||||
// attitude_publisher_ = node_handle_.advertise<hector_uav_msgs::AttitudeCommand>("command/attitude", 10);
|
||||
// yawrate_publisher_ = node_handle_.advertise<hector_uav_msgs::YawrateCommand>("command/yawrate", 10);
|
||||
// thrust_publisher_ = node_handle_.advertise<hector_uav_msgs::ThrustCommand>("command/thrust", 10);
|
||||
}
|
||||
}
|
||||
|
||||
~Teleop()
|
||||
{
|
||||
stop();
|
||||
}
|
||||
|
||||
void keyLoop()
|
||||
{
|
||||
char c;
|
||||
bool dirty = false;
|
||||
|
||||
// get the console in raw mode
|
||||
tcgetattr(kfd, &cooked);
|
||||
memcpy(&raw, &cooked, sizeof(struct termios));
|
||||
raw.c_lflag &= ~(ICANON | ECHO);
|
||||
// Setting a new line, then end of file
|
||||
raw.c_cc[VEOL] = 1;
|
||||
raw.c_cc[VEOF] = 2;
|
||||
tcsetattr(kfd, TCSANOW, &raw);
|
||||
|
||||
puts("Reading from keyboard");
|
||||
puts("---------------------------");
|
||||
puts("Use arrow keys and w a s d to move the quadrotor.");
|
||||
|
||||
struct pollfd ufd;
|
||||
ufd.fd = kfd;
|
||||
ufd.events = POLLIN;
|
||||
|
||||
for (;;)
|
||||
{
|
||||
boost::this_thread::interruption_point();
|
||||
|
||||
// get the next event from the keyboard
|
||||
int num;
|
||||
|
||||
if ((num = poll(&ufd, 1, 250)) < 0)
|
||||
{
|
||||
perror("poll():");
|
||||
return;
|
||||
}
|
||||
else if (num > 0)
|
||||
{
|
||||
if (read(kfd, &c, 1) < 0)
|
||||
{
|
||||
perror("read():");
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (dirty == true)
|
||||
{
|
||||
stopRobot();
|
||||
dirty = false;
|
||||
}
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
for (;;)
|
||||
{
|
||||
// get the next event from the keyboard
|
||||
if (read(kfd, &c, 1) < 0)
|
||||
{
|
||||
perror("read():");
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
// linear_ = angular_ = 0;
|
||||
ROS_DEBUG("value: 0x%02X\n", c);
|
||||
|
||||
switch (c)
|
||||
{
|
||||
case KEYCODE_W:
|
||||
ROS_DEBUG("UP");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 2.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_S:
|
||||
ROS_DEBUG("DOWN");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = -2.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_A:
|
||||
ROS_DEBUG("TURN_LEFT");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 90.0 * M_PI / 180.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_D:
|
||||
ROS_DEBUG("TURN_RIGHT");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = -90.0 * M_PI / 180.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_L:
|
||||
ROS_DEBUG("LEFT");
|
||||
axes_.x.max = 2.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_R:
|
||||
ROS_DEBUG("RIGHT");
|
||||
axes_.x.max = -2.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_F:
|
||||
ROS_DEBUG("FORWARD");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 2.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_B:
|
||||
ROS_DEBUG("BACKWARD");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = -2.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
case KEYCODE_Q:
|
||||
ROS_DEBUG("BACKWARD");
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
dirty = true;
|
||||
break;
|
||||
default:
|
||||
// max_tv = walk_vel_;
|
||||
// max_rv = yaw_rate_;
|
||||
axes_.z.max = 0.0;
|
||||
axes_.yaw.max = 0.0;
|
||||
axes_.x.max = 0.0;
|
||||
axes_.y.max = 0.0;
|
||||
dirty = false;
|
||||
}
|
||||
|
||||
// geometry_msgs::Twist twist;
|
||||
velocity_.linear.x = axes_.x.max;
|
||||
velocity_.linear.y = axes_.y.max;
|
||||
velocity_.linear.z = axes_.z.max;
|
||||
velocity_.angular.z = axes_.yaw.max;
|
||||
// twist.angular.z = a_scale_ * angular_;
|
||||
// twist.linear.x = l_scale_ * linear_;
|
||||
if (dirty == true)
|
||||
{
|
||||
velocity_publisher_.publish(velocity_);
|
||||
dirty = false;
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy)
|
||||
// {
|
||||
// velocity_.linear.x = getAxis(joy, axes_.x);
|
||||
// velocity_.linear.y = getAxis(joy, axes_.y);
|
||||
// velocity_.linear.z = getAxis(joy, axes_.z);
|
||||
// velocity_.angular.z = getAxis(joy, axes_.yaw);
|
||||
// if (getButton(joy, buttons_.slow.button))
|
||||
// {
|
||||
// velocity_.linear.x *= slow_factor_;
|
||||
// velocity_.linear.y *= slow_factor_;
|
||||
// velocity_.linear.z *= slow_factor_;
|
||||
// velocity_.angular.z *= slow_factor_;
|
||||
// }
|
||||
// velocity_publisher_.publish(velocity_);
|
||||
// }
|
||||
|
||||
// void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy)
|
||||
// {
|
||||
// attitude_.roll = getAxis(joy, axes_.x);
|
||||
// attitude_.pitch = getAxis(joy, axes_.y);
|
||||
// attitude_publisher_.publish(attitude_);
|
||||
|
||||
// thrust_.thrust = getAxis(joy, axes_.z);
|
||||
// thrust_publisher_.publish(thrust_);
|
||||
|
||||
// yawrate_.turnrate = getAxis(joy, axes_.yaw);
|
||||
// if (getButton(joy, buttons_.slow.button))
|
||||
// {
|
||||
// yawrate_.turnrate *= slow_factor_;
|
||||
// }
|
||||
// yawrate_publisher_.publish(yawrate_);
|
||||
// }
|
||||
|
||||
// sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis)
|
||||
// {
|
||||
// if (axis.axis == 0)
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
// sensor_msgs::Joy::_axes_type::value_type sign = 1.0;
|
||||
// if (axis.axis < 0)
|
||||
// {
|
||||
// sign = -1.0;
|
||||
// axis.axis = -axis.axis;
|
||||
// }
|
||||
// if ((size_t)axis.axis > joy->axes.size())
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
// return sign * joy->axes[axis.axis - 1] * axis.max;
|
||||
// }
|
||||
|
||||
// sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button)
|
||||
// {
|
||||
// if (button <= 0)
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
// if ((size_t)button > joy->buttons.size())
|
||||
// {
|
||||
// return 0;
|
||||
// }
|
||||
// return joy->buttons[button - 1];
|
||||
// }
|
||||
|
||||
void stop()
|
||||
{
|
||||
if (velocity_publisher_.getNumSubscribers() > 0)
|
||||
{
|
||||
velocity_ = geometry_msgs::Twist();
|
||||
velocity_publisher_.publish(velocity_);
|
||||
}
|
||||
// if (attitude_publisher_.getNumSubscribers() > 0)
|
||||
// {
|
||||
// attitude_ = hector_uav_msgs::AttitudeCommand();
|
||||
// attitude_publisher_.publish(attitude_);
|
||||
// }
|
||||
// if (thrust_publisher_.getNumSubscribers() > 0)
|
||||
// {
|
||||
// thrust_ = hector_uav_msgs::ThrustCommand();
|
||||
// thrust_publisher_.publish(thrust_);
|
||||
// }
|
||||
// if (yawrate_publisher_.getNumSubscribers() > 0)
|
||||
// {
|
||||
// yawrate_ = hector_uav_msgs::YawrateCommand();
|
||||
// yawrate_publisher_.publish(yawrate_);
|
||||
// }
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace hector_quadrotor
|
||||
|
||||
int
|
||||
main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "quadrotor_teleop_key");
|
||||
|
||||
hector_quadrotor::Teleop teleop;
|
||||
// ros::spin();
|
||||
signal(SIGINT, quit);
|
||||
|
||||
teleop.keyLoop();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,123 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(suv_move)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 11)
|
||||
if (NOT CMAKE_BUILD_TYPE)
|
||||
# Options: Debug, Release, MinSizeRel, RelWithDebInfo
|
||||
message(STATUS "No build type selected, default to Release")
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
endif()
|
||||
|
||||
set(CMAKE_CXX_FLAGS "-fopenmp")
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
rosflight_msgs
|
||||
rosplane_msgs
|
||||
dynamic_reconfigure
|
||||
sensor_msgs
|
||||
tf
|
||||
)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
|
||||
################################################
|
||||
## 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 run_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/Follower.cfg
|
||||
# cfg/Controller.cfg
|
||||
#)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
# LIBRARIES
|
||||
CATKIN_DEPENDS roscpp rospy rosflight_msgs rosplane_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
|
||||
|
||||
## Declare a C++ executable
|
||||
#add_executable(rosplane_controller
|
||||
# src/controller_base.cpp
|
||||
# src/controller_example.cpp)
|
||||
#add_dependencies(rosplane_controller ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
|
||||
#target_link_libraries(rosplane_controller ${catkin_LIBRARIES})
|
||||
|
||||
#add_executable(rosplane_controller99
|
||||
# src/controller_base99.cpp
|
||||
# src/controller_example99.cpp)
|
||||
#add_dependencies(rosplane_controller99 ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
|
||||
#target_link_libraries(rosplane_controller99 ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(suv_pseudo_controller
|
||||
# src/controller_base.cpp
|
||||
# src/controller_example.cpp)
|
||||
src/pseudo_controller_base.cpp
|
||||
src/pseudo_controller_example.cpp)
|
||||
add_dependencies(suv_pseudo_controller ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
|
||||
target_link_libraries(suv_pseudo_controller ${catkin_LIBRARIES})
|
||||
|
||||
## Declare a C++ executable
|
||||
#add_executable(rosplane_estimator
|
||||
# src/estimator_base.cpp
|
||||
# src/estimator_example.cpp)
|
||||
#add_dependencies(rosplane_estimator ${catkin_EXPORTED_TARGETS})
|
||||
#target_link_libraries(rosplane_estimator ${catkin_LIBRARIES})
|
||||
|
||||
## Declare a C++ executable
|
||||
add_executable(suv_estimator_statesub
|
||||
src/estimator_base_statesub.cpp
|
||||
)
|
||||
add_dependencies(suv_estimator_statesub ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(suv_estimator_statesub ${catkin_LIBRARIES})
|
||||
|
||||
## Declare a C++ executable
|
||||
#add_executable(rosplane_estimator_updata_statesub
|
||||
# src/estimator_base_updata_statesub.cpp
|
||||
# )
|
||||
#add_dependencies(rosplane_estimator_updata_statesub ${catkin_EXPORTED_TARGETS})
|
||||
#target_link_libraries(rosplane_estimator_updata_statesub ${catkin_LIBRARIES})
|
||||
|
||||
## Declare a C++ executable
|
||||
add_executable(suv_estimator_99pub
|
||||
src/estimator_base_99pub.cpp
|
||||
)
|
||||
add_dependencies(suv_estimator_99pub ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(suv_estimator_99pub ${catkin_LIBRARIES})
|
||||
|
||||
## Declare a C++ executable
|
||||
add_executable(suv_path_follower
|
||||
src/path_follower_example.cpp
|
||||
src/path_follower_base.cpp)
|
||||
add_dependencies(suv_path_follower ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
|
||||
target_link_libraries(suv_path_follower ${catkin_LIBRARIES})
|
||||
|
||||
|
||||
## Declare a C++ executable
|
||||
add_executable(suv_path_manager
|
||||
src/path_manager_base.cpp
|
||||
src/path_manager_example.cpp)
|
||||
add_dependencies(suv_path_manager ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(suv_path_manager ${catkin_LIBRARIES})
|
||||
|
||||
|
||||
## Declare a C++ executable
|
||||
add_executable(suv_path_planner
|
||||
src/path_planner.cpp)
|
||||
add_dependencies(suv_path_planner ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(suv_path_planner ${catkin_LIBRARIES})
|
||||
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
#!/usr/bin/env python
|
||||
PACKAGE = "rosplane"
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# trim
|
||||
trim = gen.add_group("Trim")
|
||||
trim.add("TRIM_E", double_t, 0, "Elevator trim", 0, -1, 1)
|
||||
trim.add("TRIM_A", double_t, 0, "Aileron trim", 0, -1, 1)
|
||||
trim.add("TRIM_R", double_t, 0, "Rudder trim", 0, -1, 1)
|
||||
trim.add("TRIM_T", double_t, 0, "Throttle trim", 0.6, 0, 1)
|
||||
|
||||
# course hold
|
||||
course = gen.add_group("Course")
|
||||
course.add("COURSE_KP", double_t, 0, "Course proportional gain", 0.7329, 0, 2)
|
||||
course.add("COURSE_KD", double_t, 0, "Course derivative gain", 0, -1, 0)
|
||||
course.add("COURSE_KI", double_t, 0, "Course integral gain", 0.0, 0, 0.2)
|
||||
|
||||
# roll hold
|
||||
roll = gen.add_group("Roll")
|
||||
roll.add("ROLL_KP", double_t, 0, "Roll proportional gain", 1.17, 0, 3)
|
||||
roll.add("ROLL_KD", double_t, 0, "Roll derivative gain", -0.13, -1, 0)
|
||||
roll.add("ROLL_KI", double_t, 0, "Roll integral gain", 0, 0, 0.2)
|
||||
|
||||
# pitch hold
|
||||
pitch = gen.add_group("Pitch")
|
||||
pitch.add("PITCH_KP", double_t, 0, "Pitch proportional gain", 1.0, 0, 3)
|
||||
pitch.add("PITCH_KD", double_t, 0, "Pitch derivative gain", -0.17, -0.4, 0)
|
||||
pitch.add("PITCH_KI", double_t, 0, "Pitch integral gain", 0, 0, 0.2)
|
||||
pitch.add("PITCH_FF", double_t, 0, "Pitch feed forward value", 0, -1, 1)
|
||||
|
||||
# airspeed with pitch hold
|
||||
as_pitch = gen.add_group("Airspeed with Pitch")
|
||||
as_pitch.add("AS_PITCH_KP", double_t, 0, "Airspeed with pitch proportional gain", -0.0713, 0, 0.2)
|
||||
as_pitch.add("AS_PITCH_KD", double_t, 0, "Airspeed with pitch derivative gain", -0.0635, -0.2, 0)
|
||||
as_pitch.add("AS_PITCH_KI", double_t, 0, "Airspeed with pitch integral gain", 0, 0, 0.2)
|
||||
|
||||
# airspeed with throttle hold
|
||||
as_thr = gen.add_group("Airspeed with Throttle")
|
||||
as_thr.add("AS_THR_KP", double_t, 0, "Airspeed with throttle proportional gain", 3.2, 0, 10)
|
||||
as_thr.add("AS_THR_KD", double_t, 0, "Airspeed with throttle derivative gain", 0, -5, 0)
|
||||
as_thr.add("AS_THR_KI", double_t, 0, "Airspeed with throttle integral gain", 1.0, 0, 10)
|
||||
|
||||
# altitude hold
|
||||
alt = gen.add_group("Altitude")
|
||||
alt.add("ALT_KP", double_t, 0, "Altitude proportional gain", 0.045, 0, 0.1)
|
||||
alt.add("ALT_KD", double_t, 0, "Altitude derivative gain", 0, -0.05, 0)
|
||||
alt.add("ALT_KI", double_t, 0, "Altitude integral gain", 0.01, 0, 0.05)
|
||||
|
||||
# side-slip hold
|
||||
sideslip = gen.add_group("Side Slip")
|
||||
sideslip.add("BETA_KP", double_t, 0, "Side slip proportional gain", -0.1164, 0, 0.3)
|
||||
sideslip.add("BETA_KD", double_t, 0, "Side slip derivative gain", 0, -0.15, 0)
|
||||
sideslip.add("BETA_KI", double_t, 0, "Side slip integral gain", -0.0037111, 0, 0.05)
|
||||
|
||||
exit(gen.generate(PACKAGE, "rosplane", "Controller"))
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
#!/usr/bin/env python
|
||||
PACKAGE = "rosplane"
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
||||
gen = ParameterGenerator()
|
||||
|
||||
# Chi Infinity
|
||||
gen.add("CHI_INFTY", double_t, 0, "Chi Infinity", 1.0472, 0 , 1.5708)
|
||||
|
||||
# K Path
|
||||
gen.add("K_PATH", double_t, 0, "K Path", 0.025, 0, 1)
|
||||
|
||||
# K Orbit
|
||||
gen.add("K_ORBIT", double_t, 0, "K Orbit", 4.0, 0, 15)
|
||||
|
||||
exit(gen.generate(PACKAGE, "rosplane", "Follower"))
|
||||
|
|
@ -0,0 +1,145 @@
|
|||
/**
|
||||
* @file controller_base.h
|
||||
*
|
||||
* Base class definition for autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php
|
||||
*
|
||||
* @author Gary Ellingson <gary.ellingson@byu.edu>
|
||||
*/
|
||||
|
||||
#ifndef CONTROLLER_BASE_H
|
||||
#define CONTROLLER_BASE_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <rosflight_msgs/Command.h>
|
||||
#include <enemy_suv_msgs/State.h>
|
||||
#include <enemy_suv_msgs/Controller_Commands.h>
|
||||
#include <enemy_suv_msgs/Controller_Internals.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <rosplane/ControllerConfig.h>
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
enum class alt_zones
|
||||
{
|
||||
TAKE_OFF,
|
||||
CLIMB,
|
||||
DESCEND,
|
||||
ALTITUDE_HOLD
|
||||
};
|
||||
|
||||
class controller_base
|
||||
{
|
||||
public:
|
||||
controller_base();
|
||||
float spin();
|
||||
|
||||
protected:
|
||||
|
||||
struct input_s
|
||||
{
|
||||
float Ts; /** time step */
|
||||
float h; /** altitude */
|
||||
float va; /** airspeed */
|
||||
float phi; /** roll angle */
|
||||
float psi; //
|
||||
float theta; /** pitch angle */
|
||||
float chi; /** course angle */
|
||||
float p; /** body frame roll rate */
|
||||
float q; /** body frame pitch rate */
|
||||
float r; /** body frame yaw rate */
|
||||
float Va_c; /** commanded airspeed (m/s) */
|
||||
float h_c; /** commanded altitude (m) */
|
||||
float chi_c; /** commanded course (rad) */
|
||||
float phi_ff; /** feed forward term for orbits (rad) */
|
||||
};
|
||||
|
||||
struct output_s
|
||||
{
|
||||
float theta_c; //俯仰角--即平行于机身轴线并指向飞行器前方的向量与地面的夹角
|
||||
float phi_c; //滚转角--机体坐标系OZb轴与通过机体OXb轴的铅垂面间的夹角，机体向右滚为正，反之为负
|
||||
float delta_a; //副翼偏移量-转弯用-副翼跳变造成油门和升降舵数值的波动
|
||||
float delta_e; //升降舵偏移量-拉高降低
|
||||
float delta_r; //方向舵偏移量-保持稳定，消除测滑
|
||||
float delta_t; //油门偏移量
|
||||
alt_zones current_zone;
|
||||
};
|
||||
|
||||
struct params_s
|
||||
{
|
||||
double alt_hz; /**< altitude hold zone */
|
||||
double alt_toz; /**< altitude takeoff zone */
|
||||
double tau;
|
||||
double c_kp;
|
||||
double c_kd;
|
||||
double c_ki;
|
||||
double r_kp;
|
||||
double r_kd;
|
||||
double r_ki;
|
||||
double p_kp;
|
||||
double p_kd;
|
||||
double p_ki;
|
||||
double p_ff;
|
||||
double a_p_kp;
|
||||
double a_p_kd;
|
||||
double a_p_ki;
|
||||
double a_t_kp;
|
||||
double a_t_kd;
|
||||
double a_t_ki;
|
||||
double a_kp;
|
||||
double a_kd;
|
||||
double a_ki;
|
||||
double b_kp;
|
||||
double b_kd;
|
||||
double b_ki;
|
||||
double trim_e;
|
||||
double trim_a;
|
||||
double trim_r;
|
||||
double trim_t;
|
||||
double max_e;
|
||||
double max_a;
|
||||
double max_r;
|
||||
double max_t;
|
||||
double pwm_rad_e;
|
||||
double pwm_rad_a;
|
||||
double pwm_rad_r;
|
||||
};
|
||||
|
||||
virtual void control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0;
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle nh_private_;
|
||||
ros::Subscriber vehicle_state_sub_;
|
||||
ros::Subscriber controller_commands_sub_;
|
||||
ros::Publisher actuators_pub_;
|
||||
ros::Publisher internals_pub_;
|
||||
ros::Timer act_pub_timer_;
|
||||
|
||||
struct params_s params_; /**< params */
|
||||
enemy_suv_msgs::Controller_Commands controller_commands_;
|
||||
enemy_suv_msgs::State vehicle_state_;
|
||||
|
||||
void vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg);
|
||||
void controller_commands_callback(const enemy_suv_msgs::Controller_CommandsConstPtr &msg);
|
||||
bool command_recieved_;
|
||||
|
||||
dynamic_reconfigure::Server<rosplane::ControllerConfig> server_;
|
||||
dynamic_reconfigure::Server<rosplane::ControllerConfig>::CallbackType func_;
|
||||
|
||||
void reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level);
|
||||
|
||||
/**
|
||||
* Convert from deflection angle to pwm
|
||||
*/
|
||||
void convert_to_pwm(struct output_s &output);
|
||||
|
||||
/**
|
||||
* Publish the outputs
|
||||
*/
|
||||
void actuator_controls_publish(const ros::TimerEvent &);
|
||||
};
|
||||
} //end namespace
|
||||
|
||||
#endif // CONTROLLER_BASE_H
|
||||
|
|
@ -0,0 +1,145 @@
|
|||
/**
|
||||
* @file controller_base99.h
|
||||
*
|
||||
* Base class definition for autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php
|
||||
*
|
||||
* @author Gary Ellingson <gary.ellingson@byu.edu>
|
||||
*/
|
||||
|
||||
#ifndef CONTROLLER_BASE99_H
|
||||
#define CONTROLLER_BASE99_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <rosflight_msgs/Command.h>
|
||||
#include <enemy_suv_msgs/State.h>
|
||||
#include <enemy_suv_msgs/Controller_Commands.h>
|
||||
#include <enemy_suv_msgs/Controller_Internals.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <rosplane/ControllerConfig.h>
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
enum class alt_zones
|
||||
{
|
||||
TAKE_OFF,
|
||||
CLIMB,
|
||||
DESCEND,
|
||||
ALTITUDE_HOLD
|
||||
};
|
||||
|
||||
class controller_base99
|
||||
{
|
||||
public:
|
||||
controller_base99();
|
||||
float spin();
|
||||
|
||||
protected:
|
||||
|
||||
struct input_s
|
||||
{
|
||||
float Ts; /** time step */
|
||||
float h; /** altitude */
|
||||
float va; /** airspeed */
|
||||
float phi; /** roll angle */
|
||||
float psi; //
|
||||
float theta; /** pitch angle */
|
||||
float chi; /** course angle */
|
||||
float p; /** body frame roll rate */
|
||||
float q; /** body frame pitch rate */
|
||||
float r; /** body frame yaw rate */
|
||||
float Va_c; /** commanded airspeed (m/s) */
|
||||
float h_c; /** commanded altitude (m) */
|
||||
float chi_c; /** commanded course (rad) */
|
||||
float phi_ff; /** feed forward term for orbits (rad) */
|
||||
};
|
||||
|
||||
struct output_s
|
||||
{
|
||||
float theta_c; //俯仰角--即平行于机身轴线并指向飞行器前方的向量与地面的夹角
|
||||
float phi_c; //滚转角--机体坐标系OZb轴与通过机体OXb轴的铅垂面间的夹角，机体向右滚为正，反之为负
|
||||
float delta_a; //副翼偏移量-转弯用-副翼跳变造成油门和升降舵数值的波动
|
||||
float delta_e; //升降舵偏移量-拉高降低
|
||||
float delta_r; //方向舵偏移量-保持稳定，消除测滑
|
||||
float delta_t; //油门偏移量
|
||||
alt_zones current_zone;
|
||||
};
|
||||
|
||||
struct params_s
|
||||
{
|
||||
double alt_hz; /**< altitude hold zone */
|
||||
double alt_toz; /**< altitude takeoff zone */
|
||||
double tau;
|
||||
double c_kp;
|
||||
double c_kd;
|
||||
double c_ki;
|
||||
double r_kp;
|
||||
double r_kd;
|
||||
double r_ki;
|
||||
double p_kp;
|
||||
double p_kd;
|
||||
double p_ki;
|
||||
double p_ff;
|
||||
double a_p_kp;
|
||||
double a_p_kd;
|
||||
double a_p_ki;
|
||||
double a_t_kp;
|
||||
double a_t_kd;
|
||||
double a_t_ki;
|
||||
double a_kp;
|
||||
double a_kd;
|
||||
double a_ki;
|
||||
double b_kp;
|
||||
double b_kd;
|
||||
double b_ki;
|
||||
double trim_e;
|
||||
double trim_a;
|
||||
double trim_r;
|
||||
double trim_t;
|
||||
double max_e;
|
||||
double max_a;
|
||||
double max_r;
|
||||
double max_t;
|
||||
double pwm_rad_e;
|
||||
double pwm_rad_a;
|
||||
double pwm_rad_r;
|
||||
};
|
||||
|
||||
virtual void control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0;
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle nh_private_;
|
||||
ros::Subscriber vehicle_state_sub_;
|
||||
ros::Subscriber controller_commands_sub_;
|
||||
ros::Publisher actuators_pub_;
|
||||
ros::Publisher internals_pub_;
|
||||
ros::Timer act_pub_timer_;
|
||||
|
||||
struct params_s params_; /**< params */
|
||||
enemy_suv_msgs::Controller_Commands controller_commands_;
|
||||
enemy_suv_msgs::State vehicle_state_;
|
||||
|
||||
void vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg);
|
||||
void controller_commands_callback(const enemy_suv_msgs::Controller_CommandsConstPtr &msg);
|
||||
bool command_recieved_;
|
||||
|
||||
dynamic_reconfigure::Server<rosplane::ControllerConfig> server_;
|
||||
dynamic_reconfigure::Server<rosplane::ControllerConfig>::CallbackType func_;
|
||||
|
||||
void reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level);
|
||||
|
||||
/**
|
||||
* Convert from deflection angle to pwm
|
||||
*/
|
||||
void convert_to_pwm(struct output_s &output);
|
||||
|
||||
/**
|
||||
* Publish the outputs
|
||||
*/
|
||||
void actuator_controls_publish(const ros::TimerEvent &);
|
||||
};
|
||||
} //end namespace
|
||||
|
||||
#endif // CONTROLLER_BASE99_H
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
#ifndef CONTROLLER_EXAMPLE_H
|
||||
#define CONTROLLER_EXAMPLE_H
|
||||
|
||||
#include "controller_base.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
class controller_example : public controller_base
|
||||
{
|
||||
public:
|
||||
controller_example();
|
||||
private:
|
||||
virtual void control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||
alt_zones current_zone;
|
||||
|
||||
float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s ¶ms, float Ts);
|
||||
float c_error_;
|
||||
float c_integrator_;
|
||||
|
||||
float roll_hold(float phi_c, float phi, float p, const struct params_s ¶ms, float Ts);
|
||||
float r_error_;
|
||||
float r_integrator;
|
||||
|
||||
float pitch_hold(float theta_c, float theta, float q, const struct params_s ¶ms, float Ts);
|
||||
float p_error_;
|
||||
float p_integrator_;
|
||||
|
||||
float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s ¶ms, float Ts);
|
||||
float ap_error_;
|
||||
float ap_integrator_;
|
||||
float ap_differentiator_;
|
||||
|
||||
float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s ¶ms, float Ts);
|
||||
float at_error_;
|
||||
float at_integrator_;
|
||||
float at_differentiator_;
|
||||
|
||||
float altitiude_hold(float h_c, float h, const struct params_s ¶ms, float Ts);
|
||||
float a_error_;
|
||||
float a_integrator_;
|
||||
float a_differentiator_;
|
||||
|
||||
// float cooridinated_turn_hold(float v, const struct params_s ¶ms, float Ts);
|
||||
// float ct_error_;
|
||||
// float ct_integrator_;
|
||||
// float ct_differentiator_;
|
||||
|
||||
float sat(float value, float up_limit, float low_limit);
|
||||
};
|
||||
} //end namespace
|
||||
|
||||
#endif // CONTROLLER_EXAMPLE_H
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
#ifndef CONTROLLER_EXAMPLE99_H
|
||||
#define CONTROLLER_EXAMPLE99_H
|
||||
|
||||
#include "controller_base99.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
class controller_example99 : public controller_base99
|
||||
{
|
||||
public:
|
||||
controller_example99();
|
||||
private:
|
||||
virtual void control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||
alt_zones current_zone;
|
||||
|
||||
float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s ¶ms, float Ts);
|
||||
float c_error_;
|
||||
float c_integrator_;
|
||||
|
||||
float roll_hold(float phi_c, float phi, float p, const struct params_s ¶ms, float Ts);
|
||||
float r_error_;
|
||||
float r_integrator;
|
||||
|
||||
float pitch_hold(float theta_c, float theta, float q, const struct params_s ¶ms, float Ts);
|
||||
float p_error_;
|
||||
float p_integrator_;
|
||||
|
||||
float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s ¶ms, float Ts);
|
||||
float ap_error_;
|
||||
float ap_integrator_;
|
||||
float ap_differentiator_;
|
||||
|
||||
float decend_airspeed_with_pitch_hold(float h_c, float h, const struct params_s ¶ms, float Ts);
|
||||
float decend_airspeed_oil(float h_c, float h, const params_s ¶ms, float Ts);
|
||||
|
||||
float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s ¶ms, float Ts);
|
||||
float at_error_;
|
||||
float at_integrator_;
|
||||
float at_differentiator_;
|
||||
|
||||
float altitiude_hold(float h_c, float h, const struct params_s ¶ms, float Ts);
|
||||
float a_error_;
|
||||
float a_integrator_;
|
||||
float a_differentiator_;
|
||||
|
||||
// float cooridinated_turn_hold(float v, const struct params_s ¶ms, float Ts);
|
||||
// float ct_error_;
|
||||
// float ct_integrator_;
|
||||
// float ct_differentiator_;
|
||||
|
||||
float sat(float value, float up_limit, float low_limit);
|
||||
};
|
||||
} //end namespace
|
||||
|
||||
#endif // CONTROLLER_EXAMPLE99_H
|
||||
|
|
@ -0,0 +1,131 @@
|
|||
/**
|
||||
* @file estimator_base.h
|
||||
*
|
||||
* Base class definition for autopilot estimator in chapter 8 of UAVbook, see http://uavbook.byu.edu/doku.php
|
||||
*
|
||||
* @author Gary Ellingson <gary.ellingson@byu.edu>
|
||||
*/
|
||||
|
||||
#ifndef ESTIMATOR_BASE_H
|
||||
#define ESTIMATOR_BASE_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <enemy_suv_msgs/State.h>
|
||||
#include <rosflight_msgs/GPS.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <rosflight_msgs/Barometer.h>
|
||||
#include <rosflight_msgs/Airspeed.h>
|
||||
#include <rosflight_msgs/Status.h>
|
||||
#include <math.h>
|
||||
#include <Eigen/Eigen>
|
||||
|
||||
#define EARTH_RADIUS 6378145.0f
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
|
||||
class estimator_base
|
||||
{
|
||||
public:
|
||||
estimator_base();
|
||||
|
||||
protected:
|
||||
|
||||
struct input_s
|
||||
{
|
||||
float gyro_x;
|
||||
float gyro_y;
|
||||
float gyro_z;
|
||||
float accel_x;
|
||||
float accel_y;
|
||||
float accel_z;
|
||||
float static_pres;
|
||||
float diff_pres;
|
||||
bool gps_new;
|
||||
float gps_n;
|
||||
float gps_e;
|
||||
float gps_h;
|
||||
float gps_Vg;
|
||||
float gps_course;
|
||||
bool status_armed;
|
||||
bool armed_init;
|
||||
};
|
||||
|
||||
struct output_s
|
||||
{
|
||||
float pn;
|
||||
float pe;
|
||||
float h;
|
||||
float Va;
|
||||
float alpha;
|
||||
float beta;
|
||||
float phi;
|
||||
float theta;
|
||||
float psi;
|
||||
float chi;
|
||||
float p;
|
||||
float q;
|
||||
float r;
|
||||
float Vg;
|
||||
float wn;
|
||||
float we;
|
||||
};
|
||||
|
||||
struct params_s
|
||||
{
|
||||
double gravity;
|
||||
double rho;
|
||||
double sigma_accel;
|
||||
double sigma_n_gps;
|
||||
double sigma_e_gps;
|
||||
double sigma_Vg_gps;
|
||||
double sigma_course_gps;
|
||||
double Ts;
|
||||
};
|
||||
|
||||
virtual void estimate(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0;
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle nh_private_;
|
||||
ros::Publisher vehicle_state_pub_;
|
||||
ros::Subscriber gps_sub_;
|
||||
ros::Subscriber imu_sub_;
|
||||
ros::Subscriber baro_sub_;
|
||||
ros::Subscriber airspeed_sub_;
|
||||
ros::Subscriber status_sub_;
|
||||
|
||||
void update(const ros::TimerEvent &);
|
||||
void gpsCallback(const rosflight_msgs::GPS &msg);
|
||||
void imuCallback(const sensor_msgs::Imu &msg);
|
||||
void baroAltCallback(const rosflight_msgs::Barometer &msg);
|
||||
void airspeedCallback(const rosflight_msgs::Airspeed &msg);
|
||||
void statusCallback(const rosflight_msgs::Status &msg);
|
||||
|
||||
double update_rate_;
|
||||
ros::Timer update_timer_;
|
||||
std::string gps_topic_;
|
||||
std::string imu_topic_;
|
||||
std::string baro_topic_;
|
||||
std::string airspeed_topic_;
|
||||
std::string status_topic_;
|
||||
|
||||
bool gps_new_;
|
||||
bool gps_init_;
|
||||
double init_lat_; /**< Initial latitude in degrees */
|
||||
double init_lon_; /**< Initial longitude in degrees */
|
||||
float init_alt_; /**< Initial altitude in meters above MSL */
|
||||
bool armed_first_time_; /**< Arm before starting estimation */
|
||||
bool baro_init_; /**< Initial barometric pressure */
|
||||
float init_static_; /**< Initial static pressure (mbar) */
|
||||
int baro_count_; /**< Used to grab the first set of baro measurements */
|
||||
std::vector<float> init_static_vector_; /**< Used to grab the first set of baro measurements */
|
||||
|
||||
struct params_s params_;
|
||||
struct input_s input_;
|
||||
};
|
||||
|
||||
} //end namespace
|
||||
|
||||
#endif // ESTIMATOR_BASE_H
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
|
||||
// modified by kobe and zhangshuai
|
||||
|
||||
#ifndef ESTIMATOR_BASE_PUB_H
|
||||
#define ESTIMATOR_BASE_PUB_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <enemy_suv_msgs/State.h>
|
||||
#include <enemy_suv_msgs/State29.h>
|
||||
#include <math.h>
|
||||
#include <Eigen/Eigen>
|
||||
|
||||
#define EARTH_RADIUS 6378145.0f
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
class estimator_base_99pub
|
||||
{
|
||||
public:
|
||||
estimator_base_99pub();
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle nh_private_;
|
||||
ros::Publisher vehicle_state99_pub_;
|
||||
ros::Subscriber true_state_sub_;
|
||||
|
||||
void update(const ros::TimerEvent &);
|
||||
void truestateCallback(const enemy_suv_msgs::StateConstPtr &msg);
|
||||
|
||||
double update_rate_;
|
||||
ros::Timer update_timer_;
|
||||
|
||||
std::string truth_topic_;
|
||||
|
||||
bool state_init_;
|
||||
enemy_suv_msgs::State true_state_;
|
||||
|
||||
};
|
||||
|
||||
} //end namespace
|
||||
|
||||
#endif //
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
// modified by kobe and zhangshuai
|
||||
|
||||
#ifndef ESTIMATOR_BASE_SUB_H
|
||||
#define ESTIMATOR_BASE_SUB_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <enemy_suv_msgs/State.h>
|
||||
#include <enemy_suv_msgs/State29.h> //采用新的消息头文件
|
||||
|
||||
#include <math.h>
|
||||
#include <Eigen/Eigen>
|
||||
|
||||
#define EARTH_RADIUS 6378145.0f
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
class estimator_base_statesub
|
||||
{
|
||||
public:
|
||||
estimator_base_statesub();
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle nh_private_;
|
||||
ros::Publisher vehicle_state_pub_;
|
||||
ros::Subscriber state29_sub_;
|
||||
void update(const ros::TimerEvent &);
|
||||
void state99Callback(const enemy_suv_msgs::State29ConstPtr &msg);
|
||||
|
||||
double update_rate_;
|
||||
ros::Timer update_timer_;
|
||||
std::string state29_topic_;
|
||||
|
||||
double init_lat_; /**< Initial latitude in degrees */
|
||||
double init_lon_; /**< Initial longitude in degrees */
|
||||
float init_alt_; /**< Initial altitude in meters above MSL */
|
||||
|
||||
bool state_init_;
|
||||
enemy_suv_msgs::State29 state29_;
|
||||
};
|
||||
|
||||
} //end namespace
|
||||
|
||||
#endif //
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
// modified by kobe
|
||||
|
||||
#ifndef ESTIMATOR_BASE_UPDATA_SUB_H
|
||||
#define ESTIMATOR_BASE_UPDATA_SUB_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <enemy_suv_msgs/State.h>
|
||||
#include <enemy_suv_msgs/Up_Data_New.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <Eigen/Eigen>
|
||||
|
||||
#define EARTH_RADIUS 6378145.0f
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
class estimator_base_updata_statesub
|
||||
{
|
||||
public:
|
||||
estimator_base_updata_statesub();
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle nh_private_;
|
||||
ros::Publisher vehicle_state_pub_;
|
||||
ros::Subscriber state29_sub_;
|
||||
void update(const ros::TimerEvent &);
|
||||
void state99Callback(const enemy_suv_msgs::Up_Data_NewConstPtr &msg);
|
||||
|
||||
double update_rate_;
|
||||
ros::Timer update_timer_;
|
||||
std::string state29_topic_;
|
||||
|
||||
double init_lat_; /**< Initial latitude in degrees */
|
||||
double init_lon_; /**< Initial longitude in degrees */
|
||||
float init_alt_; /**< Initial altitude in meters above MSL */
|
||||
|
||||
bool state_init_;
|
||||
enemy_suv_msgs::Up_Data_New state29_;
|
||||
};
|
||||
|
||||
} //end namespace
|
||||
|
||||
#endif //
|
||||
|
|
@ -0,0 +1,73 @@
|
|||
#ifndef ESTIMATOR_EXAMPLE_H
|
||||
#define ESTIMATOR_EXAMPLE_H
|
||||
|
||||
#include "estimator_base.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <Eigen/Eigen>
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
class estimator_example : public estimator_base
|
||||
{
|
||||
public:
|
||||
estimator_example();
|
||||
|
||||
private:
|
||||
virtual void estimate(const params_s ¶ms, const input_s &input, output_s &output);
|
||||
|
||||
// float gps_n_old_;
|
||||
// float gps_e_old_;
|
||||
// float gps_Vg_old_;
|
||||
// float gps_course_old_;
|
||||
|
||||
double lpf_a_;
|
||||
float alpha_;
|
||||
float alpha1_;
|
||||
int N_;
|
||||
|
||||
float lpf_gyro_x_;
|
||||
float lpf_gyro_y_;
|
||||
float lpf_gyro_z_;
|
||||
float lpf_static_;
|
||||
float lpf_diff_;
|
||||
float lpf_accel_x_;
|
||||
float lpf_accel_y_;
|
||||
float lpf_accel_z_;
|
||||
|
||||
float phat_;
|
||||
float qhat_;
|
||||
float rhat_;
|
||||
float Vwhat_;
|
||||
float phihat_;
|
||||
float thetahat_;
|
||||
float psihat_;
|
||||
Eigen::Vector2f xhat_a_; // 2
|
||||
Eigen::Matrix2f P_a_; // 2x2
|
||||
|
||||
Eigen::VectorXf xhat_p_; // 7
|
||||
Eigen::MatrixXf P_p_; // 7x7
|
||||
|
||||
Eigen::Matrix2f Q_a_; // 2x2
|
||||
float R_accel_;
|
||||
Eigen::Vector2f f_a_; // 2
|
||||
Eigen::Matrix2f A_a_; // 2x2
|
||||
float h_a_;
|
||||
Eigen::Vector2f C_a_; // 2
|
||||
Eigen::Vector2f L_a_; // 2
|
||||
|
||||
Eigen::MatrixXf Q_p_; // 7x7
|
||||
Eigen::MatrixXf R_p_; // 6x6
|
||||
Eigen::VectorXf f_p_; // 7
|
||||
Eigen::MatrixXf A_p_; // 7x7
|
||||
float h_p_;
|
||||
Eigen::VectorXf C_p_; // 7
|
||||
Eigen::VectorXf L_p_; // 7
|
||||
|
||||
void check_xhat_a();
|
||||
|
||||
};
|
||||
} //end namespace
|
||||
|
||||
#endif // ESTIMATOR_EXAMPLE_H
|
||||
|
|
@ -0,0 +1,103 @@
|
|||
//* @author AIRC-DA group,questions contack kobe.
|
||||
|
||||
#ifndef PATH_FOLLOWER_BASE_H
|
||||
#define PATH_FOLLOWER_BASE_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <enemy_suv_msgs/State.h>
|
||||
#include <enemy_suv_msgs/Controller_Commands.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <rosplane/FollowerConfig.h>
|
||||
#include <enemy_suv_msgs/Current_Path.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#define EARTH_RADIUS 6378145.0f
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
enum class path_type
|
||||
{
|
||||
Orbit,
|
||||
Line,
|
||||
Star //add by kobe
|
||||
};
|
||||
|
||||
class path_follower_base
|
||||
{
|
||||
public:
|
||||
path_follower_base();
|
||||
float spin();
|
||||
|
||||
protected:
|
||||
|
||||
struct input_s
|
||||
{
|
||||
enum path_type p_type;
|
||||
float Va_d;
|
||||
float r_path[3];
|
||||
float q_path[3];
|
||||
float c_orbit[3];
|
||||
float rho_orbit;
|
||||
int lam_orbit;
|
||||
float pn; /** position north */
|
||||
float pe; /** position east */
|
||||
float h; /** altitude */
|
||||
float Va; /** airspeed */
|
||||
float chi; /** course angle */
|
||||
float h_c_path; /** goal attitude */
|
||||
float Va_c_path; /** goal attitude */
|
||||
bool landing;
|
||||
bool takeoff;
|
||||
};
|
||||
|
||||
struct output_s
|
||||
{
|
||||
double Va_c; /** commanded airspeed (m/s) */
|
||||
double h_c; /** commanded altitude (m) */
|
||||
double chi_c; /** commanded course (rad) */
|
||||
double phi_ff; /** feed forward term for orbits (rad) */
|
||||
};
|
||||
|
||||
struct params_s
|
||||
{
|
||||
double chi_infty;
|
||||
double k_path;
|
||||
double k_orbit;
|
||||
};
|
||||
|
||||
virtual void follow(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0;
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle nh_private_;
|
||||
ros::Subscriber vehicle_state_sub_;
|
||||
ros::Subscriber bebop1_state_sub_;
|
||||
ros::Subscriber current_path_sub_;
|
||||
|
||||
ros::Publisher controller_commands_pub_;
|
||||
|
||||
double update_rate_ = 100.0;
|
||||
ros::Timer update_timer_;
|
||||
|
||||
enemy_suv_msgs::Controller_Commands controller_commands_;
|
||||
struct params_s params_; /**< params */
|
||||
struct input_s input_;
|
||||
|
||||
void vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg);
|
||||
bool state_init_;
|
||||
|
||||
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
|
||||
float Quaternion_to_Euler(float,float,float,float);
|
||||
|
||||
void current_path_callback(const enemy_suv_msgs::Current_PathConstPtr &msg);
|
||||
bool current_path_init_;
|
||||
|
||||
dynamic_reconfigure::Server<rosplane::FollowerConfig> server_;
|
||||
dynamic_reconfigure::Server<rosplane::FollowerConfig>::CallbackType func_;
|
||||
void reconfigure_callback(rosplane::FollowerConfig &config, uint32_t level);
|
||||
|
||||
void update(const ros::TimerEvent &);
|
||||
};
|
||||
|
||||
} // end namespace
|
||||
|
||||
#endif // PATH_FOLLOWER_BASE_H
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
#ifndef PATH_FOLLOWER_EXAMPLE_H
|
||||
#define PATH_FOLLOWER_EXAMPLE_H
|
||||
|
||||
#include "path_follower_base.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
class path_follower_example : public path_follower_base
|
||||
{
|
||||
public:
|
||||
path_follower_example();
|
||||
private:
|
||||
virtual void follow(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||
};
|
||||
|
||||
} //end namespace
|
||||
#endif // PATH_FOLLOWER_EXAMPLE_H
|
||||
|
|
@ -0,0 +1,122 @@
|
|||
//* @author AIRC-DA group,questions contack kobe.
|
||||
|
||||
#ifndef PATH_MANAGER_BASE_H
|
||||
#define PATH_MANAGER_BASE_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <enemy_suv_msgs/State.h>
|
||||
#include <enemy_suv_msgs/Current_Path.h>
|
||||
#include <enemy_suv_msgs/Goal_Info.h>
|
||||
#include <enemy_suv_msgs/Down_Data_New.h>
|
||||
// #include <commond_msgs/Waypoint.h>
|
||||
#include <enemy_suv_msgs/Waypoint.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <std_msgs/Float32.h>
|
||||
#include <std_msgs/Float32MultiArray.h>
|
||||
#include <sensor_msgs/FluidPressure.h>
|
||||
#include <math.h>
|
||||
#include <Eigen/Eigen>
|
||||
// #include <rosplane/ControllerConfig.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#define EARTH_RADIUS 6378145.0f
|
||||
namespace rosplane
|
||||
{
|
||||
class path_manager_base
|
||||
{
|
||||
public:
|
||||
path_manager_base();
|
||||
|
||||
protected:
|
||||
//modified by kobe
|
||||
|
||||
struct waypoint_s
|
||||
{
|
||||
float w[3];
|
||||
float lat;
|
||||
float lon;
|
||||
float chi_d;
|
||||
bool chi_valid;
|
||||
float Va_d;
|
||||
bool landing;
|
||||
bool takeoff;
|
||||
};
|
||||
|
||||
std::vector<waypoint_s> waypoints_;
|
||||
|
||||
waypoint_s waypoint_start_;
|
||||
waypoint_s waypoint_end_;
|
||||
bool waypoint_received_;
|
||||
|
||||
int num_waypoints_;
|
||||
int idx_a_; /** index to the waypoint that was most recently achieved */
|
||||
|
||||
float min_distance_;
|
||||
double min_distance_titude;
|
||||
bool show; //add by kobe
|
||||
bool show2;
|
||||
struct input_s
|
||||
{
|
||||
float pn; /** position north */
|
||||
float pe; /** position east */
|
||||
float lat; //latitude
|
||||
float lon; //lontitude
|
||||
float h; /** altitude */
|
||||
float chi; /** course angle */
|
||||
float va; //add by kobe
|
||||
};
|
||||
|
||||
struct output_s
|
||||
{
|
||||
float Va_c;
|
||||
int flag; /** Inicates strait line or orbital path (1 is line, 0 is orbit, 2 is star) modified by kobe*/
|
||||
float Va_d; /** Desired airspeed (m/s) */
|
||||
float r[3]; /** Vector to origin of straight line path (m) */
|
||||
float q[3]; /** Unit vector, desired direction of travel for line path */
|
||||
float c[3]; /** Center of orbital path (m) */
|
||||
float gxy[2]; /** x-y-coordinate (m) */
|
||||
float gll[2]; /** latitude-longtitude */
|
||||
float rho; /** Radius of orbital path (m) */
|
||||
float h_c;
|
||||
bool landing;
|
||||
bool takeoff;
|
||||
int8_t lambda; /** Direction of orbital path (cw is 1, ccw is -1) */
|
||||
};
|
||||
|
||||
struct params_s
|
||||
{
|
||||
double R_min;
|
||||
};
|
||||
|
||||
virtual void manage(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0;
|
||||
|
||||
private:
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle nh_private_;
|
||||
ros::Subscriber vehicle_state_sub_; /**< vehicle state subscription */
|
||||
ros::Subscriber bebop1_state_sub_;
|
||||
|
||||
ros::Subscriber new_waypoint_sub_; /**< new waypoint subscription */
|
||||
ros::Publisher current_path_pub_; /**< controller commands publication */
|
||||
ros::Publisher goal_info_pub_; /**< goal info publication */
|
||||
ros::Publisher down_data_pub_;
|
||||
ros::Subscriber waypoint_received_sub_;
|
||||
struct params_s params_;
|
||||
|
||||
enemy_suv_msgs::State vehicle_state_; /**< vehicle state */
|
||||
nav_msgs::Odometry bebop1_state_;
|
||||
|
||||
double update_rate_;
|
||||
ros::Timer update_timer_;
|
||||
|
||||
void vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg);
|
||||
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
|
||||
float Quaternion_to_Euler(float,float,float,float);
|
||||
|
||||
bool state_init_;
|
||||
void new_waypoint_callback(const enemy_suv_msgs::Waypoint &msg);
|
||||
void current_path_publish(const ros::TimerEvent &);
|
||||
void waypoint_received_callback(const enemy_suv_msgs::Waypoint &msg);
|
||||
};
|
||||
} //end namespace
|
||||
#endif // PATH_MANAGER_BASE_H
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
#ifndef PATH_MANAGER_EXAMPLE_H
|
||||
#define PATH_MANAGER_EXAMPLE_H
|
||||
|
||||
#include "path_manager_base.h"
|
||||
#include <Eigen/Eigen>
|
||||
//#include <lib/mathlib/mathlib.h>
|
||||
|
||||
|
||||
#define M_PI_F 3.14159265358979323846f
|
||||
#define M_PI_2_F 1.57079632679489661923f
|
||||
namespace rosplane
|
||||
{
|
||||
enum class fillet_state
|
||||
{
|
||||
STRAIGHT,
|
||||
ORBIT
|
||||
};
|
||||
|
||||
enum class dubin_state
|
||||
{
|
||||
FIRST,
|
||||
BEFORE_H1,
|
||||
BEFORE_H1_WRONG_SIDE,
|
||||
STRAIGHT,
|
||||
BEFORE_H3,
|
||||
BEFORE_H3_WRONG_SIDE
|
||||
};
|
||||
|
||||
class path_manager_example : public path_manager_base
|
||||
{
|
||||
public:
|
||||
path_manager_example();
|
||||
private:
|
||||
virtual void manage(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||
|
||||
void manage_line(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||
void manage_star(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);//add by kobe
|
||||
void manage_fillet(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||
fillet_state fil_state_;
|
||||
void manage_dubins(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||
dubin_state dub_state_;
|
||||
double earth_distance(double, double, double, double);
|
||||
|
||||
bool judge_condition1;
|
||||
bool judge_condition2;
|
||||
bool judge_condition3;
|
||||
bool judge_condition4;
|
||||
bool judge_condition5;
|
||||
struct dubinspath_s
|
||||
{
|
||||
|
||||
Eigen::Vector3f ps; /** the start position */
|
||||
float chis; /** the start course angle */
|
||||
Eigen::Vector3f pe; /** the end position */
|
||||
float chie; /** the end course angle */
|
||||
float R; /** turn radius */
|
||||
float L; /** length of the path */
|
||||
Eigen::Vector3f cs; /** center of the start circle */
|
||||
int lams; /** direction of the start circle */
|
||||
Eigen::Vector3f ce; /** center of the endcircle */
|
||||
int lame; /** direction of the end circle */
|
||||
Eigen::Vector3f w1; /** vector defining half plane H1 */
|
||||
Eigen::Vector3f q1; /** unit vector along striaght line path */
|
||||
Eigen::Vector3f w2; /** vector defining half plane H2 */
|
||||
Eigen::Vector3f w3; /** vector defining half plane H3 */
|
||||
Eigen::Vector3f q3; /** unit vector defining direction of half plane H3 */
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct dubinspath_s dubinspath_;
|
||||
void dubinsParameters(const struct waypoint_s start_node, const struct waypoint_s end_node, float R);
|
||||
|
||||
Eigen::Matrix3f rotz(float theta);
|
||||
float mo(float in);
|
||||
};
|
||||
} //end namespace
|
||||
#endif // PATH_MANAGER_EXAMPLE_H
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
/**
|
||||
*
|
||||
* Refer autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php
|
||||
*pseudo fix-wing 四旋翼模拟固定翼飞行，订阅当前实际及命令的速度、高度和角度，发布向前，向上下速度和角度
|
||||
* @author AIRC-DA group,questions contack kobe.
|
||||
*/
|
||||
|
||||
#ifndef PSEUDO_CONTROLLER_BASE_H
|
||||
#define PSEUDO_CONTROLLER_BASE_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <rosflight_msgs/Command.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <enemy_suv_msgs/State.h>
|
||||
#include <enemy_suv_msgs/Controller_Commands.h>
|
||||
#include <enemy_suv_msgs/Controller_Internals.h>
|
||||
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <rosplane/ControllerConfig.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
enum class alt_zones
|
||||
{
|
||||
PREPARE,
|
||||
TAKE_OFF,
|
||||
CLIMB,
|
||||
DESCEND,
|
||||
ALTITUDE_HOLD,
|
||||
LANDING
|
||||
};
|
||||
|
||||
class pseudo_controller_base
|
||||
{
|
||||
public:
|
||||
pseudo_controller_base();
|
||||
float spin();
|
||||
|
||||
protected:
|
||||
|
||||
struct input_s
|
||||
{
|
||||
float Ts; /** time step */
|
||||
float va; /** airspeed */
|
||||
float h; /** altitude */
|
||||
float chi; /** course angle */
|
||||
float Va_c; /** commanded airspeed (m/s) */
|
||||
float h_c; /** commanded altitude (m) */
|
||||
float chi_c; /** commanded course (north)(-pi to pi,clockwise:-) */
|
||||
bool takeoff;
|
||||
bool landing;
|
||||
};
|
||||
|
||||
struct output_s
|
||||
{
|
||||
//float theta_c; //俯仰角--即平行于机身轴线并指向飞行器前方的向量与地面的夹角
|
||||
//float phi_c; //滚转角--机体坐标系OZb轴与通过机体OXb轴的铅垂面间的夹角，机体向右滚为正，反之为负
|
||||
//float delta_a; //副翼偏移量-转弯用-副翼跳变造成油门和升降舵数值的波动
|
||||
//float delta_e; //升降舵偏移量-拉高降低
|
||||
//float delta_r; //方向舵偏移量-保持稳定，消除测滑
|
||||
//float delta_t; //油门偏移量
|
||||
float forward_trans; //前进力度
|
||||
float up_down_trans; //向上、向下力度,上正下负
|
||||
float left_right_trans; //向左右力度,左正右负
|
||||
float roll_trans; //左右滚转，x轴,逆时针为正，顺时针为负，[-pi/2,pi/2]
|
||||
float rotate_trans; //z旋转,逆时针为正，顺时针为负，[-pi/2,pi/2]
|
||||
float rotate_value;
|
||||
alt_zones current_zone;
|
||||
};
|
||||
|
||||
struct params_s
|
||||
{
|
||||
double alt_hz; /**< altitude hold zone */
|
||||
double alt_toz; /**< altitude takeoff zone */
|
||||
double tau;
|
||||
double c_kp;
|
||||
//double c_kd;
|
||||
double c_ki;
|
||||
|
||||
double a_p_kp;
|
||||
double a_p_kd;
|
||||
double a_p_ki;
|
||||
|
||||
double a_t_ki;
|
||||
double a_t_kp;
|
||||
double a_t_kd;
|
||||
|
||||
double a_kp;
|
||||
double a_kd;
|
||||
double a_ki;
|
||||
|
||||
double trim_t;
|
||||
//double max_a;y
|
||||
double max_t;
|
||||
};
|
||||
bool reached;
|
||||
virtual void control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0;
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle nh_private_;
|
||||
ros::Subscriber vehicle_state_sub_;
|
||||
ros::Subscriber bebop1_state_sub_;
|
||||
|
||||
ros::Subscriber controller_commands_sub_;
|
||||
ros::Publisher pesudors_pub_;//actuators
|
||||
ros::Publisher internals_pub_;
|
||||
ros::Timer act_pub_timer_;
|
||||
|
||||
struct params_s params_; /**< params */
|
||||
enemy_suv_msgs::Controller_Commands controller_commands_;
|
||||
enemy_suv_msgs::State vehicle_state_;
|
||||
nav_msgs::Odometry bebop1_state_;
|
||||
|
||||
void vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg);
|
||||
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
|
||||
float Quaternion_to_Euler(float,float,float,float);
|
||||
|
||||
void controller_commands_callback(const enemy_suv_msgs::Controller_CommandsConstPtr &msg);
|
||||
bool command_recieved_;
|
||||
|
||||
dynamic_reconfigure::Server<rosplane::ControllerConfig> server_;
|
||||
dynamic_reconfigure::Server<rosplane::ControllerConfig>::CallbackType func_;
|
||||
|
||||
void reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level);
|
||||
|
||||
/*Convert from deflection angle to pwm*/
|
||||
//void convert_to_pwm(struct output_s &output);
|
||||
|
||||
/* Publish the outputs */
|
||||
void actuator_controls_publish(const ros::TimerEvent &);
|
||||
};
|
||||
} //end namespace
|
||||
|
||||
#endif // PSEUDO_CONTROLLER_BASE_H
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
#ifndef PSEUDO_CONTROLLER_EXAMPLE_H
|
||||
#define PSEUDO_CONTROLLER_EXAMPLE_H
|
||||
|
||||
#include "pseudo_controller_base.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
class pseudo_controller_example : public pseudo_controller_base
|
||||
{
|
||||
public:
|
||||
pseudo_controller_example();
|
||||
private:
|
||||
virtual void control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||
alt_zones current_zone;
|
||||
|
||||
float pseudo_course_hold(float chi,float chi_c, const params_s ¶ms, float Ts);
|
||||
//float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s ¶ms, float Ts);
|
||||
float c_error_;
|
||||
float c_integrator_;
|
||||
|
||||
|
||||
float r_error_;
|
||||
float r_integrator_;
|
||||
|
||||
|
||||
//float roll_hold(float phi_c, float phi, float p, const struct params_s ¶ms, float Ts);
|
||||
//float r_error_;
|
||||
//float r_integrator_;
|
||||
|
||||
//float pitch_hold(float theta_c, float theta, float q, const struct params_s ¶ms, float Ts);
|
||||
//float p_error_;
|
||||
//float p_integrator_;
|
||||
|
||||
|
||||
float pseudo_course(float chi, float chi_c);
|
||||
float pseudo_left_right(float chi, float chi_c, float forward_trans);
|
||||
|
||||
//float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s ¶ms, float Ts);
|
||||
float pseudo_pitch_hold(float h_c, float h, const struct params_s ¶ms, float Ts);
|
||||
float ap_error_;
|
||||
float ap_integrator_;
|
||||
float ap_differentiator_;
|
||||
|
||||
//float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s ¶ms, float Ts);
|
||||
float pseudo_throttle_hold(float Va_c, float Va, const params_s ¶ms, float Ts);
|
||||
float at_error_;
|
||||
float at_integrator_;
|
||||
float at_differentiator_;
|
||||
|
||||
//float altitiude_hold(float h_c, float h, const struct params_s ¶ms, float Ts);
|
||||
float pseudo_altitiude_hold(float h_c, float h, const params_s ¶ms, float Ts);
|
||||
float a_error_;
|
||||
float a_integrator_;
|
||||
float a_differentiator_;
|
||||
|
||||
// float cooridinated_turn_hold(float v, const struct params_s ¶ms, float Ts);
|
||||
// float ct_error_;
|
||||
// float ct_integrator_;
|
||||
// float ct_differentiator_;
|
||||
|
||||
float sat(float value, float up_limit, float low_limit);
|
||||
};
|
||||
} //end namespace
|
||||
|
||||
#endif // PSEUDO_CONTROLLER_EXAMPLE_H
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>suv_move</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The suv_move package</description>
|
||||
|
||||
<maintainer email="gary.ellingson@byu.edu">Gary Ellingson</maintainer>
|
||||
|
||||
<author email="gary.ellingson@byu.edu">Gary Ellingson</author>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>cmake_modules</build_depend>
|
||||
<build_depend>dynamic_reconfigure</build_depend>
|
||||
<build_depend>rosflight_msgs</build_depend>
|
||||
<build_depend>rosplane_msgs</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<run_depend>dynamic_reconfigure</run_depend>
|
||||
<run_depend>rosflight_msgs</run_depend>
|
||||
<run_depend>rosplane_msgs</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
|
||||
|
||||
<export>
|
||||
</export>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
TRIM_E: 0
|
||||
TRIM_A: 0
|
||||
TRIM_R: 0
|
||||
TRIM_T: 0.6
|
||||
|
||||
COURSE_KP: 0.7329
|
||||
COURSE_KD: 0
|
||||
COURSE_KI: 0.07
|
||||
|
||||
ROLL_KP: 1.17
|
||||
ROLL_KD: -0.13
|
||||
ROLL_KI: 0
|
||||
|
||||
PITCH_KP: 0.1
|
||||
PITCH_KD: -0.017
|
||||
PITCH_KI: 0
|
||||
PITCH_FF: 0
|
||||
|
||||
AS_PITCH_KP: -0.0713
|
||||
AS_PITCH_KD: -0.0635
|
||||
AS_PITCH_KI: 0
|
||||
|
||||
AS_THR_KP: 3.2
|
||||
AS_THR_KD: 0
|
||||
AS_THR_KI: 1.0
|
||||
|
||||
ALT_KP: 0.045
|
||||
ALT_KD: 0
|
||||
ALT_KI: 0.01
|
||||
|
||||
BETA_KP: -0.1164
|
||||
BETA_KD: 0
|
||||
BETA_KI: -0.0037111
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
TRIM_E: 0
|
||||
TRIM_A: 0
|
||||
TRIM_R: 0
|
||||
TRIM_T: 0.6
|
||||
|
||||
COURSE_KP: 0.7329
|
||||
COURSE_KD: 0
|
||||
COURSE_KI: 0.0
|
||||
|
||||
ROLL_KP: 1.2855
|
||||
ROLL_KD: -0.1
|
||||
ROLL_KI: 0
|
||||
|
||||
PITCH_KP: 1.0
|
||||
PITCH_KD: -0.17
|
||||
PITCH_KI: 0
|
||||
PITCH_FF: 0
|
||||
|
||||
AS_PITCH_KP: -0.0713
|
||||
AS_PITCH_KD: -0.0635
|
||||
AS_PITCH_KI: 0
|
||||
|
||||
|
||||
AS_THR_KP: 3.2
|
||||
AS_THR_KD: 0
|
||||
AS_THR_KI: 1.0
|
||||
|
||||
|
||||
ALT_KP: 0.045
|
||||
ALT_KD: 0
|
||||
ALT_KI: 0.01
|
||||
|
||||
|
||||
BETA_KP: -0.1164
|
||||
BETA_KD: 0
|
||||
BETA_KI: -0.0037111
|
||||
|
||||
|
||||
CHI_INFTY: 1.0472
|
||||
K_PATH: 0.025
|
||||
K_ORBIT: 2.00
|
||||
|
|
@ -0,0 +1,203 @@
|
|||
#include "controller_base.h"
|
||||
#include "controller_example.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
controller_base::controller_base():
|
||||
nh_(ros::NodeHandle()),
|
||||
nh_private_(ros::NodeHandle())
|
||||
{
|
||||
vehicle_state_sub_ = nh_.subscribe("state", 10, &controller_base::vehicle_state_callback, this);
|
||||
controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &controller_base::controller_commands_callback,
|
||||
this);
|
||||
|
||||
memset(&vehicle_state_, 0, sizeof(vehicle_state_));
|
||||
memset(&controller_commands_, 0, sizeof(controller_commands_));
|
||||
|
||||
nh_private_.param<double>("TRIM_E", params_.trim_e, 0.0);
|
||||
nh_private_.param<double>("TRIM_A", params_.trim_a, 0.0);
|
||||
nh_private_.param<double>("TRIM_R", params_.trim_r, 0.0);
|
||||
nh_private_.param<double>("TRIM_T", params_.trim_t, 0.6);
|
||||
nh_private_.param<double>("PWM_RAD_E", params_.pwm_rad_e, 1.0);
|
||||
nh_private_.param<double>("PWM_RAD_A", params_.pwm_rad_a, 1.0);
|
||||
nh_private_.param<double>("PWM_RAD_R", params_.pwm_rad_r, 1.0);
|
||||
nh_private_.param<double>("ALT_TOZ", params_.alt_toz, 20.0); /**< altitude takeoff zone */
|
||||
nh_private_.param<double>("ALT_HZ", params_.alt_hz, 10.0); /**< altitude hold zone */
|
||||
nh_private_.param<double>("TAU", params_.tau, 5.0);
|
||||
nh_private_.param<double>("COURSE_KP", params_.c_kp, 0.7329);
|
||||
nh_private_.param<double>("COURSE_KD", params_.c_kd, 0.0);
|
||||
nh_private_.param<double>("COURSE_KI", params_.c_ki, 0.0);
|
||||
nh_private_.param<double>("ROLL_KP", params_.r_kp, 1.2855);
|
||||
nh_private_.param<double>("ROLL_KD", params_.r_kd, -0.325);
|
||||
nh_private_.param<double>("ROLL_KI", params_.r_ki, 0.0);//0.10f);
|
||||
nh_private_.param<double>("PITCH_KP", params_.p_kp, 1.0);
|
||||
nh_private_.param<double>("PITCH_KD", params_.p_kd, -0.17);
|
||||
nh_private_.param<double>("PITCH_KI", params_.p_ki, 0.0);
|
||||
nh_private_.param<double>("PITCH_FF", params_.p_ff, 0.0);
|
||||
nh_private_.param<double>("AS_PITCH_KP", params_.a_p_kp, -0.0713);
|
||||
nh_private_.param<double>("AS_PITCH_KD", params_.a_p_kd, -0.0635);
|
||||
nh_private_.param<double>("AS_PITCH_KI", params_.a_p_ki, 0.0);
|
||||
nh_private_.param<double>("AS_THR_KP", params_.a_t_kp, 3.2);
|
||||
nh_private_.param<double>("AS_THR_KD", params_.a_t_kd, 0.0);
|
||||
nh_private_.param<double>("AS_THR_KI", params_.a_t_ki, 0.0);
|
||||
nh_private_.param<double>("ALT_KP", params_.a_kp, 0.045);
|
||||
nh_private_.param<double>("ALT_KD", params_.a_kd, 0.0);
|
||||
nh_private_.param<double>("ALT_KI", params_.a_ki, 0.01);
|
||||
nh_private_.param<double>("BETA_KP", params_.b_kp, -0.1164);
|
||||
nh_private_.param<double>("BETA_KD", params_.b_kd, 0.0);
|
||||
nh_private_.param<double>("BETA_KI", params_.b_ki, -0.0037111);
|
||||
nh_private_.param<double>("MAX_E", params_.max_e, 0.610);
|
||||
nh_private_.param<double>("MAX_A", params_.max_a, 0.523);
|
||||
nh_private_.param<double>("MAX_R", params_.max_r, 0.523);
|
||||
nh_private_.param<double>("MAX_T", params_.max_t, 1.0);
|
||||
|
||||
func_ = boost::bind(&controller_base::reconfigure_callback, this, _1, _2);
|
||||
server_.setCallback(func_);
|
||||
|
||||
actuators_pub_ = nh_.advertise<rosflight_msgs::Command>("command", 10);
|
||||
internals_pub_ = nh_.advertise<enemy_suv_msgs::Controller_Internals>("controller_inners", 10);
|
||||
act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &controller_base::actuator_controls_publish, this);
|
||||
|
||||
command_recieved_ = false;
|
||||
}
|
||||
|
||||
void controller_base::vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg)
|
||||
{
|
||||
vehicle_state_ = *msg;
|
||||
}
|
||||
|
||||
void controller_base::controller_commands_callback(const enemy_suv_msgs::Controller_CommandsConstPtr &msg)
|
||||
{
|
||||
command_recieved_ = true;
|
||||
controller_commands_ = *msg;
|
||||
}
|
||||
|
||||
void controller_base::reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level)
|
||||
{
|
||||
params_.trim_e = config.TRIM_E;
|
||||
params_.trim_a = config.TRIM_A;
|
||||
params_.trim_r = config.TRIM_R;
|
||||
params_.trim_t = config.TRIM_T;
|
||||
|
||||
params_.c_kp = config.COURSE_KP;
|
||||
params_.c_kd = config.COURSE_KD;
|
||||
params_.c_ki = config.COURSE_KI;
|
||||
|
||||
params_.r_kp = config.ROLL_KP;
|
||||
params_.r_kd = config.ROLL_KD;
|
||||
params_.r_ki = config.ROLL_KI;
|
||||
|
||||
params_.p_kp = config.PITCH_KP;
|
||||
params_.p_kd = config.PITCH_KD;
|
||||
params_.p_ki = config.PITCH_KI;
|
||||
params_.p_ff = config.PITCH_FF;
|
||||
|
||||
params_.a_p_kp = config.AS_PITCH_KP;
|
||||
params_.a_p_kd = config.AS_PITCH_KD;
|
||||
params_.a_p_ki = config.AS_PITCH_KI;
|
||||
|
||||
params_.a_t_kp = config.AS_THR_KP;
|
||||
params_.a_t_kd = config.AS_THR_KD;
|
||||
params_.a_t_ki = config.AS_THR_KI;
|
||||
|
||||
params_.a_kp = config.ALT_KP;
|
||||
params_.a_kd = config.ALT_KD;
|
||||
params_.a_ki = config.ALT_KI;
|
||||
|
||||
params_.b_kp = config.BETA_KP;
|
||||
params_.b_kd = config.BETA_KD;
|
||||
params_.b_ki = config.BETA_KI;
|
||||
}
|
||||
|
||||
void controller_base::convert_to_pwm(controller_base::output_s &output)
|
||||
{
|
||||
output.delta_e = output.delta_e*params_.pwm_rad_e;
|
||||
output.delta_a = output.delta_a*params_.pwm_rad_a;
|
||||
output.delta_r = output.delta_r*params_.pwm_rad_r;
|
||||
}
|
||||
|
||||
void controller_base::actuator_controls_publish(const ros::TimerEvent &)
|
||||
{
|
||||
struct input_s input;
|
||||
input.h = -vehicle_state_.position[2];
|
||||
input.va = vehicle_state_.Va;
|
||||
input.phi = vehicle_state_.phi;
|
||||
input.theta = vehicle_state_.theta;
|
||||
input.chi = vehicle_state_.chi;
|
||||
input.psi = vehicle_state_.psi;
|
||||
input.p = vehicle_state_.p;
|
||||
input.q = vehicle_state_.q;
|
||||
input.r = vehicle_state_.r;
|
||||
input.Va_c = controller_commands_.Va_c;
|
||||
input.h_c = controller_commands_.h_c;
|
||||
input.chi_c = controller_commands_.chi_c;
|
||||
input.phi_ff = controller_commands_.phi_ff;
|
||||
input.Ts = 0.01f;
|
||||
|
||||
struct output_s output;
|
||||
//ROS_INFO("%g",input.va);
|
||||
//std::cout<<"command_recieved_"<<command_recieved_<<std::endl;
|
||||
if (command_recieved_ == true)
|
||||
{
|
||||
control(params_, input, output);
|
||||
|
||||
convert_to_pwm(output);
|
||||
|
||||
rosflight_msgs::Command actuators;
|
||||
/* publish actuator controls */
|
||||
|
||||
actuators.ignore = 0;
|
||||
actuators.mode = rosflight_msgs::Command::MODE_PASS_THROUGH;
|
||||
actuators.x = output.delta_a;//(isfinite(output.delta_a)) ? output.delta_a : 0.0f;
|
||||
actuators.y = output.delta_e;//(isfinite(output.delta_e)) ? output.delta_e : 0.0f;
|
||||
actuators.z = output.delta_r;//(isfinite(output.delta_r)) ? output.delta_r : 0.0f;
|
||||
actuators.F = output.delta_t;//(isfinite(output.delta_t)) ? output.delta_t : 0.0f;
|
||||
|
||||
actuators_pub_.publish(actuators);
|
||||
//ROS_INFO("Hello-----------------------------------------------------------------------");
|
||||
//ROS_INFO("command is here: %f %f %f",input.va,input.psi,input.chi);
|
||||
//ROS_INFO("%g %g %g %g",actuators.x,actuators.y,actuators.z,actuators.F);
|
||||
//std::cout<<"actuators.x: "<<actuators.x<<std::endl;
|
||||
//std::cout<<"actuators.y: "<<actuators.y<<std::endl;
|
||||
//std::cout<<"actuators.z: "<<actuators.z<<std::endl;
|
||||
//std::cout<<"actuators.F: "<<actuators.F<<std::endl;
|
||||
if (internals_pub_.getNumSubscribers() > 0)
|
||||
{
|
||||
enemy_suv_msgs::Controller_Internals inners;
|
||||
inners.phi_c = output.phi_c;
|
||||
inners.theta_c = output.theta_c;
|
||||
switch (output.current_zone)
|
||||
{
|
||||
case alt_zones::TAKE_OFF:
|
||||
inners.alt_zone = inners.ZONE_TAKE_OFF;
|
||||
break;
|
||||
case alt_zones::CLIMB:
|
||||
inners.alt_zone = inners.ZONE_CLIMB;
|
||||
break;
|
||||
case alt_zones::DESCEND:
|
||||
inners.alt_zone = inners.ZONE_DESEND;
|
||||
break;
|
||||
case alt_zones::ALTITUDE_HOLD:
|
||||
inners.alt_zone = inners.ZONE_ALTITUDE_HOLD;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
inners.aux_valid = false;
|
||||
internals_pub_.publish(inners);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} //end namespace
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_controller");
|
||||
rosplane::controller_base *cont = new rosplane::controller_example();
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,203 @@
|
|||
#include "controller_base99.h"
|
||||
#include "controller_example99.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
controller_base99::controller_base99():
|
||||
nh_(ros::NodeHandle()),
|
||||
nh_private_(ros::NodeHandle())
|
||||
{
|
||||
vehicle_state_sub_ = nh_.subscribe("state", 10, &controller_base99::vehicle_state_callback, this);
|
||||
controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &controller_base99::controller_commands_callback,
|
||||
this);
|
||||
|
||||
memset(&vehicle_state_, 0, sizeof(vehicle_state_));
|
||||
memset(&controller_commands_, 0, sizeof(controller_commands_));
|
||||
|
||||
nh_private_.param<double>("TRIM_E", params_.trim_e, 0.0);
|
||||
nh_private_.param<double>("TRIM_A", params_.trim_a, 0.0);
|
||||
nh_private_.param<double>("TRIM_R", params_.trim_r, 0.0);
|
||||
nh_private_.param<double>("TRIM_T", params_.trim_t, 0.6);
|
||||
nh_private_.param<double>("PWM_RAD_E", params_.pwm_rad_e, 1.0);
|
||||
nh_private_.param<double>("PWM_RAD_A", params_.pwm_rad_a, 1.0);
|
||||
nh_private_.param<double>("PWM_RAD_R", params_.pwm_rad_r, 1.0);
|
||||
nh_private_.param<double>("ALT_TOZ", params_.alt_toz, 20.0); /**< altitude takeoff zone */
|
||||
nh_private_.param<double>("ALT_HZ", params_.alt_hz, 10.0); /**< altitude hold zone */
|
||||
nh_private_.param<double>("TAU", params_.tau, 5.0);
|
||||
nh_private_.param<double>("COURSE_KP", params_.c_kp, 0.7329);
|
||||
nh_private_.param<double>("COURSE_KD", params_.c_kd, 0.0);
|
||||
nh_private_.param<double>("COURSE_KI", params_.c_ki, 0.0);
|
||||
nh_private_.param<double>("ROLL_KP", params_.r_kp, 1.2855);
|
||||
nh_private_.param<double>("ROLL_KD", params_.r_kd, -0.325);
|
||||
nh_private_.param<double>("ROLL_KI", params_.r_ki, 0.0);//0.10f);
|
||||
nh_private_.param<double>("PITCH_KP", params_.p_kp, 1.0);
|
||||
nh_private_.param<double>("PITCH_KD", params_.p_kd, -0.17);
|
||||
nh_private_.param<double>("PITCH_KI", params_.p_ki, 0.0);
|
||||
nh_private_.param<double>("PITCH_FF", params_.p_ff, 0.0);
|
||||
nh_private_.param<double>("AS_PITCH_KP", params_.a_p_kp, -0.0713);
|
||||
nh_private_.param<double>("AS_PITCH_KD", params_.a_p_kd, -0.0635);
|
||||
nh_private_.param<double>("AS_PITCH_KI", params_.a_p_ki, 0.0);
|
||||
nh_private_.param<double>("AS_THR_KP", params_.a_t_kp, 3.2);
|
||||
nh_private_.param<double>("AS_THR_KD", params_.a_t_kd, 0.0);
|
||||
nh_private_.param<double>("AS_THR_KI", params_.a_t_ki, 0.0);
|
||||
nh_private_.param<double>("ALT_KP", params_.a_kp, 0.045);
|
||||
nh_private_.param<double>("ALT_KD", params_.a_kd, 0.0);
|
||||
nh_private_.param<double>("ALT_KI", params_.a_ki, 0.01);
|
||||
nh_private_.param<double>("BETA_KP", params_.b_kp, -0.1164);
|
||||
nh_private_.param<double>("BETA_KD", params_.b_kd, 0.0);
|
||||
nh_private_.param<double>("BETA_KI", params_.b_ki, -0.0037111);
|
||||
nh_private_.param<double>("MAX_E", params_.max_e, 0.610);
|
||||
nh_private_.param<double>("MAX_A", params_.max_a, 0.523);
|
||||
nh_private_.param<double>("MAX_R", params_.max_r, 0.523);
|
||||
nh_private_.param<double>("MAX_T", params_.max_t, 1.0);
|
||||
|
||||
func_ = boost::bind(&controller_base99::reconfigure_callback, this, _1, _2);
|
||||
server_.setCallback(func_);
|
||||
|
||||
actuators_pub_ = nh_.advertise<rosflight_msgs::Command>("command", 10);
|
||||
internals_pub_ = nh_.advertise<enemy_suv_msgs::Controller_Internals>("controller_inners", 10);
|
||||
act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &controller_base99::actuator_controls_publish, this);
|
||||
|
||||
command_recieved_ = false;
|
||||
}
|
||||
|
||||
void controller_base99::vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg)
|
||||
{
|
||||
vehicle_state_ = *msg;
|
||||
}
|
||||
|
||||
void controller_base99::controller_commands_callback(const enemy_suv_msgs::Controller_CommandsConstPtr &msg)
|
||||
{
|
||||
command_recieved_ = true;
|
||||
controller_commands_ = *msg;
|
||||
}
|
||||
|
||||
void controller_base99::reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level)
|
||||
{
|
||||
params_.trim_e = config.TRIM_E;
|
||||
params_.trim_a = config.TRIM_A;
|
||||
params_.trim_r = config.TRIM_R;
|
||||
params_.trim_t = config.TRIM_T;
|
||||
|
||||
params_.c_kp = config.COURSE_KP;
|
||||
params_.c_kd = config.COURSE_KD;
|
||||
params_.c_ki = config.COURSE_KI;
|
||||
|
||||
params_.r_kp = config.ROLL_KP;
|
||||
params_.r_kd = config.ROLL_KD;
|
||||
params_.r_ki = config.ROLL_KI;
|
||||
|
||||
params_.p_kp = config.PITCH_KP;
|
||||
params_.p_kd = config.PITCH_KD;
|
||||
params_.p_ki = config.PITCH_KI;
|
||||
params_.p_ff = config.PITCH_FF;
|
||||
|
||||
params_.a_p_kp = config.AS_PITCH_KP;
|
||||
params_.a_p_kd = config.AS_PITCH_KD;
|
||||
params_.a_p_ki = config.AS_PITCH_KI;
|
||||
|
||||
params_.a_t_kp = config.AS_THR_KP;
|
||||
params_.a_t_kd = config.AS_THR_KD;
|
||||
params_.a_t_ki = config.AS_THR_KI;
|
||||
|
||||
params_.a_kp = config.ALT_KP;
|
||||
params_.a_kd = config.ALT_KD;
|
||||
params_.a_ki = config.ALT_KI;
|
||||
|
||||
params_.b_kp = config.BETA_KP;
|
||||
params_.b_kd = config.BETA_KD;
|
||||
params_.b_ki = config.BETA_KI;
|
||||
}
|
||||
|
||||
void controller_base99::convert_to_pwm(controller_base99::output_s &output)
|
||||
{
|
||||
output.delta_e = output.delta_e*params_.pwm_rad_e;
|
||||
output.delta_a = output.delta_a*params_.pwm_rad_a;
|
||||
output.delta_r = output.delta_r*params_.pwm_rad_r;
|
||||
}
|
||||
|
||||
void controller_base99::actuator_controls_publish(const ros::TimerEvent &)
|
||||
{
|
||||
struct input_s input;
|
||||
input.h = -vehicle_state_.position[2];//>0
|
||||
input.va = vehicle_state_.Va;
|
||||
input.phi = vehicle_state_.phi;
|
||||
input.theta = vehicle_state_.theta;
|
||||
input.chi = vehicle_state_.chi;
|
||||
input.psi = vehicle_state_.psi;
|
||||
input.p = vehicle_state_.p;
|
||||
input.q = vehicle_state_.q;
|
||||
input.r = vehicle_state_.r;
|
||||
input.Va_c = controller_commands_.Va_c;
|
||||
input.h_c = controller_commands_.h_c;
|
||||
input.chi_c = controller_commands_.chi_c;
|
||||
input.phi_ff = controller_commands_.phi_ff;
|
||||
input.Ts = 0.01f;
|
||||
|
||||
struct output_s output;
|
||||
//ROS_INFO("%g",input.va);
|
||||
//std::cout<<"command_recieved_"<<command_recieved_<<std::endl;
|
||||
if (command_recieved_ == true)
|
||||
{
|
||||
control(params_, input, output);
|
||||
|
||||
convert_to_pwm(output);
|
||||
|
||||
rosflight_msgs::Command actuators;
|
||||
/* publish actuator controls */
|
||||
|
||||
actuators.ignore = 0;
|
||||
actuators.mode = rosflight_msgs::Command::MODE_PASS_THROUGH;
|
||||
actuators.x = output.delta_a;//(isfinite(output.delta_a)) ? output.delta_a : 0.0f;
|
||||
actuators.y = output.delta_e;//(isfinite(output.delta_e)) ? output.delta_e : 0.0f;
|
||||
actuators.z = output.delta_r;//(isfinite(output.delta_r)) ? output.delta_r : 0.0f;
|
||||
actuators.F = output.delta_t;//(isfinite(output.delta_t)) ? output.delta_t : 0.0f;
|
||||
|
||||
actuators_pub_.publish(actuators);
|
||||
//ROS_INFO("Hello-----------------------------------------------------------------------");
|
||||
//ROS_INFO("command is here: %f %f %f",input.va,input.psi,input.chi);
|
||||
//ROS_INFO("%g %g %g %g",actuators.x,actuators.y,actuators.z,actuators.F);
|
||||
//std::cout<<"actuators.x: "<<actuators.x<<std::endl;
|
||||
//std::cout<<"actuators.y: "<<actuators.y<<std::endl;
|
||||
//std::cout<<"actuators.z: "<<actuators.z<<std::endl;
|
||||
//std::cout<<"actuators.F: "<<actuators.F<<std::endl;
|
||||
if (internals_pub_.getNumSubscribers() > 0)
|
||||
{
|
||||
enemy_suv_msgs::Controller_Internals inners;
|
||||
inners.phi_c = output.phi_c;
|
||||
inners.theta_c = output.theta_c;
|
||||
switch (output.current_zone)
|
||||
{
|
||||
case alt_zones::TAKE_OFF:
|
||||
inners.alt_zone = inners.ZONE_TAKE_OFF;
|
||||
break;
|
||||
case alt_zones::CLIMB:
|
||||
inners.alt_zone = inners.ZONE_CLIMB;
|
||||
break;
|
||||
case alt_zones::DESCEND:
|
||||
inners.alt_zone = inners.ZONE_DESEND;
|
||||
break;
|
||||
case alt_zones::ALTITUDE_HOLD:
|
||||
inners.alt_zone = inners.ZONE_ALTITUDE_HOLD;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
inners.aux_valid = false;
|
||||
internals_pub_.publish(inners);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} //end namespace
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_controller99");
|
||||
rosplane::controller_base99 *cont = new rosplane::controller_example99();
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,265 @@
|
|||
#include "controller_example.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
controller_example::controller_example() : controller_base()
|
||||
{
|
||||
current_zone = alt_zones::TAKE_OFF;
|
||||
|
||||
c_error_ = 0;
|
||||
c_integrator_ = 0;
|
||||
r_error_ = 0;
|
||||
r_integrator = 0;
|
||||
p_error_ = 0;
|
||||
p_integrator_ = 0;
|
||||
|
||||
}
|
||||
|
||||
void controller_example::control(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts)
|
||||
output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts);
|
||||
output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts);
|
||||
switch (current_zone)
|
||||
{
|
||||
case alt_zones::TAKE_OFF:
|
||||
output.phi_c = 0;
|
||||
output.delta_a = roll_hold(0.0, input.phi, input.p, params, input.Ts);
|
||||
output.delta_t = params.max_t;
|
||||
output.theta_c = 15.0*3.14/180.0;
|
||||
if (input.h >= params.alt_toz)
|
||||
{
|
||||
ROS_WARN("climb");
|
||||
current_zone = alt_zones::CLIMB;
|
||||
ap_error_ = 0;
|
||||
ap_integrator_ = 0;
|
||||
ap_differentiator_ = 0;
|
||||
}
|
||||
break;
|
||||
case alt_zones::CLIMB:
|
||||
output.delta_t = params.max_t;
|
||||
|
||||
//modified by kobe
|
||||
if (input.Va_c < input.va)
|
||||
{
|
||||
output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);
|
||||
}
|
||||
else
|
||||
{
|
||||
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
|
||||
}
|
||||
//
|
||||
|
||||
if (input.h >= input.h_c - params.alt_hz)
|
||||
{
|
||||
ROS_WARN("hold");
|
||||
current_zone = alt_zones::ALTITUDE_HOLD;
|
||||
at_error_ = 0;
|
||||
at_integrator_ = 0;
|
||||
at_differentiator_ = 0;
|
||||
a_error_ = 0;
|
||||
a_integrator_ = 0;
|
||||
a_differentiator_ = 0;
|
||||
}
|
||||
else if (input.h <= params.alt_toz)
|
||||
{
|
||||
ROS_WARN("takeoff");
|
||||
current_zone = alt_zones::TAKE_OFF;
|
||||
}
|
||||
break;
|
||||
case alt_zones::DESCEND:
|
||||
output.delta_t = 0;
|
||||
output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);
|
||||
if (input.h <= input.h_c + params.alt_hz)
|
||||
{
|
||||
ROS_WARN("hold");
|
||||
current_zone = alt_zones::ALTITUDE_HOLD;
|
||||
at_error_ = 0;
|
||||
at_integrator_ = 0;
|
||||
at_differentiator_ = 0;
|
||||
a_error_ = 0;
|
||||
a_integrator_ = 0;
|
||||
a_differentiator_ = 0;
|
||||
}
|
||||
break;
|
||||
case alt_zones::ALTITUDE_HOLD:
|
||||
output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
|
||||
if (input.h >= input.h_c + params.alt_hz)
|
||||
{
|
||||
ROS_WARN("desend");
|
||||
current_zone = alt_zones::DESCEND;
|
||||
ap_error_ = 0;
|
||||
ap_integrator_ = 0;
|
||||
ap_differentiator_ = 0;
|
||||
}
|
||||
else if (input.h <= input.h_c - params.alt_hz)
|
||||
{
|
||||
ROS_WARN("climb");
|
||||
current_zone = alt_zones::CLIMB;
|
||||
ap_error_ = 0;
|
||||
ap_integrator_ = 0;
|
||||
ap_differentiator_ = 0;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
output.current_zone = current_zone;
|
||||
output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
|
||||
}
|
||||
|
||||
float controller_example::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = chi_c - chi;
|
||||
|
||||
c_integrator_ = c_integrator_ + (Ts/2.0)*(error + c_error_);
|
||||
|
||||
float up = params.c_kp*error;
|
||||
float ui = params.c_ki*c_integrator_;
|
||||
float ud = params.c_kd*r;
|
||||
|
||||
float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0);
|
||||
if (fabs(params.c_ki) >= 0.00001)
|
||||
{
|
||||
float phi_c_unsat = up + ui + ud + phi_ff;
|
||||
c_integrator_ = c_integrator_ + (Ts/params.c_ki)*(phi_c - phi_c_unsat);
|
||||
}
|
||||
|
||||
c_error_ = error;
|
||||
return phi_c;
|
||||
}
|
||||
|
||||
float controller_example::roll_hold(float phi_c, float phi, float p, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = phi_c - phi;
|
||||
|
||||
r_integrator = r_integrator + (Ts/2.0)*(error + r_error_);
|
||||
|
||||
float up = params.r_kp*error;
|
||||
float ui = params.r_ki*r_integrator;
|
||||
float ud = params.r_kd*p;
|
||||
|
||||
float delta_a = sat(up + ui + ud, params.max_a, -params.max_a);
|
||||
if (fabs(params.r_ki) >= 0.00001)
|
||||
{
|
||||
float delta_a_unsat = up + ui + ud;
|
||||
r_integrator = r_integrator + (Ts/params.r_ki)*(delta_a - delta_a_unsat);
|
||||
}
|
||||
|
||||
r_error_ = error;
|
||||
return delta_a;
|
||||
}
|
||||
|
||||
float controller_example::pitch_hold(float theta_c, float theta, float q, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = theta_c - theta;
|
||||
|
||||
p_integrator_ = p_integrator_ + (Ts/2.0)*(error + p_error_);
|
||||
|
||||
float up = params.p_kp*error;
|
||||
float ui = params.p_ki*p_integrator_;
|
||||
float ud = params.p_kd*q;
|
||||
|
||||
float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, params.max_e, -params.max_e);
|
||||
if (fabs(params.p_ki) >= 0.00001)
|
||||
{
|
||||
float delta_e_unsat = params.trim_e/params.pwm_rad_e + up + ui + ud;
|
||||
p_integrator_ = p_integrator_ + (Ts/params.p_ki)*(delta_e - delta_e_unsat);
|
||||
}
|
||||
|
||||
p_error_ = error;
|
||||
return delta_e;
|
||||
}
|
||||
|
||||
float controller_example::airspeed_with_pitch_hold(float Va_c, float Va, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = Va_c - Va;
|
||||
|
||||
ap_integrator_ = ap_integrator_ + (Ts/2.0)*(error + ap_error_);
|
||||
ap_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*ap_differentiator_ + (2.0 /
|
||||
(2.0*params.tau + Ts))*(error - ap_error_);
|
||||
|
||||
float up = params.a_p_kp*error;
|
||||
float ui = params.a_p_ki*ap_integrator_;
|
||||
float ud = params.a_p_kd*ap_differentiator_;
|
||||
|
||||
float theta_c = sat(up + ui + ud, 20.0*3.14/180.0, -25.0*3.14/180.0);
|
||||
if (fabs(params.a_p_ki) >= 0.00001)
|
||||
{
|
||||
float theta_c_unsat = up + ui + ud;
|
||||
ap_integrator_ = ap_integrator_ + (Ts/params.a_p_ki)*(theta_c - theta_c_unsat);
|
||||
}
|
||||
|
||||
ap_error_ = error;
|
||||
return theta_c;
|
||||
}
|
||||
|
||||
float controller_example::airspeed_with_throttle_hold(float Va_c, float Va, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = Va_c - Va;
|
||||
|
||||
at_integrator_ = at_integrator_ + (Ts/2.0)*(error + at_error_);
|
||||
at_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*at_differentiator_ + (2.0 /
|
||||
(2.0*params.tau + Ts))*(error - at_error_);
|
||||
|
||||
float up = params.a_t_kp*error;
|
||||
float ui = params.a_t_ki*at_integrator_;
|
||||
float ud = params.a_t_kd*at_differentiator_;
|
||||
|
||||
float delta_t = sat(params.trim_t + up + ui + ud, params.max_t, 0);
|
||||
if (fabs(params.a_t_ki) >= 0.00001)
|
||||
{
|
||||
float delta_t_unsat = params.trim_t + up + ui + ud;
|
||||
at_integrator_ = at_integrator_ + (Ts/params.a_t_ki)*(delta_t - delta_t_unsat);
|
||||
}
|
||||
|
||||
at_error_ = error;
|
||||
return delta_t;
|
||||
}
|
||||
|
||||
float controller_example::altitiude_hold(float h_c, float h, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = h_c - h;
|
||||
|
||||
a_integrator_ = a_integrator_ + (Ts/2.0)*(error + a_error_);
|
||||
a_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*a_differentiator_ + (2.0 /
|
||||
(2.0*params.tau + Ts))*(error - a_error_);
|
||||
|
||||
float up = params.a_kp*error;
|
||||
float ui = params.a_ki*a_integrator_;
|
||||
float ud = params.a_kd*a_differentiator_;
|
||||
|
||||
float theta_c = sat(up + ui + ud, 35.0*3.14/180.0, -35.0*3.14/180.0);
|
||||
if (fabs(params.a_ki) >= 0.00001)
|
||||
{
|
||||
float theta_c_unsat = up + ui + ud;
|
||||
a_integrator_ = a_integrator_ + (Ts/params.a_ki)*(theta_c - theta_c_unsat);
|
||||
}
|
||||
|
||||
at_error_ = error;
|
||||
return theta_c;
|
||||
}
|
||||
|
||||
//float controller_example::cooridinated_turn_hold(float v, const params_s ¶ms, float Ts)
|
||||
//{
|
||||
// //todo finish this if you want...
|
||||
// return 0;
|
||||
//}
|
||||
|
||||
float controller_example::sat(float value, float up_limit, float low_limit)
|
||||
{
|
||||
float rVal;
|
||||
if (value > up_limit)
|
||||
rVal = up_limit;
|
||||
else if (value < low_limit)
|
||||
rVal = low_limit;
|
||||
else
|
||||
rVal = value;
|
||||
|
||||
return rVal;
|
||||
}
|
||||
|
||||
} //end namespace
|
||||
|
|
@ -0,0 +1,293 @@
|
|||
#include "controller_example99.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
controller_example99::controller_example99() : controller_base99()
|
||||
{
|
||||
current_zone = alt_zones::TAKE_OFF;
|
||||
|
||||
c_error_ = 0;
|
||||
c_integrator_ = 0;
|
||||
r_error_ = 0;
|
||||
r_integrator = 0;
|
||||
p_error_ = 0;
|
||||
p_integrator_ = 0;
|
||||
|
||||
}
|
||||
|
||||
void controller_example99::control(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts)
|
||||
output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts);
|
||||
output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts);
|
||||
switch (current_zone)
|
||||
{
|
||||
case alt_zones::TAKE_OFF:
|
||||
output.phi_c = 0;
|
||||
output.delta_a = roll_hold(0.0, input.phi, input.p, params, input.Ts);
|
||||
output.delta_t = params.max_t;//modified by kobe
|
||||
output.theta_c = 1.0*3.14/180.0;
|
||||
//output.theta_c = 15.0*3.14/180.0;
|
||||
//ROS_WARN("TAKE_OFF");
|
||||
if (input.h >= params.alt_toz)
|
||||
{
|
||||
ROS_WARN("climb");
|
||||
current_zone = alt_zones::CLIMB;
|
||||
ap_error_ = 0;
|
||||
ap_integrator_ = 0;
|
||||
ap_differentiator_ = 0;
|
||||
}
|
||||
break;
|
||||
case alt_zones::CLIMB:
|
||||
output.delta_t = params.max_t;
|
||||
|
||||
//modified by kobe
|
||||
if (input.Va_c < input.va)
|
||||
{
|
||||
output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);//original
|
||||
}
|
||||
else
|
||||
{
|
||||
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
|
||||
}
|
||||
//
|
||||
|
||||
if (input.h >= input.h_c - params.alt_hz)
|
||||
{
|
||||
ROS_WARN("hold");
|
||||
current_zone = alt_zones::ALTITUDE_HOLD;
|
||||
at_error_ = 0;
|
||||
at_integrator_ = 0;
|
||||
at_differentiator_ = 0;
|
||||
a_error_ = 0;
|
||||
a_integrator_ = 0;
|
||||
a_differentiator_ = 0;
|
||||
}
|
||||
else if (input.h <= params.alt_toz)
|
||||
{
|
||||
//ROS_WARN("takeoff");
|
||||
current_zone = alt_zones::TAKE_OFF;
|
||||
}
|
||||
break;
|
||||
case alt_zones::DESCEND:
|
||||
output.delta_t = decend_airspeed_oil(input.h_c, input.h, params, input.Ts);
|
||||
//output.theta_c =airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);
|
||||
output.theta_c =decend_airspeed_with_pitch_hold(input.h_c, input.h, params, input.Ts);
|
||||
//ROS_INFO("Look here %f %f",output.theta_c,pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts));
|
||||
if (input.h <= input.h_c + params.alt_hz)
|
||||
{
|
||||
ROS_WARN("hold");
|
||||
current_zone = alt_zones::ALTITUDE_HOLD;
|
||||
at_error_ = 0;
|
||||
at_integrator_ = 0;
|
||||
at_differentiator_ = 0;
|
||||
a_error_ = 0;
|
||||
a_integrator_ = 0;
|
||||
a_differentiator_ = 0;
|
||||
}
|
||||
break;
|
||||
case alt_zones::ALTITUDE_HOLD:
|
||||
output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
|
||||
//ROS_INFO("Look here %f %f %f",output.delta_t,output.theta_c,input.Va_c);
|
||||
if (input.h >= input.h_c + 200)//add by kobe
|
||||
//if (input.h >= input.h_c + params.alt_hz)//original
|
||||
{
|
||||
ROS_WARN("desend");
|
||||
current_zone = alt_zones::DESCEND;
|
||||
ap_error_ = 0;
|
||||
ap_integrator_ = 0;
|
||||
ap_differentiator_ = 0;
|
||||
}
|
||||
else if (input.h <= input.h_c - 200)
|
||||
//else if (input.h <= input.h_c - params.alt_hz)
|
||||
{
|
||||
ROS_WARN("climb");
|
||||
current_zone = alt_zones::CLIMB;
|
||||
ap_error_ = 0;
|
||||
ap_integrator_ = 0;
|
||||
ap_differentiator_ = 0;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
output.current_zone = current_zone;
|
||||
output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
|
||||
|
||||
}
|
||||
|
||||
float controller_example99::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = chi_c - chi;
|
||||
|
||||
c_integrator_ = c_integrator_ + (Ts/2.0)*(error + c_error_);
|
||||
|
||||
float up = params.c_kp*error;
|
||||
float ui = params.c_ki*c_integrator_;
|
||||
float ud = params.c_kd*r;
|
||||
|
||||
float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0); //add by kobe
|
||||
//float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0);
|
||||
if (fabs(params.c_ki) >= 0.00001)
|
||||
{
|
||||
float phi_c_unsat = up + ui + ud + phi_ff;
|
||||
c_integrator_ = c_integrator_ + (Ts/params.c_ki)*(phi_c - phi_c_unsat);
|
||||
}
|
||||
|
||||
c_error_ = error;
|
||||
return phi_c;
|
||||
}
|
||||
|
||||
float controller_example99::roll_hold(float phi_c, float phi, float p, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = phi_c - phi;
|
||||
|
||||
r_integrator = r_integrator + (Ts/2.0)*(error + r_error_);
|
||||
|
||||
float up = params.r_kp*error;
|
||||
float ui = params.r_ki*r_integrator;
|
||||
float ud = params.r_kd*p;
|
||||
|
||||
float delta_a = sat(up + ui + ud,params.max_a, -params.max_a);//add by kobe,0.1,-0.1
|
||||
//float delta_a = sat(up + ui + ud, params.max_a, -params.max_a);
|
||||
|
||||
if (fabs(params.r_ki) >= 0.00001)
|
||||
{
|
||||
float delta_a_unsat = up + ui + ud;
|
||||
r_integrator = r_integrator + (Ts/params.r_ki)*(delta_a - delta_a_unsat);
|
||||
}
|
||||
|
||||
r_error_ = error;
|
||||
return delta_a;
|
||||
}
|
||||
|
||||
float controller_example99::pitch_hold(float theta_c, float theta, float q, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = theta_c - theta;
|
||||
|
||||
p_integrator_ = p_integrator_ + (Ts/2.0)*(error + p_error_);
|
||||
|
||||
float up = params.p_kp*error;
|
||||
float ui = params.p_ki*p_integrator_;
|
||||
float ud = params.p_kd*q;
|
||||
|
||||
float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, 0.1, -0.1);// add by kobe
|
||||
//float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, params.max_e, -params.max_e);
|
||||
if (fabs(params.p_ki) >= 0.00001)
|
||||
{
|
||||
float delta_e_unsat = params.trim_e/params.pwm_rad_e + up + ui + ud;
|
||||
p_integrator_ = p_integrator_ + (Ts/params.p_ki)*(delta_e - delta_e_unsat);
|
||||
}
|
||||
|
||||
p_error_ = error;
|
||||
return delta_e;
|
||||
}
|
||||
|
||||
float controller_example99::airspeed_with_pitch_hold(float Va_c, float Va, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = Va_c - Va;
|
||||
|
||||
ap_integrator_ = ap_integrator_ + (Ts/2.0)*(error + ap_error_);
|
||||
ap_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*ap_differentiator_ + (2.0 /
|
||||
(2.0*params.tau + Ts))*(error - ap_error_);
|
||||
|
||||
float up = params.a_p_kp*error;
|
||||
float ui = params.a_p_ki*ap_integrator_;
|
||||
float ud = params.a_p_kd*ap_differentiator_;
|
||||
|
||||
float theta_c = sat(up + ui + ud, 2.0*3.14/180.0, -2.0*3.14/180.0);//add by kobe
|
||||
//float theta_c = sat(up + ui + ud, 20.0*3.14/180.0, -25.0*3.14/180.0);
|
||||
if (fabs(params.a_p_ki) >= 0.00001)
|
||||
{
|
||||
float theta_c_unsat = up + ui + ud;
|
||||
ap_integrator_ = ap_integrator_ + (Ts/params.a_p_ki)*(theta_c - theta_c_unsat);
|
||||
}
|
||||
|
||||
ap_error_ = error;
|
||||
return theta_c;
|
||||
}
|
||||
|
||||
float controller_example99::decend_airspeed_with_pitch_hold(float h_c, float h, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = 0.01*(h_c - h);
|
||||
float theta_c = sat(error, 0, -90.0*3.14/180.0);
|
||||
return theta_c;
|
||||
}
|
||||
|
||||
float controller_example99::decend_airspeed_oil(float h_c, float h, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = pow(2,(h_c - h)/50);
|
||||
float theta_t = sat(error, 1, 0);
|
||||
return theta_t;
|
||||
}
|
||||
|
||||
float controller_example99::airspeed_with_throttle_hold(float Va_c, float Va, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = Va_c - Va;
|
||||
|
||||
at_integrator_ = at_integrator_ + (Ts/2.0)*(error + at_error_);
|
||||
at_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*at_differentiator_ + (2.0 /
|
||||
(2.0*params.tau + Ts))*(error - at_error_);
|
||||
|
||||
float up = params.a_t_kp*error;
|
||||
float ui = params.a_t_ki*at_integrator_;
|
||||
float ud = params.a_t_kd*at_differentiator_;
|
||||
|
||||
float delta_t = sat(params.trim_t + up + ui + ud, params.max_t, 0);
|
||||
if (fabs(params.a_t_ki) >= 0.00001)
|
||||
{
|
||||
float delta_t_unsat = params.trim_t + up + ui + ud;
|
||||
at_integrator_ = at_integrator_ + (Ts/params.a_t_ki)*(delta_t - delta_t_unsat);
|
||||
}
|
||||
|
||||
at_error_ = error;
|
||||
return delta_t;
|
||||
}
|
||||
|
||||
float controller_example99::altitiude_hold(float h_c, float h, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = h_c - h;
|
||||
|
||||
a_integrator_ = a_integrator_ + (Ts/2.0)*(error + a_error_);
|
||||
a_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*a_differentiator_ + (2.0 /
|
||||
(2.0*params.tau + Ts))*(error - a_error_);
|
||||
|
||||
float up = params.a_kp*error;
|
||||
float ui = params.a_ki*a_integrator_;
|
||||
float ud = params.a_kd*a_differentiator_;
|
||||
|
||||
float theta_c = sat(up + ui + ud, 5.0*3.14/180.0, -5.0*3.14/180.0);//add by kobe
|
||||
//float theta_c = sat(up + ui + ud, 35.0*3.14/180.0, -35.0*3.14/180.0);
|
||||
if (fabs(params.a_ki) >= 0.00001)
|
||||
{
|
||||
float theta_c_unsat = up + ui + ud;
|
||||
a_integrator_ = a_integrator_ + (Ts/params.a_ki)*(theta_c - theta_c_unsat);
|
||||
}
|
||||
|
||||
at_error_ = error;
|
||||
return theta_c;
|
||||
}
|
||||
|
||||
//float controller_example99::cooridinated_turn_hold(float v, const params_s ¶ms, float Ts)
|
||||
//{
|
||||
// //todo finish this if you want...
|
||||
// return 0;
|
||||
//}
|
||||
|
||||
float controller_example99::sat(float value, float up_limit, float low_limit)
|
||||
{
|
||||
float rVal;
|
||||
if (value > up_limit)
|
||||
rVal = up_limit;
|
||||
else if (value < low_limit)
|
||||
rVal = low_limit;
|
||||
else
|
||||
rVal = value;
|
||||
|
||||
return rVal;
|
||||
}
|
||||
|
||||
} //end namespace
|
||||
|
|
@ -0,0 +1,228 @@
|
|||
#include "estimator_base.h"
|
||||
#include "estimator_example.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
estimator_base::estimator_base():
|
||||
nh_(ros::NodeHandle()),
|
||||
nh_private_(ros::NodeHandle("~"))
|
||||
{
|
||||
nh_private_.param<std::string>("gps_topic", gps_topic_, "gps");
|
||||
nh_private_.param<std::string>("imu_topic", imu_topic_, "imu/data");
|
||||
nh_private_.param<std::string>("baro_topic", baro_topic_, "baro");
|
||||
nh_private_.param<std::string>("airspeed_topic", airspeed_topic_, "airspeed");
|
||||
nh_private_.param<std::string>("status_topic", status_topic_, "status");
|
||||
nh_private_.param<double>("update_rate", update_rate_, 100.0);
|
||||
params_.Ts = 1.0f/update_rate_;
|
||||
params_.gravity = 9.8;
|
||||
nh_private_.param<double>("rho", params_.rho, 1.225);
|
||||
nh_private_.param<double>("sigma_accel", params_.sigma_accel, 0.0245);
|
||||
nh_private_.param<double>("sigma_n_gps", params_.sigma_n_gps, 0.21);
|
||||
nh_private_.param<double>("sigma_e_gps", params_.sigma_e_gps, 0.21);
|
||||
nh_private_.param<double>("sigma_Vg_gps", params_.sigma_Vg_gps, 0.0500);
|
||||
nh_private_.param<double>("sigma_couse_gps", params_.sigma_course_gps, 0.0045);
|
||||
|
||||
gps_sub_ = nh_.subscribe(gps_topic_, 10, &estimator_base::gpsCallback, this);
|
||||
imu_sub_ = nh_.subscribe(imu_topic_, 10, &estimator_base::imuCallback, this);
|
||||
baro_sub_ = nh_.subscribe(baro_topic_, 10, &estimator_base::baroAltCallback, this);
|
||||
airspeed_sub_ = nh_.subscribe(airspeed_topic_, 10, &estimator_base::airspeedCallback, this);
|
||||
status_sub_ = nh_.subscribe(status_topic_, 1, &estimator_base::statusCallback, this);
|
||||
update_timer_ = nh_.createTimer(ros::Duration(1.0/update_rate_), &estimator_base::update, this);
|
||||
vehicle_state_pub_ = nh_.advertise<enemy_suv_msgs::State>("state", 10);
|
||||
init_static_ = 0;
|
||||
baro_count_ = 0;
|
||||
armed_first_time_ = false;
|
||||
}
|
||||
|
||||
void estimator_base::update(const ros::TimerEvent &)
|
||||
{
|
||||
struct output_s output;
|
||||
|
||||
if (armed_first_time_)
|
||||
{
|
||||
estimate(params_, input_, output);
|
||||
}
|
||||
else
|
||||
{
|
||||
output.pn = output.pe = output.h = 0;
|
||||
output.phi = output.theta = output.psi = 0;
|
||||
output.alpha = output.beta = output.chi = 0;
|
||||
output.p = output.q = output.r = 0;
|
||||
output.Va = 0;
|
||||
}
|
||||
|
||||
input_.gps_new = false;
|
||||
|
||||
enemy_suv_msgs::State msg;
|
||||
msg.header.stamp = ros::Time::now();
|
||||
msg.header.frame_id = 1; // Denotes global frame
|
||||
|
||||
msg.position[0] = output.pn;
|
||||
msg.position[1] = output.pe;
|
||||
msg.position[2] = -output.h;
|
||||
if (gps_init_)
|
||||
{
|
||||
msg.initial_lat = init_lat_;
|
||||
msg.initial_lon = init_lon_;
|
||||
msg.initial_alt = init_alt_;
|
||||
}
|
||||
msg.Va = output.Va;
|
||||
msg.alpha = output.alpha;
|
||||
msg.beta = output.beta;
|
||||
msg.phi = output.phi;
|
||||
msg.theta = output.theta;
|
||||
msg.psi = output.psi;
|
||||
msg.chi = output.chi;
|
||||
msg.p = output.p;
|
||||
msg.q = output.q;
|
||||
msg.r = output.r;
|
||||
msg.Vg = output.Vg;
|
||||
msg.wn = output.wn;
|
||||
msg.we = output.we;
|
||||
msg.quat_valid = false;
|
||||
|
||||
msg.psi_deg = fmod(output.psi, 2*M_PI)*180/M_PI; //-360 to 360
|
||||
msg.psi_deg += (msg.psi_deg < -180 ? 360 : 0);
|
||||
msg.psi_deg -= (msg.psi_deg > 180 ? 360 : 0);
|
||||
msg.chi_deg = fmod(output.chi, 2*M_PI)*180/M_PI; //-360 to 360
|
||||
msg.chi_deg += (msg.chi_deg < -180 ? 360 : 0);
|
||||
msg.chi_deg -= (msg.chi_deg > 180 ? 360 : 0);
|
||||
|
||||
vehicle_state_pub_.publish(msg);
|
||||
}
|
||||
|
||||
void estimator_base::gpsCallback(const rosflight_msgs::GPS &msg)
|
||||
{
|
||||
if (msg.fix != true || msg.NumSat < 4 || !std::isfinite(msg.latitude))
|
||||
{
|
||||
input_.gps_new = false;
|
||||
return;
|
||||
}
|
||||
if (!gps_init_)
|
||||
{
|
||||
gps_init_ = true;
|
||||
init_alt_ = msg.altitude;
|
||||
init_lat_ = msg.latitude;
|
||||
init_lon_ = msg.longitude;
|
||||
}
|
||||
else
|
||||
{
|
||||
input_.gps_n = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
|
||||
input_.gps_e = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
|
||||
input_.gps_h = msg.altitude - init_alt_;
|
||||
input_.gps_Vg = msg.speed;
|
||||
if (msg.speed > 0.3)
|
||||
input_.gps_course = msg.ground_course;
|
||||
input_.gps_new = true;
|
||||
//ROS_INFO("%g %g %g",input_.gps_n,input_.gps_e,input_.gps_h);
|
||||
}
|
||||
}
|
||||
|
||||
void estimator_base::imuCallback(const sensor_msgs::Imu &msg)
|
||||
{
|
||||
input_.accel_x = msg.linear_acceleration.x;
|
||||
input_.accel_y = msg.linear_acceleration.y;
|
||||
input_.accel_z = msg.linear_acceleration.z;
|
||||
|
||||
input_.gyro_x = msg.angular_velocity.x;
|
||||
input_.gyro_y = msg.angular_velocity.y;
|
||||
input_.gyro_z = msg.angular_velocity.z;
|
||||
}
|
||||
|
||||
void estimator_base::baroAltCallback(const rosflight_msgs::Barometer &msg)
|
||||
{
|
||||
|
||||
if (armed_first_time_ && !baro_init_)
|
||||
{
|
||||
if (baro_count_ < 100)
|
||||
{
|
||||
init_static_ += msg.pressure;
|
||||
init_static_vector_.push_back(msg.pressure);
|
||||
input_.static_pres = 0;
|
||||
baro_count_ += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
init_static_ = std::accumulate(init_static_vector_.begin(), init_static_vector_.end(),
|
||||
0.0)/init_static_vector_.size();
|
||||
baro_init_ = true;
|
||||
|
||||
//Check that it got a good calibration.
|
||||
std::sort(init_static_vector_.begin(), init_static_vector_.end());
|
||||
float q1 = (init_static_vector_[24] + init_static_vector_[25])/2.0;
|
||||
float q3 = (init_static_vector_[74] + init_static_vector_[75])/2.0;
|
||||
float IQR = q3 - q1;
|
||||
float upper_bound = q3 + 2.0*IQR;
|
||||
float lower_bound = q1 - 2.0*IQR;
|
||||
for (int i = 0; i < 100; i++)
|
||||
{
|
||||
if (init_static_vector_[i] > upper_bound)
|
||||
{
|
||||
baro_init_ = false;
|
||||
baro_count_ = 0;
|
||||
init_static_vector_.clear();
|
||||
ROS_WARN("Bad baro calibration. Recalibrating");
|
||||
break;
|
||||
}
|
||||
else if (init_static_vector_[i] < lower_bound)
|
||||
{
|
||||
baro_init_ = false;
|
||||
baro_count_ = 0;
|
||||
init_static_vector_.clear();
|
||||
ROS_WARN("Bad baro calibration. Recalibrating");
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
float static_pres_old = input_.static_pres;
|
||||
input_.static_pres = -msg.pressure + init_static_;
|
||||
|
||||
float gate_gain = 1.35*params_.rho*params_.gravity;
|
||||
if (input_.static_pres < static_pres_old - gate_gain)
|
||||
{
|
||||
input_.static_pres = static_pres_old - gate_gain;
|
||||
}
|
||||
else if (input_.static_pres > static_pres_old + gate_gain)
|
||||
{
|
||||
input_.static_pres = static_pres_old + gate_gain;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void estimator_base::airspeedCallback(const rosflight_msgs::Airspeed &msg)
|
||||
{
|
||||
float diff_pres_old = input_.diff_pres;
|
||||
input_.diff_pres = msg.differential_pressure;
|
||||
|
||||
float gate_gain = pow(3, 2)*params_.rho/2.0;
|
||||
if (input_.diff_pres < diff_pres_old - gate_gain)
|
||||
{
|
||||
input_.diff_pres = diff_pres_old - gate_gain;
|
||||
}
|
||||
else if (input_.diff_pres > diff_pres_old + gate_gain)
|
||||
{
|
||||
input_.diff_pres = diff_pres_old + gate_gain;
|
||||
}
|
||||
}
|
||||
|
||||
void estimator_base::statusCallback(const rosflight_msgs::Status &msg)
|
||||
{
|
||||
if (!armed_first_time_ && msg.armed)
|
||||
armed_first_time_ = true;
|
||||
}
|
||||
|
||||
} //end namespace
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_estimator");
|
||||
rosplane::estimator_base *est = new rosplane::estimator_example();
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,76 @@
|
|||
//* @author AIRC-DA group,questions contack kobe.
|
||||
|
||||
#include "estimator_base_99pub.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
estimator_base_99pub::estimator_base_99pub():
|
||||
nh_(ros::NodeHandle()),
|
||||
nh_private_(ros::NodeHandle("~"))
|
||||
{
|
||||
|
||||
true_state_sub_ = nh_.subscribe("truth", 1, &estimator_base_99pub::truestateCallback, this);
|
||||
nh_private_.param<double>("update_rate", update_rate_, 100.0);
|
||||
update_timer_ = nh_.createTimer(ros::Duration(1.0/update_rate_), &estimator_base_99pub::update, this);
|
||||
vehicle_state99_pub_ = nh_.advertise<enemy_suv_msgs::State29>("state29", 10);
|
||||
state_init_ = false;
|
||||
}
|
||||
|
||||
void estimator_base_99pub::update(const ros::TimerEvent &)
|
||||
{
|
||||
enemy_suv_msgs::State29 msg;
|
||||
msg.header.stamp = ros::Time::now();
|
||||
msg.header.frame_id = 1; // Denotes global frame
|
||||
if (state_init_)
|
||||
{
|
||||
msg.position_gps[0]=(true_state_.position[0]*180)/(EARTH_RADIUS*M_PI)+true_state_.initial_lat;
|
||||
// pn = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
|
||||
msg.position_gps[1]=(true_state_.position[1]*180)/(EARTH_RADIUS*cos(true_state_.initial_lat*M_PI/180.0)*M_PI)+true_state_.initial_lon;
|
||||
// pe = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
|
||||
// input_.gps_h = msg.altitude - init_alt_;
|
||||
msg.position_gps[2] =-true_state_.position[2]-true_state_.initial_alt; //>0
|
||||
msg.attitude_angle[0] = true_state_.phi;
|
||||
msg.attitude_angle[1] = true_state_.theta;
|
||||
msg.attitude_angle[2] = true_state_.psi;
|
||||
msg.velocity[0] = true_state_.Vn;
|
||||
msg.velocity[1] = true_state_.Ve;
|
||||
msg.velocity[2] = true_state_.Vd;
|
||||
msg.angular_velocity[0] = true_state_.p;
|
||||
msg.angular_velocity[1] = true_state_.q;
|
||||
msg.angular_velocity[2] = true_state_.r;
|
||||
msg.acceleration[0] = -1;
|
||||
msg.acceleration[1] = -1;
|
||||
msg.acceleration[2] = -1;
|
||||
msg.electric_quantity = -1;
|
||||
msg.state_word = -1;
|
||||
// ROS_INFO("GPS-position: %g %g %g",msg.position_gps[0],msg.position_gps[1],msg.position_gps[2]);
|
||||
// ROS_INFO("xyz-position: %g %g %g",true_state_.position[0],true_state_.position[1],true_state_.position[2]);
|
||||
}
|
||||
else
|
||||
{
|
||||
// ROS_WARN("No plane truth data.");
|
||||
}
|
||||
|
||||
vehicle_state99_pub_.publish(msg);
|
||||
}
|
||||
|
||||
|
||||
void estimator_base_99pub::truestateCallback(const enemy_suv_msgs::StateConstPtr &msg)
|
||||
{
|
||||
true_state_ = *msg;
|
||||
state_init_ = true;
|
||||
}
|
||||
|
||||
|
||||
} //end namespace
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_estimator_29pub");
|
||||
rosplane::estimator_base_99pub *cont = new rosplane::estimator_base_99pub();
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,75 @@
|
|||
//* @author AIRC-DA group,questions contack kobe.
|
||||
|
||||
#include "estimator_base_statesub.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
estimator_base_statesub::estimator_base_statesub() : nh_(ros::NodeHandle()),
|
||||
nh_private_(ros::NodeHandle("~"))
|
||||
{
|
||||
state29_sub_ = nh_.subscribe("state29", 10, &estimator_base_statesub::state99Callback, this);
|
||||
nh_private_.param<double>("update_rate", update_rate_, 100.0);
|
||||
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &estimator_base_statesub::update, this);
|
||||
vehicle_state_pub_ = nh_.advertise<enemy_suv_msgs::State>("state", 10);
|
||||
|
||||
state_init_ = false;
|
||||
init_lat_=0; /**< Initial latitude in degrees */
|
||||
init_lon_=0; /**< Initial longitude in degrees */
|
||||
init_alt_=0; //>0
|
||||
}
|
||||
|
||||
void estimator_base_statesub::update(const ros::TimerEvent &)
|
||||
{
|
||||
|
||||
enemy_suv_msgs::State msg;
|
||||
msg.header.stamp = ros::Time::now();
|
||||
msg.header.frame_id = 1; // Denotes global frame
|
||||
if (state_init_)
|
||||
{
|
||||
msg.position[0] = EARTH_RADIUS*(state29_.position_gps[0] - init_lat_)*M_PI/180.0;
|
||||
// pn = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
|
||||
msg.position[1] = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(state29_.position_gps[1] - init_lon_)*M_PI/180.0;
|
||||
// pe = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
|
||||
msg.position[2] = -state29_.position_gps[2]- init_alt_; //<0
|
||||
//input_.gps_h = msg.altitude - init_alt_;
|
||||
msg.Wd=state29_.position_gps[0];
|
||||
msg.Jd=state29_.position_gps[1];
|
||||
msg.Va=sqrt(pow(state29_.velocity[0], 2.0) + pow(state29_.velocity[1], 2.0) + pow(state29_.velocity[2], 2.0));
|
||||
msg.Vg=msg.Va; //true for rosflight
|
||||
msg.phi=state29_.attitude_angle[0];
|
||||
msg.theta=state29_.attitude_angle[1];
|
||||
msg.psi=state29_.attitude_angle[2];
|
||||
//msg.chi=msg.psi;//watch here
|
||||
msg.chi = atan2(msg.Va*sin(msg.psi), msg.Va*cos(msg.psi));
|
||||
msg.p=state29_.angular_velocity[0];
|
||||
msg.q=state29_.angular_velocity[1];
|
||||
msg.r=state29_.angular_velocity[2];
|
||||
|
||||
msg.initial_lat = init_lat_;
|
||||
msg.initial_lon = init_lon_;
|
||||
msg.initial_alt = init_alt_;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
// ROS_WARN("No State29 data.");
|
||||
}
|
||||
vehicle_state_pub_.publish(msg);
|
||||
}
|
||||
|
||||
void estimator_base_statesub::state99Callback(const enemy_suv_msgs::State29ConstPtr &msg)
|
||||
{
|
||||
state29_ = *msg;
|
||||
state_init_ = true;
|
||||
}
|
||||
} // namespace rosplane
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_estimator_statesub");
|
||||
rosplane::estimator_base_statesub *cont = new rosplane::estimator_base_statesub();
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,76 @@
|
|||
//* @author AIRC-DA group,questions contack kobe.
|
||||
|
||||
#include "estimator_base_updata_statesub.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
estimator_base_updata_statesub::estimator_base_updata_statesub() : nh_(ros::NodeHandle()),
|
||||
nh_private_(ros::NodeHandle("~"))
|
||||
{
|
||||
state29_sub_ = nh_.subscribe("/serial_commu_up", 10, &estimator_base_updata_statesub::state99Callback, this);
|
||||
nh_private_.param<double>("update_rate", update_rate_, 100.0);
|
||||
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &estimator_base_updata_statesub::update, this);
|
||||
vehicle_state_pub_ = nh_.advertise<enemy_suv_msgs::State>("state", 10);
|
||||
|
||||
state_init_ = false;
|
||||
init_lat_=0; /**< Initial latitude in degrees */
|
||||
init_lon_=0; /**< Initial longitude in degrees */
|
||||
init_alt_=0; //>0
|
||||
}
|
||||
|
||||
void estimator_base_updata_statesub::update(const ros::TimerEvent &)
|
||||
{
|
||||
|
||||
enemy_suv_msgs::State msg;
|
||||
msg.header.stamp = ros::Time::now();
|
||||
msg.header.frame_id = 1; // Denotes global frame
|
||||
if (state_init_)
|
||||
{
|
||||
msg.position[0] = EARTH_RADIUS*(state29_.latitude - init_lat_)*M_PI/180.0;
|
||||
// pn = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
|
||||
msg.position[1] = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(state29_.longitude - init_lon_)*M_PI/180.0;
|
||||
// pe = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
|
||||
msg.position[2] = -state29_.altitude- init_alt_; //<0
|
||||
//input_.gps_h = msg.altitude - init_alt_;
|
||||
msg.Wd=state29_.latitude;
|
||||
msg.Jd=state29_.longitude;
|
||||
msg.Va=sqrt(pow(state29_.north_direction_speed, 2.0) + pow(state29_.east_direction_speed, 2.0) + pow(state29_.ground_direction_speed, 2.0));
|
||||
msg.Vg=msg.Va; //true for rosflight
|
||||
msg.phi=state29_.roll_angle;
|
||||
msg.theta=state29_.pitch_angle;
|
||||
msg.psi=state29_.yaw_angle;
|
||||
//msg.chi=msg.psi;//watch here
|
||||
msg.chi = atan2(msg.Va*sin(msg.psi), msg.Va*cos(msg.psi));
|
||||
msg.p=state29_.angular_rate_x;
|
||||
msg.q=state29_.angular_rate_y;
|
||||
msg.r=state29_.angular_rate_z;
|
||||
|
||||
msg.initial_lat = init_lat_;
|
||||
msg.initial_lon = init_lon_;
|
||||
msg.initial_alt = init_alt_;
|
||||
// ROS_WARN("Recieved up-data data.");
|
||||
//ROS_INFO("Hello--- %f %f %f %f",state29_.latitude,state29_.longitude,msg.Vg,state29_.altitude);
|
||||
}
|
||||
else
|
||||
{
|
||||
// ROS_WARN("No up-data data.");
|
||||
}
|
||||
vehicle_state_pub_.publish(msg);
|
||||
}
|
||||
|
||||
void estimator_base_updata_statesub::state99Callback(const enemy_suv_msgs::Up_Data_NewConstPtr &msg)
|
||||
{
|
||||
state29_ = *msg;
|
||||
state_init_ = true;
|
||||
}
|
||||
} // namespace rosplane
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_estimator_updata_statesub");
|
||||
rosplane::estimator_base_updata_statesub *cont = new rosplane::estimator_base_updata_statesub();
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,416 @@
|
|||
#include "estimator_base.h"
|
||||
#include "estimator_example.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
float radians(float degrees)
|
||||
{
|
||||
return M_PI*degrees/180.0;
|
||||
}
|
||||
|
||||
estimator_example::estimator_example() :
|
||||
estimator_base(),
|
||||
xhat_a_(Eigen::Vector2f::Zero()),
|
||||
P_a_(Eigen::Matrix2f::Identity()),
|
||||
Q_a_(Eigen::Matrix2f::Identity()),
|
||||
xhat_p_(Eigen::VectorXf::Zero(7)),
|
||||
P_p_(Eigen::MatrixXf::Identity(7, 7)),
|
||||
Q_p_(Eigen::MatrixXf::Identity(7, 7)),
|
||||
R_p_(Eigen::MatrixXf::Zero(7, 7)),
|
||||
f_p_(7),
|
||||
A_p_(7, 7),
|
||||
C_p_(7),
|
||||
L_p_(7)
|
||||
{
|
||||
P_a_ *= powf(radians(5.0f), 2);
|
||||
|
||||
Q_a_(0, 0) = 0.00000001;
|
||||
Q_a_(1, 1) = 0.00000001;
|
||||
|
||||
P_p_ = Eigen::MatrixXf::Identity(7, 7);
|
||||
P_p_(0, 0) = .03;
|
||||
P_p_(1, 1) = .03;
|
||||
P_p_(2, 2) = .01;
|
||||
P_p_(3, 3) = radians(5.0f);
|
||||
P_p_(4, 4) = .04;
|
||||
P_p_(5, 5) = .04;
|
||||
P_p_(6, 6) = radians(5.0f);
|
||||
|
||||
Q_p_ *= 0.0001f;
|
||||
Q_p_(3, 3) = 0.000001f;
|
||||
|
||||
phat_ = 0;
|
||||
qhat_ = 0;
|
||||
rhat_ = 0;
|
||||
phihat_ = 0;
|
||||
thetahat_ = 0;
|
||||
psihat_ = 0;
|
||||
Vwhat_ = 0;
|
||||
|
||||
lpf_static_ = 0;
|
||||
lpf_diff_ = 0;
|
||||
|
||||
N_ = 10;
|
||||
|
||||
alpha_ = 0.0f;
|
||||
}
|
||||
|
||||
void estimator_example::estimate(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
if (alpha_ == 0.0f) //initailze stuff that comes from params
|
||||
{
|
||||
R_accel_ = powf(params.sigma_accel, 2);
|
||||
|
||||
R_p_(0, 0) = powf(params.sigma_n_gps, 2);
|
||||
R_p_(1, 1) = powf(params.sigma_e_gps, 2);
|
||||
R_p_(2, 2) = powf(params.sigma_Vg_gps, 2);
|
||||
R_p_(3, 3) = powf(params.sigma_course_gps, 2);
|
||||
R_p_(4, 4) = 0.001;
|
||||
R_p_(5, 5) = 0.001;
|
||||
|
||||
float lpf_a = 50.0;
|
||||
float lpf_a1 = 8.0;
|
||||
alpha_ = exp(-lpf_a*params.Ts);
|
||||
alpha1_ = exp(-lpf_a1*params.Ts);
|
||||
}
|
||||
|
||||
// low pass filter gyros to estimate angular rates
|
||||
lpf_gyro_x_ = alpha_*lpf_gyro_x_ + (1 - alpha_)*input.gyro_x;
|
||||
lpf_gyro_y_ = alpha_*lpf_gyro_y_ + (1 - alpha_)*input.gyro_y;
|
||||
lpf_gyro_z_ = alpha_*lpf_gyro_z_ + (1 - alpha_)*input.gyro_z;
|
||||
|
||||
float phat = lpf_gyro_x_;
|
||||
float qhat = lpf_gyro_y_;
|
||||
float rhat = lpf_gyro_z_;
|
||||
|
||||
// low pass filter static pressure sensor and invert to esimate altitude
|
||||
lpf_static_ = alpha1_*lpf_static_ + (1 - alpha1_)*input.static_pres;
|
||||
float hhat = lpf_static_/params.rho/params.gravity;
|
||||
|
||||
// low pass filter diff pressure sensor and invert to extimate Va
|
||||
lpf_diff_ = alpha1_*lpf_diff_ + (1 - alpha1_)*input.diff_pres;
|
||||
|
||||
// when the plane isn't moving or moving slowly, the noise in the sensor
|
||||
// will cause the differential pressure to go negative. This will catch
|
||||
// those cases.
|
||||
if (lpf_diff_ <= 0)
|
||||
lpf_diff_ = 0.000001;
|
||||
|
||||
float Vahat = sqrt(2/params.rho*lpf_diff_);
|
||||
|
||||
// low pass filter accelerometers
|
||||
lpf_accel_x_ = alpha_*lpf_accel_x_ + (1 - alpha_)*input.accel_x;
|
||||
lpf_accel_y_ = alpha_*lpf_accel_y_ + (1 - alpha_)*input.accel_y;
|
||||
lpf_accel_z_ = alpha_*lpf_accel_z_ + (1 - alpha_)*input.accel_z;
|
||||
|
||||
// implement continuous-discrete EKF to estimate roll and pitch angles
|
||||
// prediction step
|
||||
float cp; // cos(phi)
|
||||
float sp; // sin(phi)
|
||||
float tt; // tan(thata)
|
||||
float ct; // cos(thata)
|
||||
float st; // sin(theta)
|
||||
for (int i = 0; i < N_; i++)
|
||||
{
|
||||
cp = cosf(xhat_a_(0)); // cos(phi)
|
||||
sp = sinf(xhat_a_(0)); // sin(phi)
|
||||
tt = tanf(xhat_a_(1)); // tan(thata)
|
||||
ct = cosf(xhat_a_(1)); // cos(thata)
|
||||
|
||||
f_a_(0) = phat + (qhat*sp + rhat*cp)*tt;
|
||||
f_a_(1) = qhat*cp - rhat*sp;
|
||||
|
||||
A_a_ = Eigen::Matrix2f::Zero();
|
||||
A_a_(0, 0) = (qhat*cp - rhat*sp)*tt;
|
||||
A_a_(0, 1) = (qhat*sp + rhat*cp)/ct/ct;
|
||||
A_a_(1, 0) = -qhat*sp - rhat*cp;
|
||||
|
||||
xhat_a_ += f_a_*(params.Ts/N_);
|
||||
P_a_ += (A_a_*P_a_ + P_a_*A_a_.transpose() + Q_a_)*(params.Ts/N_);
|
||||
}
|
||||
// measurement updates
|
||||
cp = cosf(xhat_a_(0));
|
||||
sp = sinf(xhat_a_(0));
|
||||
ct = cosf(xhat_a_(1));
|
||||
st = sinf(xhat_a_(1));
|
||||
Eigen::Matrix2f I;
|
||||
I = Eigen::Matrix2f::Identity();
|
||||
|
||||
// x-axis accelerometer
|
||||
h_a_ = qhat*Vahat*st + params.gravity*st;
|
||||
C_a_ = Eigen::Vector2f::Zero();
|
||||
C_a_(1) = qhat*Vahat*ct + params.gravity*ct;
|
||||
L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_);
|
||||
P_a_ = (I - L_a_*C_a_.transpose())*P_a_;
|
||||
xhat_a_ += L_a_*((hhat < 15 ? lpf_accel_x_/3 : lpf_accel_x_) - h_a_);
|
||||
|
||||
// y-axis accelerometer
|
||||
h_a_ = rhat*Vahat*ct - phat*Vahat*st - params.gravity*ct*sp;
|
||||
C_a_ = Eigen::Vector2f::Zero();
|
||||
C_a_(0) = -params.gravity*cp*ct;
|
||||
C_a_(1) = -rhat*Vahat*st - phat*Vahat*ct + params.gravity*st*sp;
|
||||
L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_);
|
||||
P_a_ = (I - L_a_*C_a_.transpose())*P_a_;
|
||||
xhat_a_ += L_a_*(lpf_accel_y_ - h_a_);
|
||||
|
||||
// z-axis accelerometer
|
||||
h_a_ = -qhat*Vahat*ct - params.gravity*ct*cp;
|
||||
C_a_ = Eigen::Vector2f::Zero();
|
||||
C_a_(0) = params.gravity*sp*ct;
|
||||
C_a_(1) = (qhat*Vahat + params.gravity*cp)*st;
|
||||
L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_);
|
||||
P_a_ = (I - L_a_*C_a_.transpose())*P_a_;
|
||||
xhat_a_ += L_a_*(lpf_accel_z_ - h_a_);
|
||||
|
||||
check_xhat_a();
|
||||
|
||||
float phihat = xhat_a_(0);
|
||||
float thetahat = xhat_a_(1);
|
||||
|
||||
// implement continous-discrete EKF to estimate pn, pe, chi, Vg
|
||||
// prediction step
|
||||
float psidot, tmp, Vgdot;
|
||||
if (fabsf(xhat_p_(2)) < 0.01f)
|
||||
{
|
||||
xhat_p_(2) = 0.01; // prevent devide by zero
|
||||
}
|
||||
|
||||
for (int i = 0; i < N_; i++)
|
||||
{
|
||||
psidot = (qhat*sinf(phihat) + rhat*cosf(phihat))/cosf(thetahat);
|
||||
tmp = -psidot*Vahat*(xhat_p_(4)*cosf(xhat_p_(6)) + xhat_p_(5)*sinf(xhat_p_(6)))/xhat_p_(2);
|
||||
Vgdot = ((Vahat*cosf(xhat_p_(6)) + xhat_p_(4))*(-psidot*Vahat*sinf(xhat_p_(6))) + (Vahat*sinf(xhat_p_(
|
||||
6)) + xhat_p_(5))*(psidot*Vahat*cosf(xhat_p_(6))))/xhat_p_(2);
|
||||
|
||||
f_p_ = Eigen::VectorXf::Zero(7);
|
||||
f_p_(0) = xhat_p_(2)*cosf(xhat_p_(3));
|
||||
f_p_(1) = xhat_p_(2)*sinf(xhat_p_(3));
|
||||
f_p_(2) = Vgdot;
|
||||
f_p_(3) = params.gravity/xhat_p_(2)*tanf(phihat)*cosf(xhat_p_(3) - xhat_p_(6));
|
||||
f_p_(6) = psidot;
|
||||
|
||||
A_p_ = Eigen::MatrixXf::Zero(7, 7);
|
||||
A_p_(0, 2) = cos(xhat_p_(3));
|
||||
A_p_(0, 3) = -xhat_p_(2)*sinf(xhat_p_(3));
|
||||
A_p_(1, 2) = sin(xhat_p_(3));
|
||||
A_p_(1, 3) = xhat_p_(2)*cosf(xhat_p_(3));
|
||||
A_p_(2, 2) = -Vgdot/xhat_p_(2);
|
||||
A_p_(2, 4) = -psidot*Vahat*sinf(xhat_p_(6))/xhat_p_(2);
|
||||
A_p_(2, 5) = psidot*Vahat*cosf(xhat_p_(6))/xhat_p_(2);
|
||||
A_p_(2, 6) = tmp;
|
||||
A_p_(3, 2) = -params.gravity/powf(xhat_p_(2), 2)*tanf(phihat)*cosf(xhat_p_(3) - xhat_p_(6));
|
||||
A_p_(3, 3) = -params.gravity/xhat_p_(2)*tanf(phihat)*sinf(xhat_p_(3) - xhat_p_(6));
|
||||
A_p_(3, 6) = params.gravity/xhat_p_(2)*tanf(phihat)*sinf(xhat_p_(3) - xhat_p_(6));
|
||||
|
||||
xhat_p_ += f_p_*(params.Ts/N_);
|
||||
P_p_ += (A_p_*P_p_ + P_p_*A_p_.transpose() + Q_p_)*(params.Ts/N_);
|
||||
}
|
||||
|
||||
// while(xhat_p(3) > radians(180.0f)) xhat_p(3) = xhat_p(3) - radians(360.0f);
|
||||
// while(xhat_p(3) < radians(-180.0f)) xhat_p(3) = xhat_p(3) + radians(360.0f);
|
||||
// if(xhat_p(3) > radians(180.0f) || xhat_p(3) < radians(-180.0f))
|
||||
// {
|
||||
// ROS_WARN("Course estimate not wrapped from -pi to pi");
|
||||
// xhat_p(3) = 0;
|
||||
// }
|
||||
|
||||
// measurement updates
|
||||
if (input.gps_new)
|
||||
{
|
||||
Eigen::MatrixXf I_p(7, 7);
|
||||
I_p = Eigen::MatrixXf::Identity(7, 7);
|
||||
|
||||
// gps North position
|
||||
h_p_ = xhat_p_(0);
|
||||
C_p_ = Eigen::VectorXf::Zero(7);
|
||||
C_p_(0) = 1;
|
||||
L_p_ = (P_p_*C_p_)/(R_p_(0, 0) + (C_p_.transpose()*P_p_*C_p_));
|
||||
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
|
||||
xhat_p_ = xhat_p_ + L_p_*(input.gps_n - h_p_);
|
||||
|
||||
// gps East position
|
||||
h_p_ = xhat_p_(1);
|
||||
C_p_ = Eigen::VectorXf::Zero(7);
|
||||
C_p_(1) = 1;
|
||||
L_p_ = (P_p_*C_p_)/(R_p_(1, 1) + (C_p_.transpose()*P_p_*C_p_));
|
||||
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
|
||||
xhat_p_ = xhat_p_ + L_p_*(input.gps_e - h_p_);
|
||||
|
||||
// gps ground speed
|
||||
h_p_ = xhat_p_(2);
|
||||
C_p_ = Eigen::VectorXf::Zero(7);
|
||||
C_p_(2) = 1;
|
||||
L_p_ = (P_p_*C_p_)/(R_p_(2, 2) + (C_p_.transpose()*P_p_*C_p_));
|
||||
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
|
||||
xhat_p_ = xhat_p_ + L_p_*(input.gps_Vg - h_p_);
|
||||
|
||||
// gps course
|
||||
//wrap course measurement
|
||||
float gps_course = fmodf(input.gps_course, radians(360.0f));
|
||||
|
||||
while (gps_course - xhat_p_(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f);
|
||||
while (gps_course - xhat_p_(3) < radians(-180.0f)) gps_course = gps_course + radians(360.0f);
|
||||
h_p_ = xhat_p_(3);
|
||||
C_p_ = Eigen::VectorXf::Zero(7);
|
||||
C_p_(3) = 1;
|
||||
L_p_ = (P_p_*C_p_)/(R_p_(3, 3) + (C_p_.transpose()*P_p_*C_p_));
|
||||
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
|
||||
xhat_p_ = xhat_p_ + L_p_*(gps_course - h_p_);
|
||||
|
||||
// // pseudo measurement #1 y_1 = Va*cos(psi)+wn-Vg*cos(chi)
|
||||
// h_p = Vahat*cosf(xhat_p(6)) + xhat_p(4) - xhat_p(2)*cosf(xhat_p(3)); // pseudo measurement
|
||||
// C_p = Eigen::VectorXf::Zero(7);
|
||||
// C_p(2) = -cos(xhat_p(3));
|
||||
// C_p(3) = xhat_p(2)*sinf(xhat_p(3));
|
||||
// C_p(4) = 1;
|
||||
// C_p(6) = -Vahat*sinf(xhat_p(6));
|
||||
// L_p = (P_p*C_p)/(R_p(4,4) + (C_p.transpose()*P_p*C_p));
|
||||
// P_p = (I_p - L_p*C_p.transpose())*P_p;
|
||||
// xhat_p = xhat_p + L_p*(0 - h_p);
|
||||
|
||||
// // pseudo measurement #2 y_2 = Va*sin(psi) + we - Vg*sin(chi)
|
||||
// h_p = Vahat*sinf(xhat_p(6))+xhat_p(5)-xhat_p(2)*sinf(xhat_p(3)); // pseudo measurement
|
||||
// C_p = Eigen::VectorXf::Zero(7);
|
||||
// C_p(2) = -sin(xhat_p(3));
|
||||
// C_p(3) = -xhat_p(2)*cosf(xhat_p(3));
|
||||
// C_p(5) = 1;
|
||||
// C_p(6) = Vahat*cosf(xhat_p(6));
|
||||
// L_p = (P_p*C_p)/(R_p(5,5) + (C_p.transpose()*P_p*C_p));
|
||||
// P_p = (I_p - L_p*C_p.transpose())*P_p;
|
||||
// xhat_p = xhat_p + L_p*(0 - h_p);
|
||||
|
||||
if (xhat_p_(0) > 10000 || xhat_p_(0) < -10000)
|
||||
{
|
||||
ROS_WARN("gps n limit reached");
|
||||
xhat_p_(0) = input.gps_n;
|
||||
}
|
||||
if (xhat_p_(1) > 10000 || xhat_p_(1) < -10000)
|
||||
{
|
||||
ROS_WARN("gps e limit reached");
|
||||
xhat_p_(1) = input.gps_e;
|
||||
}
|
||||
}
|
||||
|
||||
bool problem = false;
|
||||
int prob_index;
|
||||
for (int i = 0; i < 7; i++)
|
||||
{
|
||||
if (!std::isfinite(xhat_p_(i)))
|
||||
{
|
||||
if (!problem)
|
||||
{
|
||||
problem = true;
|
||||
prob_index = i;
|
||||
}
|
||||
switch (i)
|
||||
{
|
||||
case 0:
|
||||
xhat_p_(i) = input.gps_n;
|
||||
break;
|
||||
case 1:
|
||||
xhat_p_(i) = input.gps_e;
|
||||
break;
|
||||
case 2:
|
||||
xhat_p_(i) = input.gps_Vg;
|
||||
break;
|
||||
case 3:
|
||||
xhat_p_(i) = input.gps_course;
|
||||
break;
|
||||
case 6:
|
||||
xhat_p_(i) = input.gps_course;
|
||||
break;
|
||||
default:
|
||||
xhat_p_(i) = 0;
|
||||
}
|
||||
P_p_ = Eigen::MatrixXf::Identity(7, 7);
|
||||
P_p_(0, 0) = .03;
|
||||
P_p_(1, 1) = .03;
|
||||
P_p_(2, 2) = .01;
|
||||
P_p_(3, 3) = radians(5.0f);
|
||||
P_p_(4, 4) = .04;
|
||||
P_p_(5, 5) = .04;
|
||||
P_p_(6, 6) = radians(5.0f);
|
||||
}
|
||||
}
|
||||
if (problem)
|
||||
{
|
||||
ROS_WARN("position estimator reinitialized due to non-finite state %d", prob_index);
|
||||
}
|
||||
if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f))
|
||||
{
|
||||
//xhat_p(3) = fmodf(xhat_p(3),2*M_PI);
|
||||
xhat_p_(6) = fmodf(xhat_p_(6), 2*M_PI);
|
||||
}
|
||||
|
||||
float pnhat = xhat_p_(0);
|
||||
float pehat = xhat_p_(1);
|
||||
float Vghat = xhat_p_(2);
|
||||
float chihat = xhat_p_(3);
|
||||
float wnhat = xhat_p_(4);
|
||||
float wehat = xhat_p_(5);
|
||||
float psihat = xhat_p_(6);
|
||||
|
||||
output.pn = pnhat;
|
||||
output.pe = pehat;
|
||||
output.h = hhat;
|
||||
output.Va = Vahat;
|
||||
output.alpha = 0;
|
||||
output.beta = 0;
|
||||
output.phi = phihat;
|
||||
output.theta = thetahat;
|
||||
output.chi = chihat;
|
||||
output.p = phat;
|
||||
output.q = qhat;
|
||||
output.r = rhat;
|
||||
output.Vg = Vghat;
|
||||
output.wn = wnhat;
|
||||
output.we = wehat;
|
||||
output.psi = psihat;
|
||||
}
|
||||
|
||||
void estimator_example::check_xhat_a()
|
||||
{
|
||||
if (xhat_a_(0) > radians(85.0) || xhat_a_(0) < radians(-85.0) || !std::isfinite(xhat_a_(0)))
|
||||
{
|
||||
if (!std::isfinite(xhat_a_(0)))
|
||||
{
|
||||
xhat_a_(0) = 0;
|
||||
P_a_ = Eigen::Matrix2f::Identity();
|
||||
P_a_ *= powf(radians(20.0f), 2);
|
||||
ROS_WARN("attiude estimator reinitialized due to non-finite roll");
|
||||
}
|
||||
else if (xhat_a_(0) > radians(85.0))
|
||||
{
|
||||
xhat_a_(0) = radians(82.0);
|
||||
//ROS_WARN("max roll angle");
|
||||
}
|
||||
else if (xhat_a_(0) < radians(-85.0))
|
||||
{
|
||||
xhat_a_(0) = radians(-82.0);
|
||||
//ROS_WARN("min roll angle");
|
||||
}
|
||||
}
|
||||
if (xhat_a_(1) > radians(80.0) || xhat_a_(1) < radians(-80.0) || !std::isfinite(xhat_a_(1)))
|
||||
{
|
||||
if (!std::isfinite(xhat_a_(1)))
|
||||
{
|
||||
xhat_a_(1) = 0;
|
||||
P_a_ = Eigen::Matrix2f::Identity();
|
||||
P_a_ *= powf(radians(20.0f), 2);
|
||||
ROS_WARN("attiude estimator reinitialized due to non-finite pitch");
|
||||
}
|
||||
else if (xhat_a_(1) > radians(80.0))
|
||||
{
|
||||
xhat_a_(1) = radians(77.0);
|
||||
ROS_WARN("max pitch angle");
|
||||
}
|
||||
else if (xhat_a_(1) < radians(-80.0))
|
||||
{
|
||||
xhat_a_(1) = radians(-77.0);
|
||||
ROS_WARN("min pitch angle");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} //end namespace
|
||||
|
|
@ -0,0 +1,162 @@
|
|||
#include "path_follower_base.h"
|
||||
#include "path_follower_example.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
path_follower_base::path_follower_base() : nh_(ros::NodeHandle()),
|
||||
nh_private_(ros::NodeHandle("~"))
|
||||
{
|
||||
vehicle_state_sub_ = nh_.subscribe<enemy_suv_msgs::State>("state", 1, &path_follower_base::vehicle_state_callback, this);
|
||||
bebop1_state_sub_ = nh_.subscribe<nav_msgs::Odometry>("ground_truth/state", 1, &path_follower_base::bebop1_state_callback, this);
|
||||
//bebop1_state_sub_ = nh_.subscribe<nav_msgs::Odometry>("/test/odom", 1, &path_follower_base::bebop1_state_callback, this);
|
||||
|
||||
current_path_sub_ = nh_.subscribe<enemy_suv_msgs::Current_Path>("current_path", 1,
|
||||
&path_follower_base::current_path_callback, this);
|
||||
|
||||
nh_private_.param<double>("CHI_INFTY", params_.chi_infty, 1.0472);
|
||||
nh_private_.param<double>("K_PATH", params_.k_path, 0.025);
|
||||
nh_private_.param<double>("K_ORBIT", params_.k_orbit, 4.0);
|
||||
|
||||
func_ = boost::bind(&path_follower_base::reconfigure_callback, this, _1, _2);
|
||||
server_.setCallback(func_);
|
||||
|
||||
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &path_follower_base::update, this);
|
||||
controller_commands_pub_ = nh_.advertise<enemy_suv_msgs::Controller_Commands>("controller_commands", 1);
|
||||
|
||||
state_init_ = false;
|
||||
current_path_init_ = false;
|
||||
}
|
||||
|
||||
void path_follower_base::update(const ros::TimerEvent &)
|
||||
{
|
||||
|
||||
struct output_s output;
|
||||
|
||||
if (state_init_ == true && current_path_init_ == true)
|
||||
{
|
||||
follow(params_, input_, output);
|
||||
enemy_suv_msgs::Controller_Commands msg;
|
||||
msg.chi_c = output.chi_c;
|
||||
msg.landing=input_.landing;
|
||||
msg.takeoff=input_.takeoff;
|
||||
if (input_.Va_c_path == 0)
|
||||
{
|
||||
// ROS_INFO("Hello1");
|
||||
msg.Va_c = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
// ROS_INFO("Hello2");
|
||||
msg.Va_c = input_.Va_d;
|
||||
}
|
||||
|
||||
// ROS_INFO("what %f",msg.Va_c);
|
||||
|
||||
msg.h_c = output.h_c;
|
||||
msg.phi_ff = output.phi_ff;
|
||||
controller_commands_pub_.publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
void path_follower_base::vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg)
|
||||
{
|
||||
// input_.pn = msg->position[0]; /** position north */
|
||||
// input_.pe = msg->position[1]; /** position east */
|
||||
// input_.h = -msg->position[2]; /** altitude */
|
||||
// input_.chi = msg->chi;
|
||||
// input_.Va = msg->Va;
|
||||
|
||||
// state_init_ = true;
|
||||
}
|
||||
void path_follower_base::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
|
||||
{
|
||||
// ROS_INFO("%g", msg->pose.pose.position.x); /** position north */
|
||||
input_.pn = msg->pose.pose.position.x; /** position north */
|
||||
input_.pe = msg->pose.pose.position.y; /** position east */
|
||||
input_.h = msg->pose.pose.position.z; /** altitude */
|
||||
input_.Va = sqrt(pow(msg->twist.twist.linear.x, 2) + pow(msg->twist.twist.linear.y, 2));
|
||||
input_.chi = path_follower_base::Quaternion_to_Euler(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
|
||||
// ROS_INFO("%g %g %g %g %g",input_.pn,input_.pe,input_.h,input_.Va,input_.chi);
|
||||
state_init_ = true;
|
||||
}
|
||||
|
||||
float path_follower_base::Quaternion_to_Euler(float q0, float q1, float q2, float q3)
|
||||
{
|
||||
float Radx;
|
||||
float sum;
|
||||
sum = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
if (sum == 0)
|
||||
{
|
||||
sum = 0.0001;
|
||||
}
|
||||
q0 = q0 / sum;
|
||||
q1 = q1 / sum;
|
||||
q2 = q2 / sum;
|
||||
q3 = q3 / sum;
|
||||
if (fabs(q2) < fabs(q3))
|
||||
{
|
||||
Radx = asin(2.0f * (q2 * q3 + q0 * q1));
|
||||
return Radx;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (q2 * q3 > 0)
|
||||
{
|
||||
Radx = 3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
|
||||
return Radx;
|
||||
}
|
||||
else
|
||||
{
|
||||
Radx = -3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
|
||||
return Radx;
|
||||
}
|
||||
}
|
||||
//Radx = asin(2.0f*(q2*q3 + q0*q1));
|
||||
//Rady = atan2(-2 * q1 * q3 + 2 * q0 * q2, q3*q3 - q2 * q2 - q1 * q1 + q0 * q0);
|
||||
//Radz = atan2(2 * q1*q2 - 2 * q0*q3, q2*q2 - q3*q3 + q0*q0 - q1*q1);
|
||||
//return Radx;
|
||||
}
|
||||
void path_follower_base::current_path_callback(const enemy_suv_msgs::Current_PathConstPtr &msg)
|
||||
{
|
||||
if (msg->path_type == msg->LINE_PATH)
|
||||
input_.p_type = path_type::Line;
|
||||
else if (msg->path_type == msg->ORBIT_PATH)
|
||||
input_.p_type = path_type::Orbit;
|
||||
else if (msg->path_type == msg->STAR_PATH) // add by kobe
|
||||
input_.p_type = path_type::Star;
|
||||
|
||||
input_.Va_d = msg->Va_d;
|
||||
// ROS_INFO("hello %f",input_.Va_d);
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
input_.r_path[i] = msg->r[i];
|
||||
input_.q_path[i] = msg->q[i];
|
||||
input_.c_orbit[i] = msg->c[i];
|
||||
}
|
||||
input_.rho_orbit = msg->rho;
|
||||
input_.lam_orbit = msg->lambda;
|
||||
input_.h_c_path = msg->h_c; //add by kobe
|
||||
input_.Va_c_path = msg->Va_d;
|
||||
input_.landing = msg->landing;
|
||||
input_.takeoff = msg->takeoff;
|
||||
current_path_init_ = true;
|
||||
}
|
||||
|
||||
void path_follower_base::reconfigure_callback(rosplane::FollowerConfig &config, uint32_t level)
|
||||
{
|
||||
params_.chi_infty = config.CHI_INFTY;
|
||||
params_.k_path = config.K_PATH;
|
||||
params_.k_orbit = config.K_ORBIT;
|
||||
}
|
||||
} // namespace rosplane
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_path_follower");
|
||||
rosplane::path_follower_base *path = new rosplane::path_follower_example();
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
#include "path_follower_example.h"
|
||||
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
path_follower_example::path_follower_example()
|
||||
{
|
||||
}
|
||||
|
||||
void path_follower_example::follow(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
if (input.p_type == path_type::Line) // follow straight line path specified by r and q
|
||||
{
|
||||
// compute wrapped version of the path angle
|
||||
float chi_q = atan2f(input.q_path[1], input.q_path[0]);
|
||||
|
||||
// modified by kobe
|
||||
float chi=input.chi;
|
||||
while (chi < -M_PI)
|
||||
chi += 2*M_PI;
|
||||
while (chi > M_PI)
|
||||
chi -= 2*M_PI;
|
||||
|
||||
|
||||
while (chi_q - chi < -M_PI)
|
||||
chi_q += 2*M_PI;
|
||||
while (chi_q - chi > M_PI)
|
||||
chi_q -= 2*M_PI;
|
||||
|
||||
float path_error = -sinf(chi_q)*(input.pn - input.r_path[0]) + cosf(chi_q)*(input.pe - input.r_path[1]);
|
||||
// heading command
|
||||
output.chi_c = chi_q - params.chi_infty*2/M_PI*atanf(params.k_path*path_error);
|
||||
//ROS_INFO("hello1: %g %g %g %g",chi_q,input.chi,chi,output.chi_c);
|
||||
|
||||
// desired altitude
|
||||
output.h_c = -input.h_c_path;//h_c_path is negative;h_c is positive.
|
||||
//kobe modified end
|
||||
|
||||
/* ////for truth not state
|
||||
while (chi_q - input.chi < -M_PI)
|
||||
chi_q += 2*M_PI;
|
||||
while (chi_q - input.chi > M_PI)
|
||||
chi_q -= 2*M_PI;
|
||||
float path_error = -sinf(chi_q)*(input.pn - input.r_path[0]) + cosf(chi_q)*(input.pe - input.r_path[1]);
|
||||
// heading command
|
||||
output.chi_c = chi_q - params.chi_infty*2/M_PI*atanf(params.k_path*path_error); //desired altitude
|
||||
float h_d = -input.r_path[2] - sqrtf(powf((input.r_path[0] - input.pn), 2) + powf((input.r_path[1] - input.pe),
|
||||
2))*(input.q_path[2])/sqrtf(powf(input.q_path[0], 2) + powf(input.q_path[1], 2));
|
||||
// commanded altitude is desired altitude
|
||||
output.h_c = h_d;// -input.r_path[2] - x// important for hover// kobe denote
|
||||
*/
|
||||
output.phi_ff = 0.0;
|
||||
}
|
||||
else if(input.p_type == path_type::Orbit)// follow a orbit path specified by c_orbit, rho_orbit, and lam_orbit
|
||||
{
|
||||
float d = sqrtf(powf((input.pn - input.c_orbit[0]), 2) + powf((input.pe - input.c_orbit[1]),
|
||||
2)); // distance from orbit center
|
||||
// compute wrapped version of angular position on orbit
|
||||
float varphi = atan2f(input.pe - input.c_orbit[1], input.pn - input.c_orbit[0]);
|
||||
while ((varphi - input.chi) < -M_PI)
|
||||
varphi += 2*M_PI;
|
||||
while ((varphi - input.chi) > M_PI)
|
||||
varphi -= 2*M_PI;
|
||||
//compute orbit error
|
||||
float norm_orbit_error = (d - input.rho_orbit)/input.rho_orbit;
|
||||
output.chi_c = varphi + input.lam_orbit*(M_PI/2 + atanf(params.k_orbit*norm_orbit_error));
|
||||
|
||||
// commanded altitude is the height of the orbit
|
||||
float h_d = -input.c_orbit[2];
|
||||
output.h_c = h_d;
|
||||
output.phi_ff = 0;//(norm_orbit_error < 0.5 ? input.lam_orbit*atanf(input.Va*input.Va/(9.8*input.rho_orbit)) : 0);
|
||||
}
|
||||
else if(input.p_type == path_type::Star)// towards to a point in any direction on platform,r is the center;add by kobe
|
||||
{
|
||||
|
||||
}
|
||||
output.Va_c = input.Va_d;
|
||||
}
|
||||
|
||||
} //end namespace
|
||||
|
|
@ -0,0 +1,238 @@
|
|||
#include "path_manager_base.h"
|
||||
#include "path_manager_example.h"
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
path_manager_base::path_manager_base() : nh_(ros::NodeHandle()), /** nh_ stuff added here */
|
||||
nh_private_(ros::NodeHandle("~"))
|
||||
{
|
||||
nh_private_.param<double>("R_min", params_.R_min, 25); //25
|
||||
nh_private_.param<double>("update_rate", update_rate_, 10.0);
|
||||
|
||||
vehicle_state_sub_ = nh_.subscribe("state", 10, &path_manager_base::vehicle_state_callback, this);
|
||||
bebop1_state_sub_ = nh_.subscribe("ground_truth/state", 10, &path_manager_base::bebop1_state_callback, this);
|
||||
//bebop1_state_sub_ = nh_.subscribe("/test/odom", 10, &path_manager_base::bebop1_state_callback, this);
|
||||
|
||||
new_waypoint_sub_ = nh_.subscribe("waypoint_path", 10, &path_manager_base::new_waypoint_callback, this);
|
||||
|
||||
current_path_pub_ = nh_.advertise<enemy_suv_msgs::Current_Path>("current_path", 10);
|
||||
goal_info_pub_ = nh_.advertise<enemy_suv_msgs::Goal_Info>("Goal_Info", 10);
|
||||
down_data_pub_ = nh_.advertise<enemy_suv_msgs::Down_Data_New>("serial_commu_down", 10);
|
||||
|
||||
waypoint_received_sub_ = nh_.subscribe("new_waypoint", 10, &path_manager_base::waypoint_received_callback, this);
|
||||
|
||||
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &path_manager_base::current_path_publish, this);
|
||||
|
||||
num_waypoints_ = 0;
|
||||
|
||||
state_init_ = false;
|
||||
|
||||
waypoint_received_ = false;
|
||||
}
|
||||
|
||||
void path_manager_base::waypoint_received_callback(const enemy_suv_msgs::Waypoint &msg)
|
||||
{
|
||||
float ell = sqrtf((waypoint_end_.w[0] - msg.w[0]) * (waypoint_end_.w[0] - msg.w[0]) +
|
||||
(waypoint_end_.w[1] - msg.w[1]) * (waypoint_end_.w[1] - msg.w[1]));
|
||||
if (ell < 2.0 * params_.R_min)
|
||||
{
|
||||
ROS_ERROR("The distance between nodes must be larger than 2R.");
|
||||
return;
|
||||
}
|
||||
waypoint_start_ = waypoint_end_;
|
||||
|
||||
waypoint_end_.w[0] = msg.w[0];
|
||||
waypoint_end_.w[1] = msg.w[1];
|
||||
waypoint_end_.w[2] = msg.w[2];
|
||||
waypoint_end_.chi_d = msg.chi_d;
|
||||
waypoint_end_.chi_valid = msg.chi_valid;
|
||||
waypoint_end_.Va_d = msg.Va_d;
|
||||
|
||||
waypoint_received_ = true;
|
||||
}
|
||||
|
||||
void path_manager_base::vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg)
|
||||
{
|
||||
vehicle_state_ = *msg;
|
||||
//state_init_ = true;
|
||||
}
|
||||
|
||||
void path_manager_base::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
|
||||
{
|
||||
bebop1_state_ = *msg;
|
||||
state_init_ = true;
|
||||
}
|
||||
|
||||
void path_manager_base::new_waypoint_callback(const enemy_suv_msgs::Waypoint &msg)
|
||||
{
|
||||
if (msg.clear_wp_list == true)
|
||||
{
|
||||
waypoints_.clear();
|
||||
num_waypoints_ = 0;
|
||||
idx_a_ = 0;
|
||||
min_distance_ = 1000000000; //add by kobe, just need to be larger than the large-enter-ball's radius.
|
||||
min_distance_titude = 10000000000;
|
||||
//return;
|
||||
}
|
||||
if (msg.set_current || num_waypoints_ == 0)//mean to set the start point as the a goal-never use---kobe denote
|
||||
{
|
||||
waypoint_s currentwp;
|
||||
currentwp.w[0] = bebop1_state_.pose.pose.position.x;
|
||||
currentwp.w[1] = bebop1_state_.pose.pose.position.y;
|
||||
currentwp.w[2] = (-bebop1_state_.pose.pose.position.z > -25 ? msg.w[2] : -bebop1_state_.pose.pose.position.z);
|
||||
currentwp.chi_d= path_manager_base::Quaternion_to_Euler(bebop1_state_.pose.pose.orientation.x,bebop1_state_.pose.pose.orientation.y,bebop1_state_.pose.pose.orientation.z,bebop1_state_.pose.pose.orientation.w);
|
||||
// currentwp.w[0] = vehicle_state_.position[0];
|
||||
// currentwp.w[1] = vehicle_state_.position[1];
|
||||
// currentwp.w[2] = (vehicle_state_.position[2] > -25 ? msg.w[2] : vehicle_state_.position[2]);
|
||||
// currentwp.chi_d = vehicle_state_.chi;
|
||||
// add by kobe
|
||||
currentwp.lat = vehicle_state_.Wd;
|
||||
currentwp.lon = vehicle_state_.Jd;
|
||||
|
||||
currentwp.chi_valid = msg.chi_valid;
|
||||
currentwp.Va_d = msg.Va_d;
|
||||
currentwp.landing=msg.landing;
|
||||
currentwp.takeoff=msg.takeoff;
|
||||
waypoints_.clear();
|
||||
waypoints_.push_back(currentwp);
|
||||
num_waypoints_ = 1;
|
||||
idx_a_ = 0;
|
||||
min_distance_ = 10000000000; //add by kobe, just need to be larger than the large-enter-ball's radius.
|
||||
min_distance_titude = 10000000000;
|
||||
show = 1;
|
||||
show2=1;
|
||||
}
|
||||
waypoint_s nextwp;
|
||||
nextwp.w[0] = msg.w[0];
|
||||
nextwp.w[1] = msg.w[1];
|
||||
nextwp.w[2] = msg.w[2];
|
||||
// add by kobe
|
||||
nextwp.lat=msg.lat;
|
||||
nextwp.lon=msg.lon;
|
||||
|
||||
nextwp.chi_d = msg.chi_d;
|
||||
nextwp.chi_valid = msg.chi_valid;
|
||||
nextwp.Va_d = msg.Va_d;
|
||||
nextwp.landing=msg.landing;
|
||||
nextwp.takeoff=msg.takeoff;
|
||||
waypoints_.push_back(nextwp);
|
||||
num_waypoints_++;
|
||||
// ROS_INFO("num_waypoints_: %d", num_waypoints_);
|
||||
}
|
||||
|
||||
void path_manager_base::current_path_publish(const ros::TimerEvent &)
|
||||
{
|
||||
struct input_s input;
|
||||
input.pn = bebop1_state_.pose.pose.position.x; /** position north */
|
||||
input.pe = bebop1_state_.pose.pose.position.y; /** position east */
|
||||
input.h = bebop1_state_.pose.pose.position.z; /** altitude */
|
||||
input.chi =path_manager_base::Quaternion_to_Euler(bebop1_state_.pose.pose.orientation.x,bebop1_state_.pose.pose.orientation.y,bebop1_state_.pose.pose.orientation.z,bebop1_state_.pose.pose.orientation.w);
|
||||
input.va = sqrt(pow(bebop1_state_.twist.twist.linear.x,2)+pow(bebop1_state_.twist.twist.linear.y,2));
|
||||
// input.pn = vehicle_state_.position[0]; /** position north */
|
||||
// input.pe = vehicle_state_.position[1]; /** position east */
|
||||
// input.h = -vehicle_state_.position[2]; /** altitude */
|
||||
// input.chi = vehicle_state_.chi;
|
||||
// input.va = vehicle_state_.Va;
|
||||
|
||||
//add by kobe
|
||||
input.lat=vehicle_state_.Wd;
|
||||
input.lon=vehicle_state_.Jd;
|
||||
|
||||
struct output_s output;
|
||||
|
||||
if (state_init_ == true)
|
||||
{
|
||||
manage(params_, input, output);
|
||||
}
|
||||
|
||||
enemy_suv_msgs::Current_Path current_path;
|
||||
|
||||
enemy_suv_msgs::Goal_Info goal_info;
|
||||
enemy_suv_msgs::Down_Data_New down_data;
|
||||
// modified by kobe
|
||||
if (output.flag == 1)
|
||||
current_path.path_type = current_path.LINE_PATH;
|
||||
else if (output.flag == 0)
|
||||
current_path.path_type = current_path.ORBIT_PATH;
|
||||
else if (output.flag == 2)
|
||||
current_path.path_type = current_path.STAR_PATH;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
current_path.r[i] = output.r[i];
|
||||
current_path.q[i] = output.q[i];
|
||||
current_path.c[i] = output.c[i];
|
||||
}
|
||||
current_path.rho = output.rho;
|
||||
current_path.lambda = output.lambda;
|
||||
current_path.h_c = output.h_c;
|
||||
current_path.Va_d = output.Va_c; //new speed
|
||||
current_path.takeoff=output.takeoff;
|
||||
current_path.landing=output.landing;
|
||||
current_path_pub_.publish(current_path);
|
||||
|
||||
goal_info.altitude=-output.h_c;
|
||||
goal_info.v_d=output.Va_d;
|
||||
goal_info.action=1;//desired action move
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
goal_info.xy[i] = output.gxy[i];
|
||||
goal_info.ll[i] = output.gll[i];
|
||||
}
|
||||
goal_info_pub_.publish(goal_info); // add by kobe
|
||||
|
||||
down_data.latitude = output.gll[0];
|
||||
down_data.longitude = output.gll[1];
|
||||
down_data.altitude = -output.h_c;
|
||||
down_data.plane_speed= output.Va_d;
|
||||
down_data.status_word= 1;
|
||||
//ROS_INFO("Look here %f %f %f %f",down_data.latitude,down_data.longitude,down_data.altitude,output.Va_d);
|
||||
down_data_pub_.publish(down_data); // add by kobe
|
||||
}
|
||||
|
||||
//add by kobe
|
||||
float path_manager_base::Quaternion_to_Euler(float q0, float q1, float q2, float q3)
|
||||
{
|
||||
float Radx;
|
||||
float sum;
|
||||
sum = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
if (sum == 0)
|
||||
{
|
||||
sum = 0.0001;
|
||||
}
|
||||
q0 = q0 / sum;
|
||||
q1 = q1 / sum;
|
||||
q2 = q2 / sum;
|
||||
q3 = q3 / sum;
|
||||
if (fabs(q2) < fabs(q3))
|
||||
{
|
||||
Radx = asin(2.0f * (q2 * q3 + q0 * q1));
|
||||
return Radx;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (q2 * q3 > 0)
|
||||
{
|
||||
Radx = 3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
|
||||
return Radx;
|
||||
}
|
||||
else
|
||||
{
|
||||
Radx = -3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
|
||||
return Radx;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace rosplane
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_path_manager");
|
||||
rosplane::path_manager_base *est = new rosplane::path_manager_example();
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,717 @@
|
|||
#include "path_manager_example.h"
|
||||
#include "ros/ros.h"
|
||||
#include <cmath>
|
||||
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
path_manager_example::path_manager_example() : path_manager_base()
|
||||
{
|
||||
fil_state_ = fillet_state::STRAIGHT;
|
||||
// use this !
|
||||
dub_state_ = dubin_state::FIRST;
|
||||
}
|
||||
// done
|
||||
void path_manager_example::manage(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
|
||||
//ROS_INFO("num_waypoints_: %d",num_waypoints_);
|
||||
if (num_waypoints_ < 2)
|
||||
{
|
||||
ROS_WARN_THROTTLE(4, "No waypoints received! Loitering about origin at 50m");
|
||||
output.flag = 0; //modified by kobe
|
||||
output.Va_d = 12;
|
||||
output.c[0] = 0.0f;
|
||||
output.c[1] = 0.0f;
|
||||
output.c[2] = -50.0f;
|
||||
// Radius of orbital path (m)
|
||||
output.rho = params.R_min;
|
||||
// Direction of orbital path (clockwise is 1, counterclockwise is -1)
|
||||
output.lambda = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// do this, idx_a_ is initialized zero
|
||||
if (waypoints_[idx_a_].chi_valid)
|
||||
{
|
||||
manage_dubins(params, input, output);
|
||||
}
|
||||
else
|
||||
{
|
||||
/** Switch the following for flying directly to waypoints, or filleting corners */
|
||||
// manage_line(params, input, output);
|
||||
//manage_fillet(params, input, output);
|
||||
manage_star(params, input, output); //add by kobe
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// add by kobe
|
||||
void path_manager_example::manage_star(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
Eigen::Vector3f w_start;
|
||||
w_start << input.pn, input.pe, -input.h; //aircraft's position
|
||||
|
||||
int idx_b;
|
||||
int idx_c;
|
||||
float the_distance_;
|
||||
double the_distance_titude; //for latitude and longtitude
|
||||
idx_b = idx_a_ + 1;
|
||||
|
||||
if (idx_a_ < num_waypoints_ - 2)
|
||||
{
|
||||
idx_c = idx_b + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
idx_c = idx_b;
|
||||
}
|
||||
Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
|
||||
|
||||
Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
|
||||
Eigen::Vector3f w_i(waypoints_[idx_b].w); //the goal
|
||||
|
||||
the_distance_ = sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2));
|
||||
the_distance_titude = earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon);
|
||||
|
||||
output.flag = 1; //modified by kobe
|
||||
output.Va_d = waypoints_[idx_b].Va_d; //modified by kobe
|
||||
|
||||
//ROS_INFO("position: %g %g %g",w_start(0),w_start(1),w_start(2));
|
||||
//ROS_INFO("Current info are: %f %f %f %f", input.va, -input.h,w_start(0),w_start(1));
|
||||
|
||||
output.r[0] = w_start(0);
|
||||
output.r[1] = w_start(1);
|
||||
output.r[2] = w_start(2);
|
||||
Eigen::Vector3f q_im1 = (w_i - w_start).normalized();
|
||||
output.q[0] = q_im1(0);
|
||||
output.q[1] = q_im1(1);
|
||||
output.q[2] = q_im1(2);
|
||||
|
||||
output.gxy[0] = w_i(0);
|
||||
output.gxy[1] = w_i(1);
|
||||
output.gll[0] = waypoints_[idx_b].lat;
|
||||
output.gll[1] = waypoints_[idx_b].lon;
|
||||
output.h_c = w_i(2); //altitude of the goal
|
||||
|
||||
if (idx_a_ == 0&&show2 == 1)
|
||||
{
|
||||
ROS_INFO("First goal info are: %f %f", w_i(0), w_i(1));
|
||||
ROS_INFO("Desired fly speed and height are: %f %f", output.Va_d, -w_i(2));
|
||||
show2=0;
|
||||
}
|
||||
//enter the slow-down area,recompute the speed
|
||||
if (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < output.Va_d * 2 && idx_a_ == num_waypoints_ - 2) //enter the slowdown-ball and the goal is the last goal
|
||||
{
|
||||
output.Va_c = sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2))/2;
|
||||
if (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 0.5 )
|
||||
{
|
||||
output.Va_c = 0;
|
||||
}
|
||||
// ROS_INFO("Now the speed is: %f",output.Va_c);
|
||||
}
|
||||
else
|
||||
{
|
||||
output.Va_c = output.Va_d;
|
||||
}
|
||||
output.landing=waypoints_[idx_b].landing;
|
||||
output.takeoff=waypoints_[idx_b].takeoff;
|
||||
/* //1.enter a tiny ball and switch goal
|
||||
if (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 5) //enter a tiny ball and switch goal
|
||||
{
|
||||
//ROS_INFO("00000o- %g %g %g", w_start(2), w_i(2), sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)));
|
||||
//ROS_INFO("last goal is: %f %f", w_i(0), w_i(1));
|
||||
//ROS_INFO("Va- %f/n", input.va);
|
||||
if (idx_a_ == num_waypoints_ - 2)
|
||||
{
|
||||
idx_a_ = idx_a_; //do not update goal
|
||||
//ROS_INFO("idx_a_- %f/n", idx_a_);
|
||||
}
|
||||
else
|
||||
{
|
||||
idx_a_++;
|
||||
}
|
||||
}
|
||||
*/
|
||||
/*
|
||||
//2.enter a half-plane and switch,ahead 5 m from the goal point
|
||||
Eigen::Vector3f w_i_plane;
|
||||
w_i_plane=w_i;
|
||||
w_i_plane(0)=w_i_plane(0)-5*( ( w_i(0)-w_im1(0) ) / ( 0.001+fabs(w_i(0)-w_im1(0)) ) );
|
||||
w_i_plane(1)=w_i_plane(1)-5*( ( w_i(1)-w_im1(1) ) / ( 0.001+fabs(w_i(1)-w_im1(1)) ) );
|
||||
if ((w_i_plane - w_start).normalized().dot((w_i_plane - w_im1).normalized())<0)//plane-method
|
||||
{
|
||||
if (idx_a_ == num_waypoints_ - 2)
|
||||
idx_a_ = idx_a_;//do not update goal
|
||||
else
|
||||
idx_a_++;
|
||||
}
|
||||
*/
|
||||
//3.Enter a large ball(large-enter-ball) first. Anyway, when the distance from the aircraft to goal become larger,switch to the next goal.
|
||||
if (the_distance_ < min_distance_)
|
||||
{
|
||||
min_distance_ = the_distance_;
|
||||
}
|
||||
if (the_distance_titude < min_distance_titude)
|
||||
{
|
||||
min_distance_titude = the_distance_titude;
|
||||
}
|
||||
|
||||
// ROS_INFO("distance is : %.20f %f", earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon), sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)));
|
||||
|
||||
//ROS_INFO("000distance- %f %f", the_distance_, min_distance_);
|
||||
//1.进入大圈且最小距离开始增大时;2.进入一个小圈时<E59C88>?分别针对北东坐标和经纬度坐标
|
||||
judge_condition1 = (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 3 && the_distance_ > min_distance_ + 1);
|
||||
judge_condition2 = (earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon) < 3 && the_distance_titude > min_distance_titude + 1);
|
||||
judge_condition3 = (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 0.5);
|
||||
judge_condition4 = (earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon) < 0.5);
|
||||
// height judge
|
||||
judge_condition5 = (sqrt(pow(w_start(2) - w_i(2), 2)) < 10);
|
||||
|
||||
//notice here
|
||||
judge_condition2=0;
|
||||
judge_condition4=0;
|
||||
|
||||
if ((judge_condition1 || judge_condition2 || judge_condition3 || judge_condition4) && judge_condition5) //switch
|
||||
{
|
||||
//ROS_INFO("distance- %f %f", the_distance_, min_distance_);
|
||||
if (idx_a_ == num_waypoints_ - 2)
|
||||
{
|
||||
if (show == 1)
|
||||
{
|
||||
ROS_INFO("This goal is reached: %f %f", w_i(0), w_i(1));
|
||||
ROS_INFO("Current position are: %f %f", input.pn, input.pe);
|
||||
ROS_INFO("No more goal, just hover and wait for the next goal.");
|
||||
ROS_INFO("Desired speed and height are: %f %f", output.Va_d, -w_i(2));
|
||||
ROS_INFO("Current speed and height are: %f %f\n\n", input.va, input.h);
|
||||
show = 0;
|
||||
}
|
||||
idx_a_ = idx_a_; //do not update goal
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_INFO("This goal is reached: %f %f", w_i(0), w_i(1));
|
||||
ROS_INFO("Current position are: %f %f", input.pn, input.pe);
|
||||
ROS_INFO("Desired speed and height are: %f %f", output.Va_d, -w_i(2));
|
||||
ROS_INFO("Current speed and height are: %f %f", input.va, input.h);
|
||||
ROS_INFO("New goal info are: %f %f\n\n", w_ip1(0), w_ip1(1));
|
||||
min_distance_ = sqrt(pow(w_i(0) - w_ip1(0), 2) + pow(w_i(1) - w_ip1(1), 2));
|
||||
idx_a_++;
|
||||
}
|
||||
}
|
||||
//
|
||||
}
|
||||
|
||||
double path_manager_example::earth_distance(double latitude1, double longitude1, double latitude2, double longitude2)
|
||||
{
|
||||
double Lat1 = latitude1 * M_PI_F / 180.00; // 纬度
|
||||
double Lat2 = latitude2 * M_PI_F / 180.00;
|
||||
double a = Lat1 - Lat2; //两点纬度之差
|
||||
double b = longitude1 * M_PI_F / 180.00 - longitude2 * M_PI_F / 180.00; //经度之差
|
||||
double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(Lat1) * cos(Lat2) * pow(sin(b / 2), 2))); //计算两点距离的公<E79A84>?/better when value is very samll
|
||||
//double s2 = acos(cos(Lat1)*cos(Lat2)*cos(b)+sin(Lat1)*sin(Lat2));//计算两点距离的公<E79A84>?
|
||||
s = s * EARTH_RADIUS; //弧长乘地球半径（半径为米<E4B8BA>?
|
||||
//s2 = s2 * EARTH_RADIUS;//弧长乘地球半径（半径为米<E4B8BA>?
|
||||
return s;
|
||||
}
|
||||
|
||||
void path_manager_example::manage_line(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
Eigen::Vector3f p;
|
||||
p << input.pn, input.pe, -input.h;
|
||||
|
||||
int idx_b;
|
||||
int idx_c;
|
||||
if (idx_a_ == num_waypoints_ - 1)
|
||||
{
|
||||
idx_b = 0;
|
||||
idx_c = 1;
|
||||
}
|
||||
else if (idx_a_ == num_waypoints_ - 2)
|
||||
{
|
||||
idx_b = num_waypoints_ - 1;
|
||||
idx_c = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
idx_b = idx_a_ + 1;
|
||||
idx_c = idx_b + 1;
|
||||
}
|
||||
|
||||
Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
|
||||
Eigen::Vector3f w_i(waypoints_[idx_b].w);
|
||||
Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
|
||||
|
||||
output.flag = true;
|
||||
output.Va_d = waypoints_[idx_a_].Va_d;
|
||||
output.r[0] = w_im1(0);
|
||||
output.r[1] = w_im1(1);
|
||||
output.r[2] = w_im1(2);
|
||||
Eigen::Vector3f q_im1 = (w_i - w_im1).normalized();
|
||||
Eigen::Vector3f q_i = (w_ip1 - w_i).normalized();
|
||||
output.q[0] = q_im1(0);
|
||||
output.q[1] = q_im1(1);
|
||||
output.q[2] = q_im1(2);
|
||||
|
||||
Eigen::Vector3f n_i = (q_im1 + q_i).normalized();
|
||||
if ((p - w_i).dot(n_i) > 0.0f)
|
||||
{
|
||||
if (idx_a_ == num_waypoints_ - 1)
|
||||
idx_a_ = 0;
|
||||
else
|
||||
idx_a_++;
|
||||
}
|
||||
}
|
||||
void path_manager_example::manage_fillet(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
if (num_waypoints_ < 3) //since it fillets don't make sense between just two points
|
||||
{
|
||||
manage_line(params, input, output);
|
||||
return;
|
||||
}
|
||||
|
||||
Eigen::Vector3f p;
|
||||
p << input.pn, input.pe, -input.h;
|
||||
|
||||
int idx_b;
|
||||
int idx_c;
|
||||
if (idx_a_ == num_waypoints_ - 1)
|
||||
{
|
||||
idx_b = 0;
|
||||
idx_c = 1;
|
||||
}
|
||||
else if (idx_a_ == num_waypoints_ - 2)
|
||||
{
|
||||
idx_b = num_waypoints_ - 1;
|
||||
idx_c = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
idx_b = idx_a_ + 1;
|
||||
idx_c = idx_b + 1;
|
||||
}
|
||||
|
||||
Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
|
||||
Eigen::Vector3f w_i(waypoints_[idx_b].w);
|
||||
Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
|
||||
|
||||
float R_min = params.R_min;
|
||||
|
||||
output.Va_d = waypoints_[idx_a_].Va_d;
|
||||
output.r[0] = w_im1(0);
|
||||
output.r[1] = w_im1(1);
|
||||
output.r[2] = w_im1(2);
|
||||
Eigen::Vector3f q_im1 = (w_i - w_im1).normalized();
|
||||
Eigen::Vector3f q_i = (w_ip1 - w_i).normalized();
|
||||
float beta = acosf(-q_im1.dot(q_i));
|
||||
|
||||
Eigen::Vector3f z;
|
||||
switch (fil_state_)
|
||||
{
|
||||
case fillet_state::STRAIGHT:
|
||||
output.flag = 1; //modified by kobe
|
||||
output.q[0] = q_im1(0);
|
||||
output.q[1] = q_im1(1);
|
||||
output.q[2] = q_im1(2);
|
||||
output.c[0] = 1;
|
||||
output.c[1] = 1;
|
||||
output.c[2] = 1;
|
||||
output.rho = 1;
|
||||
output.lambda = 1;
|
||||
z = w_i - q_im1 * (R_min / tanf(beta / 2.0));
|
||||
if ((p - z).dot(q_im1) > 0)
|
||||
fil_state_ = fillet_state::ORBIT;
|
||||
break;
|
||||
case fillet_state::ORBIT:
|
||||
output.flag = 0; //modified by kobe
|
||||
output.q[0] = q_i(0);
|
||||
output.q[1] = q_i(1);
|
||||
output.q[2] = q_i(2);
|
||||
Eigen::Vector3f c = w_i - (q_im1 - q_i).normalized() * (R_min / sinf(beta / 2.0));
|
||||
output.c[0] = c(0);
|
||||
output.c[1] = c(1);
|
||||
output.c[2] = c(2);
|
||||
output.rho = R_min;
|
||||
output.lambda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 ? 1 : -1);
|
||||
z = w_i + q_i * (R_min / tanf(beta / 2.0));
|
||||
if ((p - z).dot(q_i) > 0)
|
||||
{
|
||||
if (idx_a_ == num_waypoints_ - 1)
|
||||
idx_a_ = 0;
|
||||
else
|
||||
idx_a_++;
|
||||
fil_state_ = fillet_state::STRAIGHT;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void path_manager_example::manage_dubins(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
Eigen::Vector3f p;
|
||||
//ROS_INFO("input.pn: %f, input.pe: %f, -input.h: %f",input.pn, input.pe, -input.h);
|
||||
p << input.pn, input.pe, -input.h;
|
||||
|
||||
/* uint8 path_type # Indicates strait line or orbital path
|
||||
float32 Va_d # Desired airspeed (m/s)
|
||||
float32[3] r # Vector to origin of straight line path (m)
|
||||
float32[3] q # Unit vector, desired direction of travel for line path
|
||||
float32[3] c # Center of orbital path (m)
|
||||
float32 rho # Radius of orbital path (m)
|
||||
int8 lambda # Direction of orbital path (clockwise is 1, counterclockwise is -1)
|
||||
|
||||
uint8 ORBIT_PATH = 0
|
||||
uint8 LINE_PATH = 1
|
||||
|
||||
int8 CLOCKWISE = 1
|
||||
int8 COUNT_CLOCKWISE = -1 */
|
||||
|
||||
output.Va_d = waypoints_[idx_a_].Va_d;
|
||||
output.r[0] = 0;
|
||||
output.r[1] = 0;
|
||||
output.r[2] = 0;
|
||||
output.q[0] = 0;
|
||||
output.q[1] = 0;
|
||||
output.q[2] = 0;
|
||||
output.c[0] = 0;
|
||||
output.c[1] = 0;
|
||||
output.c[2] = 0;
|
||||
//ROS_INFO("dub_state_: %d",dub_state_);
|
||||
switch (dub_state_)
|
||||
{
|
||||
case dubin_state::FIRST:
|
||||
// path planning
|
||||
// Algorithm11 P211
|
||||
dubinsParameters(waypoints_[0], waypoints_[1], params.R_min);
|
||||
|
||||
waypoint_start_ = waypoints_[0];
|
||||
waypoint_end_ = waypoints_[1];
|
||||
|
||||
output.flag = 0; //modified by kobe
|
||||
output.c[0] = dubinspath_.cs(0);
|
||||
output.c[1] = dubinspath_.cs(1);
|
||||
output.c[2] = dubinspath_.cs(2);
|
||||
output.rho = dubinspath_.R;
|
||||
output.lambda = dubinspath_.lams;
|
||||
// p: current pos
|
||||
// w1: waypoint 1
|
||||
// q1: unit vector of H1
|
||||
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
|
||||
{
|
||||
dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
|
||||
}
|
||||
else
|
||||
{
|
||||
dub_state_ = dubin_state::BEFORE_H1;
|
||||
}
|
||||
break;
|
||||
case dubin_state::BEFORE_H1:
|
||||
//waypoint_received_ = false;
|
||||
output.flag = 0; //modified by kobe
|
||||
output.c[0] = dubinspath_.cs(0);
|
||||
output.c[1] = dubinspath_.cs(1);
|
||||
output.c[2] = dubinspath_.cs(2);
|
||||
output.rho = dubinspath_.R;
|
||||
output.lambda = dubinspath_.lams;
|
||||
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // entering H1
|
||||
{
|
||||
dub_state_ = dubin_state::STRAIGHT;
|
||||
}
|
||||
break;
|
||||
case dubin_state::BEFORE_H1_WRONG_SIDE:
|
||||
//waypoint_received_ = false;
|
||||
output.flag = 0; //modified by kobe
|
||||
output.c[0] = dubinspath_.cs(0);
|
||||
output.c[1] = dubinspath_.cs(1);
|
||||
output.c[2] = dubinspath_.cs(2);
|
||||
output.rho = dubinspath_.R;
|
||||
output.lambda = dubinspath_.lams;
|
||||
if ((p - dubinspath_.w1).dot(dubinspath_.q1) < 0) // exit H1
|
||||
{
|
||||
dub_state_ = dubin_state::BEFORE_H1;
|
||||
}
|
||||
break;
|
||||
case dubin_state::STRAIGHT:
|
||||
output.flag = 1; //modified by kobe
|
||||
output.r[0] = dubinspath_.w1(0);
|
||||
output.r[1] = dubinspath_.w1(1);
|
||||
output.r[2] = dubinspath_.w1(2);
|
||||
// output.r[0] = dubinspath_.z1(0);
|
||||
// output.r[1] = dubinspath_.z1(1);
|
||||
// output.r[2] = dubinspath_.z1(2);
|
||||
output.q[0] = dubinspath_.q1(0);
|
||||
output.q[1] = dubinspath_.q1(1);
|
||||
output.q[2] = dubinspath_.q1(2);
|
||||
output.rho = 1;
|
||||
output.lambda = 1;
|
||||
//ROS_INFO("pos: [%f,%f,%f],w2: [%f,%f,%f],q1: [%f,%f,%f]",p[0],p[1],p[2],dubinspath_.w2[0],dubinspath_.w2[1],dubinspath_.w2[2],dubinspath_.q1[0],dubinspath_.q1[1],dubinspath_.q1[2]);
|
||||
//ROS_INFO("w3: [%f,%f,%f]",dubinspath_.w3[0],dubinspath_.w3[1],dubinspath_.w3[2]);
|
||||
//ROS_INFO("w1: [%f,%f,%f]",dubinspath_.w1[0],dubinspath_.w1[1],dubinspath_.w1[2]);
|
||||
|
||||
if (waypoint_received_)
|
||||
{
|
||||
//start new path
|
||||
// plan new Dubin's path to next waypoint configuration
|
||||
waypoint_start_.w[0] = p[0];
|
||||
waypoint_start_.w[1] = p[1];
|
||||
waypoint_start_.w[2] = p[2];
|
||||
waypoint_start_.chi_d = 0;
|
||||
waypoint_start_.chi_valid = true;
|
||||
waypoint_start_.Va_d = 12;
|
||||
dubinsParameters(waypoint_start_, waypoint_end_, params.R_min);
|
||||
waypoint_received_ = false;
|
||||
|
||||
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
|
||||
{
|
||||
dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
|
||||
}
|
||||
else
|
||||
{
|
||||
dub_state_ = dubin_state::BEFORE_H1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((p - dubinspath_.w2).dot(dubinspath_.q1) >= 0) // entering H2
|
||||
{
|
||||
if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // start in H3
|
||||
{
|
||||
dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE;
|
||||
}
|
||||
else
|
||||
{
|
||||
dub_state_ = dubin_state::BEFORE_H3;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case dubin_state::BEFORE_H3:
|
||||
output.flag = 0; //modified by kobe
|
||||
output.c[0] = dubinspath_.ce(0);
|
||||
output.c[1] = dubinspath_.ce(1);
|
||||
output.c[2] = dubinspath_.ce(2);
|
||||
output.rho = dubinspath_.R;
|
||||
output.lambda = dubinspath_.lame;
|
||||
if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // entering H3
|
||||
{
|
||||
if (!waypoint_received_)
|
||||
{
|
||||
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
|
||||
{
|
||||
//dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
|
||||
dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE;
|
||||
}
|
||||
else
|
||||
{
|
||||
//dub_state_ = dubin_state::BEFORE_H1;
|
||||
dub_state_ = dubin_state::BEFORE_H3;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//start new path
|
||||
// plan new Dubin's path to next waypoint configuration
|
||||
dubinsParameters(waypoint_start_, waypoint_end_, params.R_min);
|
||||
waypoint_received_ = false;
|
||||
|
||||
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
|
||||
{
|
||||
dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
|
||||
}
|
||||
else
|
||||
{
|
||||
dub_state_ = dubin_state::BEFORE_H1;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case dubin_state::BEFORE_H3_WRONG_SIDE:
|
||||
output.flag = 0; //modified by kobe
|
||||
output.c[0] = dubinspath_.ce(0);
|
||||
output.c[1] = dubinspath_.ce(1);
|
||||
output.c[2] = dubinspath_.ce(2);
|
||||
output.rho = dubinspath_.R;
|
||||
output.lambda = dubinspath_.lame;
|
||||
/*
|
||||
if ((p - dubinspath_.w3).dot(dubinspath_.q3) < 0) // exit H3
|
||||
{
|
||||
if(!waypoint_received_)
|
||||
//dub_state_ = dubin_state::BEFORE_H1;
|
||||
dub_state_ = dubin_state::BEFORE_H3;
|
||||
else
|
||||
dub_state_ = dubin_state::BEFORE_H1;
|
||||
}
|
||||
*/
|
||||
if ((p - dubinspath_.w3).dot(dubinspath_.q3) < 0) // exit H3
|
||||
dub_state_ = dubin_state::BEFORE_H3;
|
||||
else
|
||||
dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE;
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::Matrix3f path_manager_example::rotz(float theta)
|
||||
{
|
||||
Eigen::Matrix3f R;
|
||||
R << cosf(theta), -sinf(theta), 0,
|
||||
sinf(theta), cosf(theta), 0,
|
||||
0, 0, 1;
|
||||
|
||||
return R;
|
||||
}
|
||||
// done
|
||||
float path_manager_example::mo(float in)
|
||||
{
|
||||
float val;
|
||||
if (in > 0)
|
||||
val = fmod(in, 2.0 * M_PI_F);
|
||||
else
|
||||
{
|
||||
float n = floorf(in / 2.0 / M_PI_F);
|
||||
val = in - n * 2.0 * M_PI_F;
|
||||
}
|
||||
return val;
|
||||
}
|
||||
|
||||
void path_manager_example::dubinsParameters(const waypoint_s start_node, const waypoint_s end_node, float R)
|
||||
{
|
||||
float ell = sqrtf((start_node.w[0] - end_node.w[0]) * (start_node.w[0] - end_node.w[0]) +
|
||||
(start_node.w[1] - end_node.w[1]) * (start_node.w[1] - end_node.w[1]));
|
||||
if (ell < 2.0 * R)
|
||||
{
|
||||
ROS_ERROR("SHOULD NOT HAPPEN: The distance between nodes must be larger than 2R.");
|
||||
}
|
||||
else
|
||||
{
|
||||
dubinspath_.ps(0) = start_node.w[0];
|
||||
dubinspath_.ps(1) = start_node.w[1];
|
||||
dubinspath_.ps(2) = start_node.w[2];
|
||||
dubinspath_.chis = start_node.chi_d;
|
||||
dubinspath_.pe(0) = end_node.w[0];
|
||||
dubinspath_.pe(1) = end_node.w[1];
|
||||
dubinspath_.pe(2) = end_node.w[2];
|
||||
dubinspath_.chie = end_node.chi_d;
|
||||
|
||||
Eigen::Vector3f crs = dubinspath_.ps;
|
||||
crs(0) += R * (cosf(M_PI_2_F) * cosf(dubinspath_.chis) - sinf(M_PI_2_F) * sinf(dubinspath_.chis));
|
||||
crs(1) += R * (sinf(M_PI_2_F) * cosf(dubinspath_.chis) + cosf(M_PI_2_F) * sinf(dubinspath_.chis));
|
||||
Eigen::Vector3f cls = dubinspath_.ps;
|
||||
cls(0) += R * (cosf(-M_PI_2_F) * cosf(dubinspath_.chis) - sinf(-M_PI_2_F) * sinf(dubinspath_.chis));
|
||||
cls(1) += R * (sinf(-M_PI_2_F) * cosf(dubinspath_.chis) + cosf(-M_PI_2_F) * sinf(dubinspath_.chis));
|
||||
Eigen::Vector3f cre = dubinspath_.pe;
|
||||
cre(0) += R * (cosf(M_PI_2_F) * cosf(dubinspath_.chie) - sinf(M_PI_2_F) * sinf(dubinspath_.chie));
|
||||
cre(1) += R * (sinf(M_PI_2_F) * cosf(dubinspath_.chie) + cosf(M_PI_2_F) * sinf(dubinspath_.chie));
|
||||
Eigen::Vector3f cle = dubinspath_.pe;
|
||||
cle(0) += R * (cosf(-M_PI_2_F) * cosf(dubinspath_.chie) - sinf(-M_PI_2_F) * sinf(dubinspath_.chie));
|
||||
cle(1) += R * (sinf(-M_PI_2_F) * cosf(dubinspath_.chie) + cosf(-M_PI_2_F) * sinf(dubinspath_.chie));
|
||||
|
||||
float theta, theta2;
|
||||
// compute L1
|
||||
theta = atan2f(cre(1) - crs(1), cre(0) - crs(0));
|
||||
float L1 = (crs - cre).norm() + R * mo(2.0 * M_PI_F + mo(theta - M_PI_2_F) - mo(dubinspath_.chis - M_PI_2_F)) + R * mo(2.0 * M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta - M_PI_2_F));
|
||||
|
||||
// compute L2
|
||||
ell = (cle - crs).norm();
|
||||
theta = atan2f(cle(1) - crs(1), cle(0) - crs(0));
|
||||
float L2;
|
||||
if (2.0 * R > ell)
|
||||
L2 = 9999.0f;
|
||||
else
|
||||
{
|
||||
theta2 = theta - M_PI_2_F + asinf(2.0 * R / ell);
|
||||
L2 = sqrtf(ell * ell - 4.0 * R * R) + R * mo(2.0 * M_PI_F + mo(theta2) - mo(dubinspath_.chis - M_PI_2_F)) + R * mo(2.0 * M_PI_F + mo(theta2 + M_PI_F) - mo(dubinspath_.chie + M_PI_2_F));
|
||||
}
|
||||
|
||||
// compute L3
|
||||
ell = (cre - cls).norm();
|
||||
theta = atan2f(cre(1) - cls(1), cre(0) - cls(0));
|
||||
float L3;
|
||||
if (2.0 * R > ell)
|
||||
L3 = 9999.0f;
|
||||
else
|
||||
{
|
||||
theta2 = acosf(2.0 * R / ell);
|
||||
L3 = sqrtf(ell * ell - 4 * R * R) + R * mo(2.0 * M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + theta2)) + R * mo(2.0 * M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta + theta2 - M_PI_F));
|
||||
}
|
||||
|
||||
// compute L4
|
||||
theta = atan2f(cle(1) - cls(1), cle(0) - cls(0));
|
||||
float L4 = (cls - cle).norm() + R * mo(2.0 * M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + M_PI_2_F)) + R * mo(2.0 * M_PI_F + mo(theta + M_PI_2_F) - mo(dubinspath_.chie + M_PI_2_F));
|
||||
|
||||
// L is the minimum distance
|
||||
int idx = 1;
|
||||
dubinspath_.L = L1;
|
||||
if (L2 < dubinspath_.L)
|
||||
{
|
||||
dubinspath_.L = L2;
|
||||
idx = 2;
|
||||
}
|
||||
if (L3 < dubinspath_.L)
|
||||
{
|
||||
dubinspath_.L = L3;
|
||||
idx = 3;
|
||||
}
|
||||
if (L4 < dubinspath_.L)
|
||||
{
|
||||
dubinspath_.L = L4;
|
||||
idx = 4;
|
||||
}
|
||||
|
||||
Eigen::Vector3f e1;
|
||||
// e1.zero();
|
||||
e1(0) = 1;
|
||||
e1(1) = 0;
|
||||
e1(2) = 0;
|
||||
switch (idx)
|
||||
{
|
||||
case 1:
|
||||
dubinspath_.cs = crs;
|
||||
dubinspath_.lams = 1;
|
||||
dubinspath_.ce = cre;
|
||||
dubinspath_.lame = 1;
|
||||
dubinspath_.q1 = (cre - crs).normalized();
|
||||
dubinspath_.w1 = dubinspath_.cs + (rotz(-M_PI_2_F) * dubinspath_.q1) * R;
|
||||
dubinspath_.w2 = dubinspath_.ce + (rotz(-M_PI_2_F) * dubinspath_.q1) * R;
|
||||
break;
|
||||
case 2:
|
||||
dubinspath_.cs = crs;
|
||||
dubinspath_.lams = 1;
|
||||
dubinspath_.ce = cle;
|
||||
dubinspath_.lame = -1;
|
||||
ell = (cle - crs).norm();
|
||||
theta = atan2f(cle(1) - crs(1), cle(0) - crs(0));
|
||||
theta2 = theta - M_PI_2_F + asinf(2.0 * R / ell);
|
||||
dubinspath_.q1 = rotz(theta2 + M_PI_2_F) * e1;
|
||||
dubinspath_.w1 = dubinspath_.cs + (rotz(theta2) * e1) * R;
|
||||
dubinspath_.w2 = dubinspath_.ce + (rotz(theta2 + M_PI_F) * e1) * R;
|
||||
break;
|
||||
case 3:
|
||||
dubinspath_.cs = cls;
|
||||
dubinspath_.lams = -1;
|
||||
dubinspath_.ce = cre;
|
||||
dubinspath_.lame = 1;
|
||||
ell = (cre - cls).norm();
|
||||
theta = atan2f(cre(1) - cls(1), cre(0) - cls(0));
|
||||
theta2 = acosf(2.0 * R / ell);
|
||||
dubinspath_.q1 = rotz(theta + theta2 - M_PI_2_F) * e1;
|
||||
dubinspath_.w1 = dubinspath_.cs + (rotz(theta + theta2) * e1) * R;
|
||||
dubinspath_.w2 = dubinspath_.ce + (rotz(theta + theta2 - M_PI_F) * e1) * R;
|
||||
break;
|
||||
case 4:
|
||||
dubinspath_.cs = cls;
|
||||
dubinspath_.lams = -1;
|
||||
dubinspath_.ce = cle;
|
||||
dubinspath_.lame = -1;
|
||||
dubinspath_.q1 = (cle - cls).normalized();
|
||||
dubinspath_.w1 = dubinspath_.cs + (rotz(M_PI_2_F) * dubinspath_.q1) * R;
|
||||
dubinspath_.w2 = dubinspath_.ce + (rotz(M_PI_2_F) * dubinspath_.q1) * R;
|
||||
break;
|
||||
}
|
||||
dubinspath_.w3 = dubinspath_.pe;
|
||||
dubinspath_.q3 = rotz(dubinspath_.chie) * e1;
|
||||
dubinspath_.R = R;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace rosplane
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
#include <ros/ros.h>
|
||||
#include <enemy_suv_msgs/Waypoint.h>
|
||||
#define EARTH_RADIUS 6378145.0f
|
||||
#define num_waypoints 3
|
||||
float pn2latitude(float pn,float init_lat_)
|
||||
{
|
||||
return (pn*180)/(EARTH_RADIUS*M_PI)+init_lat_;
|
||||
}
|
||||
|
||||
float pe2longtitude(float pe,float init_lat_,float init_lon_)
|
||||
{
|
||||
return (pe*180)/(EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*M_PI)+init_lon_;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_simple_path_planner");
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
ros::Publisher waypointPublisher = nh_.advertise<enemy_suv_msgs::Waypoint>("waypoint_path", 10);
|
||||
|
||||
float Va = 3;
|
||||
int tmp;
|
||||
tmp = 7; //notice here
|
||||
float wps[tmp * num_waypoints] =
|
||||
{
|
||||
// pn,pe,altitude,-,va,lat,lon
|
||||
200, 100, 0, 0, 10, pn2latitude(200,0), pe2longtitude(100,0,0),
|
||||
300, 200, 0, 0, 10, pn2latitude(300,0), pe2longtitude(200,0,0),
|
||||
0, 200, 0, 0, 10, pn2latitude(0,0), pe2longtitude(200,0,0),
|
||||
};
|
||||
|
||||
for (int i(0); i < num_waypoints; i++)
|
||||
{
|
||||
ros::Duration(0.5).sleep();
|
||||
|
||||
enemy_suv_msgs::Waypoint new_waypoint;
|
||||
|
||||
new_waypoint.w[0] = wps[i * tmp + 0];
|
||||
new_waypoint.w[1] = wps[i * tmp + 1];
|
||||
new_waypoint.w[2] = wps[i * tmp + 2];
|
||||
new_waypoint.chi_d = wps[i * tmp + 3];
|
||||
new_waypoint.Va_d = wps[i * tmp + 4];
|
||||
// add by kobe
|
||||
new_waypoint.lat = wps[i * tmp + 5];
|
||||
new_waypoint.lon = wps[i * tmp + 6];
|
||||
|
||||
new_waypoint.chi_valid = false; //kobe
|
||||
|
||||
if (i == 0)
|
||||
new_waypoint.set_current = true;
|
||||
else
|
||||
new_waypoint.set_current = false;
|
||||
|
||||
new_waypoint.clear_wp_list = false;
|
||||
|
||||
waypointPublisher.publish(new_waypoint);
|
||||
}
|
||||
ros::Duration(1.5).sleep();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,214 @@
|
|||
#include "pseudo_controller_base.h"
|
||||
#include "pseudo_controller_example.h"
|
||||
// modified by kobe
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
pseudo_controller_base::pseudo_controller_base():
|
||||
nh_(ros::NodeHandle()),
|
||||
nh_private_(ros::NodeHandle())
|
||||
{
|
||||
//ROS_INFO("hello");
|
||||
vehicle_state_sub_ = nh_.subscribe("state", 10, &pseudo_controller_base::vehicle_state_callback, this);
|
||||
bebop1_state_sub_ = nh_.subscribe("ground_truth/state", 10, &pseudo_controller_base::bebop1_state_callback, this);
|
||||
//bebop1_state_sub_ = nh_.subscribe("/test/odom", 10, &pseudo_controller_base::bebop1_state_callback, this);
|
||||
|
||||
|
||||
|
||||
controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &pseudo_controller_base::controller_commands_callback,this);
|
||||
|
||||
memset(&vehicle_state_, 0, sizeof(vehicle_state_));
|
||||
memset(&bebop1_state_, 0, sizeof(bebop1_state_));
|
||||
|
||||
memset(&controller_commands_, 0, sizeof(controller_commands_));
|
||||
|
||||
nh_private_.param<double>("TRIM_T", params_.trim_t, 0.3);
|
||||
|
||||
nh_private_.param<double>("ALT_TOZ", params_.alt_toz, 1.5);
|
||||
nh_private_.param<double>("ALT_HZ", params_.alt_hz, 0.2);
|
||||
nh_private_.param<double>("TAU", params_.tau, 5.0);
|
||||
nh_private_.param<double>("COURSE_KP", params_.c_kp, 0.7329);
|
||||
//nh_private_.param<double>("COURSE_KD", params_.c_kd, 0.0);
|
||||
nh_private_.param<double>("COURSE_KI", params_.c_ki, 0.0);
|
||||
|
||||
nh_private_.param<double>("AS_PITCH_KI", params_.a_p_ki, 0.0);
|
||||
nh_private_.param<double>("AS_THR_KP", params_.a_t_kp, 0.9);
|
||||
nh_private_.param<double>("AS_THR_KD", params_.a_t_kd, 0.0);
|
||||
nh_private_.param<double>("AS_THR_KI", params_.a_t_ki, 0.0);
|
||||
nh_private_.param<double>("ALT_KP", params_.a_kp, 0.045);
|
||||
nh_private_.param<double>("ALT_KD", params_.a_kd, 0.0);
|
||||
nh_private_.param<double>("ALT_KI", params_.a_ki, 0.0);
|
||||
|
||||
//nh_private_.param<double>("MAX_A", params_.max_a, 0.523);//rotate_trans
|
||||
//nh_private_.param<double>("MAX_T", params_.max_t, 1.0);
|
||||
nh_private_.param<double>("MAX_T", params_.max_t, 200.0);
|
||||
|
||||
func_ = boost::bind(&pseudo_controller_base::reconfigure_callback, this, _1, _2);
|
||||
server_.setCallback(func_);
|
||||
|
||||
//pesudors_pub_ = nh_.advertise<rosflight_msgs::Command>("command", 10);
|
||||
pesudors_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
|
||||
internals_pub_ = nh_.advertise<enemy_suv_msgs::Controller_Internals>("controller_inners", 10);
|
||||
act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &pseudo_controller_base::actuator_controls_publish, this);
|
||||
|
||||
command_recieved_ = false;
|
||||
}
|
||||
|
||||
void pseudo_controller_base::vehicle_state_callback(const enemy_suv_msgs::StateConstPtr &msg)
|
||||
{
|
||||
vehicle_state_ = *msg;
|
||||
}
|
||||
|
||||
void pseudo_controller_base::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
|
||||
{
|
||||
bebop1_state_ = *msg;
|
||||
}
|
||||
|
||||
void pseudo_controller_base::controller_commands_callback(const enemy_suv_msgs::Controller_CommandsConstPtr &msg)
|
||||
{
|
||||
command_recieved_ = true;
|
||||
controller_commands_ = *msg;
|
||||
}
|
||||
|
||||
void pseudo_controller_base::reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level)
|
||||
{
|
||||
params_.trim_t = config.TRIM_T;
|
||||
|
||||
params_.c_kp = config.COURSE_KP;
|
||||
//params_.c_kd = config.COURSE_KD;
|
||||
params_.c_ki = config.COURSE_KI;
|
||||
|
||||
params_.a_p_ki = config.AS_PITCH_KI;
|
||||
|
||||
params_.a_t_kp = config.AS_THR_KP;
|
||||
params_.a_t_kd = config.AS_THR_KD;
|
||||
params_.a_t_ki = config.AS_THR_KI;
|
||||
|
||||
params_.a_kp = config.ALT_KP;
|
||||
params_.a_kd = config.ALT_KD;
|
||||
params_.a_ki = config.ALT_KI;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void pseudo_controller_base::actuator_controls_publish(const ros::TimerEvent &)
|
||||
{
|
||||
struct input_s input;
|
||||
//input.h = -vehicle_state_.position[2];
|
||||
//input.va = vehicle_state_.Va;
|
||||
//input.chi = vehicle_state_.chi;
|
||||
input.h = bebop1_state_.pose.pose.position.z; /** altitude */
|
||||
input.va = sqrt(pow(bebop1_state_.twist.twist.linear.x,2)+pow(bebop1_state_.twist.twist.linear.y,2));
|
||||
input.chi =pseudo_controller_base::Quaternion_to_Euler(bebop1_state_.pose.pose.orientation.x,bebop1_state_.pose.pose.orientation.y,bebop1_state_.pose.pose.orientation.z,bebop1_state_.pose.pose.orientation.w);
|
||||
// ROS_INFO("LOOK chi: %f",input.chi);
|
||||
input.Va_c = controller_commands_.Va_c;
|
||||
// ROS_INFO("LOOK Va_c: %f",input.Va_c);
|
||||
input.h_c = controller_commands_.h_c;//3>controller_commands_.h_c)?3:controller_commands_.h_c;
|
||||
input.chi_c = controller_commands_.chi_c;
|
||||
// ROS_INFO("LOOK chi_c: %f",input.chi_c);
|
||||
input.takeoff = controller_commands_.takeoff;
|
||||
input.landing = controller_commands_.landing;
|
||||
input.Ts = 0.01f;
|
||||
if (controller_commands_.Va_c==0)
|
||||
{
|
||||
reached=1;
|
||||
}
|
||||
struct output_s output;
|
||||
|
||||
if (command_recieved_ == true)
|
||||
{
|
||||
control(params_, input, output);
|
||||
|
||||
//convert_to_pwm(output);
|
||||
geometry_msgs::Twist cmd_vel;
|
||||
//rosflight_msgs::Command pseudors;
|
||||
/* publish pseudors controls */
|
||||
cmd_vel.linear.x = output.forward_trans;//head vel
|
||||
cmd_vel.linear.y = output.left_right_trans;
|
||||
// cmd_vel.linear.z = output.up_down_trans;
|
||||
cmd_vel.angular.x = 0;
|
||||
cmd_vel.angular.y = 0;//output.roll_trans;
|
||||
if (input.va > 0.1)
|
||||
cmd_vel.angular.z = output.rotate_trans;//output.rotate_trans
|
||||
else
|
||||
cmd_vel.angular.z = 0;
|
||||
|
||||
pesudors_pub_.publish(cmd_vel);
|
||||
//ROS_INFO("hello %f",bebop1_state_.pose.pose.position.z);
|
||||
// ROS_INFO("x-y-z-w: %f %f %f %f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.linear.z,cmd_vel.angular.z);
|
||||
// ROS_INFO("six: %f %f %f %f %f %f\n",input.h,input.h_c,input.va,input.Va_c,input.chi,input.chi_c);
|
||||
// ROS_INFO("position: %f %f %f",bebop1_state_.pose.pose.position.x,bebop1_state_.pose.pose.position.y,bebop1_state_.pose.pose.position.z);
|
||||
if (internals_pub_.getNumSubscribers() > 0)
|
||||
{
|
||||
enemy_suv_msgs::Controller_Internals inners;
|
||||
//sinners.phi_c = output.phi_c;
|
||||
//inners.theta_c = output.theta_c;
|
||||
switch (output.current_zone)
|
||||
{
|
||||
case alt_zones::TAKE_OFF:
|
||||
inners.alt_zone = inners.ZONE_TAKE_OFF;
|
||||
break;
|
||||
case alt_zones::CLIMB:
|
||||
inners.alt_zone = inners.ZONE_CLIMB;
|
||||
break;
|
||||
case alt_zones::DESCEND:
|
||||
inners.alt_zone = inners.ZONE_DESEND;
|
||||
break;
|
||||
case alt_zones::ALTITUDE_HOLD:
|
||||
inners.alt_zone = inners.ZONE_ALTITUDE_HOLD;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
inners.aux_valid = false;
|
||||
internals_pub_.publish(inners);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float pseudo_controller_base::Quaternion_to_Euler(float q0,float q1,float q2,float q3)
|
||||
{
|
||||
float Radx;
|
||||
float sum;
|
||||
sum = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
|
||||
if (sum==0)
|
||||
{
|
||||
sum=0.0001;
|
||||
}
|
||||
q0 = q0 / sum;
|
||||
q1 = q1 / sum;
|
||||
q2 = q2 / sum;
|
||||
q3 = q3 / sum;
|
||||
|
||||
// ROS_INFO("LOOK here2: %f",asin(2.0f*(q2*q3 + q0*q1)));
|
||||
if (fabs(q2)<fabs(q3))
|
||||
{
|
||||
Radx = asin(2.0f*(q2*q3 + q0*q1));
|
||||
return Radx;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(q2*q3>0)
|
||||
{
|
||||
Radx = 3.14159-asin(2.0f*(q2*q3 + q0*q1));
|
||||
return Radx;
|
||||
}
|
||||
else
|
||||
{
|
||||
Radx = -3.14159-asin(2.0f*(q2*q3 + q0*q1));
|
||||
return Radx;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} //end namespace
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "rosplane_controller");
|
||||
rosplane::pseudo_controller_base *cont = new rosplane::pseudo_controller_example();
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,326 @@
|
|||
#include "pseudo_controller_example.h"
|
||||
//modified by kobe
|
||||
namespace rosplane
|
||||
{
|
||||
|
||||
pseudo_controller_example::pseudo_controller_example() : pseudo_controller_base()
|
||||
{
|
||||
current_zone = alt_zones::PREPARE;
|
||||
//current_zone = alt_zones::TAKE_OFF;
|
||||
c_error_ = 0;
|
||||
c_integrator_ = 0;
|
||||
r_error_ = 0;
|
||||
r_integrator_ = 0;
|
||||
a_error_ = 0;
|
||||
a_integrator_ = 0;
|
||||
at_error_ = 0;
|
||||
at_integrator_ = 0;
|
||||
ap_error_ = 0;
|
||||
ap_integrator_ = 0;
|
||||
}
|
||||
|
||||
void pseudo_controller_example::control(const params_s ¶ms, const input_s &input, output_s &output)
|
||||
{
|
||||
output.up_down_trans = 0;
|
||||
output.forward_trans = input.Va_c; // ZS
|
||||
output.rotate_trans = 0;
|
||||
output.left_right_trans = 0;
|
||||
|
||||
output.rotate_value=pseudo_course(input.chi, input.chi_c);
|
||||
output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
|
||||
|
||||
|
||||
// if (input.landing==true) // protect-landing first
|
||||
// {
|
||||
// current_zone = alt_zones::LANDING;
|
||||
// }
|
||||
// if (input.takeoff==true && current_zone==alt_zones::LANDING)
|
||||
// {
|
||||
// current_zone = alt_zones::PREPARE;
|
||||
// }
|
||||
|
||||
// switch (current_zone)
|
||||
// {
|
||||
// case alt_zones::PREPARE:
|
||||
// output.up_down_trans = 0.5;
|
||||
// output.forward_trans = 0;
|
||||
// output.left_right_trans = 0;
|
||||
// output.rotate_trans=0;
|
||||
// if (input.h >= 1) //
|
||||
// {
|
||||
// ROS_DEBUG("TAKEOFF");
|
||||
// current_zone = alt_zones::TAKE_OFF;
|
||||
// }
|
||||
// break;
|
||||
// case alt_zones::TAKE_OFF:
|
||||
// //output.rotate_trans = 1.5*pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);//recover
|
||||
// //output.forward_trans = 0.7;//0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
// output.forward_trans = input.Va_c;//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
// output.up_down_trans = pow(1.1,input.h)*pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
|
||||
// //output.up_down_trans = 0.01 * pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
|
||||
// //if (input.h >= params.alt_toz) //alt_toz=1.5//recovery
|
||||
// //output.left_right_trans = 1.5*pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
|
||||
// if (input.h >= 1.5) //
|
||||
// {
|
||||
// ROS_DEBUG("climb");
|
||||
// current_zone = alt_zones::CLIMB;
|
||||
// ap_error_ = 0;
|
||||
// ap_integrator_ = 0;
|
||||
// ap_differentiator_ = 0;
|
||||
// }
|
||||
// break;
|
||||
// case alt_zones::CLIMB:
|
||||
// output.forward_trans = input.Va_c;//sat(input.Va_c*cos(output.rotate_trans),5,0.5);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
// //output.forward_trans = 0.8;//0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
// output.left_right_trans = 0;//sat(input.Va_c*sin(output.rotate_trans),5,-5);//pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
|
||||
// output.up_down_trans = pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
|
||||
// output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
|
||||
|
||||
// //output.left_right_trans = 0.1 * pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
|
||||
// //output.up_down_trans = 0.01 * pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
|
||||
// //output.rotate_trans = 0.3*pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
|
||||
// ////if (input.h >= input.h_c - params.alt_hz) //alt_hz=0.2//recovery
|
||||
// if (input.h >= input.h_c - 0.3) //alt_hz=0.2
|
||||
// {
|
||||
// ROS_DEBUG("hold");
|
||||
// current_zone = alt_zones::ALTITUDE_HOLD;
|
||||
// at_error_ = 0;
|
||||
// at_integrator_ = 0;
|
||||
// at_differentiator_ = 0;
|
||||
// a_error_ = 0;
|
||||
// a_integrator_ = 0;
|
||||
// a_differentiator_ = 0;
|
||||
// }
|
||||
// //else if (input.h <= params.alt_toz) //1.5//recovery
|
||||
// // else if (input.h <= 1) //1.5
|
||||
// // {
|
||||
// // ROS_DEBUG("takeoff");
|
||||
// // current_zone = alt_zones::TAKE_OFF; // Is that necessary?
|
||||
// // }
|
||||
// break;
|
||||
// case alt_zones::DESCEND:
|
||||
// output.forward_trans = input.Va_c;//sat(input.Va_c*cos(output.rotate_trans),5,0.5);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
// output.left_right_trans = 0;//sat(input.Va_c*sin(output.rotate_trans),5,-5);//pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
|
||||
// output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
|
||||
// output.up_down_trans = pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
|
||||
|
||||
// //output.forward_trans = 0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
// //output.left_right_trans = 0.1 * pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
|
||||
// //output.rotate_trans = pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
|
||||
// //output.up_down_trans = 0.1 * pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
|
||||
// //if (input.h <= input.h_c + params.alt_hz)//recovery
|
||||
// if (input.h <= input.h_c + 0.3)
|
||||
// {
|
||||
// if (input.h < 0) //modify later
|
||||
// {
|
||||
// ROS_DEBUG("landing");
|
||||
// current_zone = alt_zones::LANDING;
|
||||
// // output.forward_trans = sat(input.Va_c*cos(output.rotate_trans),1,0);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts) * 0.5;
|
||||
// // output.up_down_trans = pseudo_pitch_hold(input.h_c, input.h, params, input.Ts) / 2;
|
||||
// // output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// ROS_DEBUG("hold");
|
||||
// current_zone = alt_zones::ALTITUDE_HOLD;
|
||||
// at_error_ = 0;
|
||||
// at_integrator_ = 0;
|
||||
// at_differentiator_ = 0;
|
||||
// a_error_ = 0;
|
||||
// a_integrator_ = 0;
|
||||
// a_differentiator_ = 0;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// case alt_zones::ALTITUDE_HOLD:
|
||||
// output.forward_trans = input.Va_c;//sat(input.Va_c*cos(output.rotate_trans),5,0.5);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
// output.left_right_trans = 0;//sat(input.Va_c*sin(output.rotate_trans),5,-5);//0.5*pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
|
||||
// output.up_down_trans = pseudo_altitiude_hold(input.h_c, input.h, params, input.Ts);
|
||||
// output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
|
||||
// //output.forward_trans = 1;//0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
|
||||
// //output.left_right_trans = pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
|
||||
// //output.up_down_trans = 0.1 * pseudo_altitiude_hold(input.h_c, input.h, params, input.Ts);
|
||||
// //output.rotate_trans = 0.7*pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
|
||||
// //if (input.h >= input.h_c + params.alt_hz)//recovery
|
||||
// if (input.h >= input.h_c + 0.3)
|
||||
// {
|
||||
// ROS_DEBUG("desend");
|
||||
// current_zone = alt_zones::DESCEND;
|
||||
// ap_error_ = 0;
|
||||
// ap_integrator_ = 0;
|
||||
// ap_differentiator_ = 0;
|
||||
// }
|
||||
// //else if (input.h <= input.h_c - params.alt_hz)//recovery
|
||||
// else if (input.h <= input.h_c - 0.3)
|
||||
// {
|
||||
// ROS_DEBUG("climb");
|
||||
// current_zone = alt_zones::CLIMB;
|
||||
// ap_error_ = 0;
|
||||
// ap_integrator_ = 0;
|
||||
// ap_differentiator_ = 0;
|
||||
// }
|
||||
// break;
|
||||
// case alt_zones::LANDING:
|
||||
// ROS_DEBUG("landing");
|
||||
// current_zone = alt_zones::LANDING;
|
||||
// ap_error_ = 0;
|
||||
// ap_integrator_ = 0;
|
||||
// ap_differentiator_ = 0;
|
||||
// output.forward_trans = 0;
|
||||
// if (input.h < 0.6)
|
||||
// {
|
||||
// output.up_down_trans = 0;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// output.up_down_trans = -input.h/3;
|
||||
// }
|
||||
|
||||
// output.rotate_trans =0;
|
||||
// output.left_right_trans = 0;
|
||||
// break;
|
||||
// default:
|
||||
// break;
|
||||
// }
|
||||
|
||||
output.current_zone = current_zone;
|
||||
//output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
|
||||
//output.up_down_trans=pseudo_pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
|
||||
}
|
||||
|
||||
float pseudo_controller_example::pseudo_course(float chi, float chi_c)
|
||||
{
|
||||
chi_c=sat(chi_c,3.14159,-3.14159);
|
||||
chi=sat(chi,3.14159,-3.14159);
|
||||
float rotate = sat(chi_c-chi,2*3.14159,-2*3.14159);//-pi to pi
|
||||
// float lr;
|
||||
// if (rotate>3.14159)
|
||||
// {
|
||||
// lr=-1;
|
||||
// }
|
||||
// else if(rotate<-3.14159)
|
||||
// {
|
||||
// lr=1;
|
||||
// }
|
||||
//rotate=rotate+lr*2*3.14159;//more easier to rotate
|
||||
|
||||
// rotate=sat(rotate,3.14159/4,-3.14159/4);//limit
|
||||
return rotate;
|
||||
}
|
||||
|
||||
float pseudo_controller_example::pseudo_left_right(float chi, float chi_c, float forward_trans)
|
||||
{
|
||||
float error= chi_c-chi;
|
||||
float a = fabs(forward_trans);
|
||||
float x = (3.14 / 4 > fabs(error)) ? fabs(error) : 3.14 / 4;
|
||||
float b = tan(x);
|
||||
float c = error / (fabs(error + 0.001));
|
||||
float left_right_trans = sat(a * b * c, forward_trans, -forward_trans);
|
||||
|
||||
return left_right_trans;
|
||||
}
|
||||
|
||||
float pseudo_controller_example::pseudo_course_hold(float chi, float chi_c, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = chi_c-chi;
|
||||
|
||||
//c_integrator_ = c_integrator_ + (Ts / 2.0) * (error + c_error_);
|
||||
|
||||
//float up = params.c_kp * error;
|
||||
//float ui = params.c_ki * c_integrator_;
|
||||
|
||||
float rotate_trans = sat((error + c_error_) / 2, 45.0 * 3.14 / 180.0, -45.0 * 3.14 / 180.0);
|
||||
//if (fabs(params.c_ki) >= 0.00001)
|
||||
//{
|
||||
//float rotate_trans_unsat = up + ui;
|
||||
//c_integrator_ = c_integrator_ + (Ts / params.c_ki) * (rotate_trans - rotate_trans_unsat);
|
||||
//}
|
||||
|
||||
c_error_ = error;
|
||||
return rotate_trans;
|
||||
}
|
||||
|
||||
float pseudo_controller_example::pseudo_pitch_hold(float h_c, float h, const struct params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = h_c - h;
|
||||
//ap_integrator_ = ap_integrator_ + (Ts / 2.0) * (error + ap_error_);
|
||||
// float up = 0.5 * error;
|
||||
//float ui = params.a_p_ki * ap_integrator_;
|
||||
//float up_down_trans = sat(up + ui, 0.8, -0.8);//recovery
|
||||
float up_down_trans = sat(0.5 * error, 2, -2);
|
||||
//if (fabs(params.a_p_ki) >= 0.00001)
|
||||
//{
|
||||
//float up_down_trans_unsat = up + ui;
|
||||
//ap_integrator_ = ap_integrator_ + (Ts / params.a_p_ki) * (up_down_trans - up_down_trans_unsat);
|
||||
//}
|
||||
// ap_error_ = error;
|
||||
return up_down_trans;
|
||||
}
|
||||
|
||||
float pseudo_controller_example::pseudo_throttle_hold(float Va_c, float Va, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = Va_c - Va;
|
||||
|
||||
//at_integrator_ = at_integrator_ + (Ts / 2.0) * (error + at_error_);
|
||||
//at_differentiator_ = (2.0 * params.tau - Ts) / (2.0 * params.tau + Ts) * at_differentiator_ + (2.0 /(2.0 * params.tau + Ts)) *(error - at_error_);
|
||||
|
||||
float up = params.a_t_kp * error;
|
||||
//float ui = params.a_t_ki * at_integrator_;
|
||||
//float ud = params.a_t_kd * at_differentiator_;
|
||||
|
||||
//float forward_trans = sat(Va + up + ui + ud, params.max_t, 0.1);
|
||||
float forward_trans = sat(Va+error, params.max_t, 0.5);
|
||||
//if (fabs(params.a_t_ki) >= 0.00001)
|
||||
//{
|
||||
//float forward_trans_unsat = params.trim_t + up + ui + ud;
|
||||
//at_integrator_ = at_integrator_ + (Ts / params.a_t_ki) * (forward_trans - forward_trans_unsat);
|
||||
//}
|
||||
|
||||
//at_error_ = error;
|
||||
return forward_trans;
|
||||
}
|
||||
|
||||
float pseudo_controller_example::pseudo_altitiude_hold(float h_c, float h, const params_s ¶ms, float Ts)
|
||||
{
|
||||
float error = h_c - h;
|
||||
|
||||
//a_integrator_ = a_integrator_ + (Ts / 2.0) * (error + a_error_);
|
||||
//a_differentiator_ = (2.0 * params.tau - Ts) / (2.0 * params.tau + Ts) * a_differentiator_ + (2.0 /(2.0 * params.tau + Ts)) * (error - a_error_);
|
||||
float up = 0.7 * error;
|
||||
//float up = params.a_kp * error;
|
||||
//ROS_INFO("hello %f",params.a_kp);
|
||||
//float ui = params.a_ki * a_integrator_;
|
||||
//float ud = params.a_kd * a_differentiator_;
|
||||
|
||||
//float up_down_trans = sat(up + ui + ud, 0.8, -0.8);//recovery
|
||||
float up_down_trans = sat(up, 2, -2);
|
||||
//if (fabs(params.a_ki) >= 0.00001)
|
||||
//{
|
||||
//float up_down_trans_unsat = up + ui + ud;
|
||||
//a_integrator_ = a_integrator_ + (Ts / params.a_ki) * (up_down_trans - up_down_trans_unsat);
|
||||
//}
|
||||
|
||||
// at_error_ = error;
|
||||
return up_down_trans;
|
||||
}
|
||||
|
||||
//float pseudo_controller_example::cooridinated_turn_hold(float v, const params_s ¶ms, float Ts)
|
||||
//{
|
||||
// //todo finish this if you want...
|
||||
// return 0;
|
||||
//}
|
||||
|
||||
float pseudo_controller_example::sat(float value, float up_limit, float low_limit)
|
||||
{
|
||||
float rVal;
|
||||
if (value > up_limit)
|
||||
rVal = up_limit;
|
||||
else if (value < low_limit)
|
||||
rVal = low_limit;
|
||||
else
|
||||
rVal = value;
|
||||
|
||||
return rVal;
|
||||
}
|
||||
|
||||
} // namespace rosplane
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit c5accd2403622ddc275e576691b9831bd6e2694f
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 9404d28c2d96f498a5d7f6f14a639f9318c30d64
|
||||
|
|
@ -0,0 +1,293 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(udp_commu)
|
||||
|
||||
## 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
|
||||
nav_msgs
|
||||
message_generation
|
||||
genmsg
|
||||
#udp_bc_broker
|
||||
)
|
||||
|
||||
## 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 run_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 run_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
|
||||
Up_Data.msg
|
||||
Down_Data.msg
|
||||
Down_Data_Speed_Nav.msg
|
||||
Leader_Receive.msg
|
||||
Broad_State.msg
|
||||
# State.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
|
||||
std_msgs
|
||||
nav_msgs
|
||||
# rosplane_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 run_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 serial_commu
|
||||
CATKIN_DEPENDS message_runtime roscpp rospy std_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}/serial_commu.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(udp_receive_node src/udp_receive.cpp)
|
||||
# add_executable(udp_send_node src/udp_send.cpp)
|
||||
# add_executable(udp_tasksend_speed_nav_node src/udp_tasksend_speed_nav.cpp)
|
||||
# add_executable(udp_gazeboreceive_speed_nav_node src/udp_gazeboreceive_speed_nav.cpp)
|
||||
# add_executable(udp_gazebosend_state_node src/udp_gazebosend_state.cpp)
|
||||
# add_executable(udp_taskreceive_state_node src/udp_taskreceive_state.cpp)
|
||||
# add_executable(leader_broadcast_node src/leader_broadcast.cpp)
|
||||
# add_executable(follower_receive_node src/follower_receive.cpp)
|
||||
# add_executable(udp_docker_send_receive_node src/udp_docker_send_receive.cpp)
|
||||
add_executable(udp_gazebo_send_receive_node src/udp_gazebo_send_receive.cpp)
|
||||
# add_executable(groundstation_broadcast_node src/groundstation_broadcast.cpp)
|
||||
|
||||
# add_executable(leader_receive_node src/leader_receive.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(udp_receive_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(udp_send_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(udp_tasksend_speed_nav_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(udp_gazeboreceive_speed_nav_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(udp_gazebosend_state_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(udp_taskreceive_state_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(leader_broadcast_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(follower_receive_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(udp_docker_send_receive_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
add_dependencies(udp_gazebo_send_receive_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
# add_dependencies(groundstation_broadcast_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
# add_dependencies(leader_receive_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
# target_link_libraries(udp_receive_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
# target_link_libraries(udp_send_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
# target_link_libraries(udp_tasksend_speed_nav_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
# target_link_libraries(udp_gazeboreceive_speed_nav_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
# target_link_libraries(udp_gazebosend_state_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
# target_link_libraries(udp_taskreceive_state_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
# target_link_libraries(leader_broadcast_node
|
||||
# UDPBCBroker
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
# target_link_libraries(follower_receive_node
|
||||
# UDPBCBroker
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
# target_link_libraries(udp_docker_send_receive_node
|
||||
# UDPBCBroker
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
target_link_libraries(udp_gazebo_send_receive_node
|
||||
# UDPBCBroker
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
# target_link_libraries(groundstation_broadcast_node
|
||||
# UDPBCBroker
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
# target_link_libraries(leader_receive_node
|
||||
# UDPBCBroker
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
|
||||
# add_dependencies(udp_receive_node udp_commu_generate_messages_cpp)
|
||||
# add_dependencies(udp_send_node udp_commu_generate_messages_cpp)
|
||||
# add_dependencies(udp_tasksend_speed_nav_node udp_commu_generate_messages_cpp)
|
||||
# add_dependencies(udp_gazeboreceive_speed_nav_node udp_gazeboreceive_speed_nav_cpp)
|
||||
# add_dependencies(udp_gazebosend_state_node udp_gazebosend_state_cpp)
|
||||
# add_dependencies(udp_taskreceive_state_node udp_taskreceive_state_cpp)
|
||||
# add_dependencies(leader_broadcast_node leader_broadcast_cpp)
|
||||
# add_dependencies(follower_receive_node follower_receive_cpp)
|
||||
# add_dependencies(udp_docker_send_receive_node udp_docker_send_receive_cpp)
|
||||
add_dependencies(udp_gazebo_send_receive_node udp_gazebo_send_receive_cpp)
|
||||
|
||||
# add_dependencies(groundstation_broadcast_node groundstation_broadcast_cpp)
|
||||
# add_dependencies(leader_receive_node leader_receive_cpp)
|
||||
|
||||
#############
|
||||
## 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
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_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_serial_commu.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)
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
<launch>
|
||||
<!-- kobuki -->
|
||||
<include file="$(find serial_commu)/launch/serial_commu.launch" />
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
<launch>
|
||||
<!-- Load controller names -->
|
||||
<!--rosparam command="load" file="$(find act_softbus)/config/controllers.yaml" ns="act_softbus"/-->
|
||||
|
||||
<node name="serial_receive_node" pkg="serial_commu" type="serial_receive_node" output="screen" >
|
||||
</node>
|
||||
<node name="serial_send_node" pkg="serial_commu" type="serial_send_node" output="screen" >
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
|
||||
<node name="udp_gazebosend_state" pkg="udp_commu" type="udp_gazebosend_state_node" output="screen">
|
||||
<param name="dockerleader_ipaddress" type="string" value="172.18.0.3"/>
|
||||
<param name="namespace" type="string" value="leader"/>
|
||||
</node>
|
||||
|
||||
<node name="udp_gazeboreceive_speed_nav" pkg="udp_commu" type="udp_gazeboreceive_speed_nav_node" output="screen">
|
||||
<param name="dockerleader_ipaddress" type="string" value="172.18.0.3"/>
|
||||
<param name="namespace" type="string" value="leader"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
string name
|
||||
nav_msgs/Odometry msg
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
int32 status_word
|
||||
float32 longitude
|
||||
float32 latitude
|
||||
float32 altitude
|
||||
float32 plane_speed
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue