diff --git a/docker/Dockerfile b/docker/Dockerfile
index b4e82fd4..fb90a02f 100644
--- a/docker/Dockerfile
+++ b/docker/Dockerfile
@@ -62,6 +62,9 @@ RUN wget http://ceres-solver.org/ceres-solver-2.1.0.tar.gz \
&& mkdir ceres-bin && cd ceres-bin \
&& cmake ../ceres-solver-2.1.0 && make -j3 && sudo make install
+# Install rospkg for rc_flight
+RUN pip3 install rospkg
+
# Build livox_ros_driver
RUN echo "source /opt/ros/melodic/setup.sh" >> /home/user/.bashrc
WORKDIR /home/user
diff --git a/run_docker.sh b/run_docker.sh
index 80b55885..ce401800 100755
--- a/run_docker.sh
+++ b/run_docker.sh
@@ -57,31 +57,16 @@ elif [ "$1" = "run" ]; then
CMD_ARGS="$CMD_ARGS --gpus all"
fi
- # TODO(ulken94): The LIO-Livox should not be mounted for aarch64.
- if [ "$ARCH" = "aarch64" ]; then
- docker run $DOCKER_OPT --privileged \
- -e DISPLAY=${DISPLAY} \
- -e TERM=xterm-256color \
- -v /tmp/.X11-unix:/tmp/.X11-unix:ro \
- -v /dev:/dev \
- -v $PWD/src:/home/user/catkin_ws/src \
- --network host \
- $CMD_ARGS \
- $DOCKER_TAG \
- $RUN_SHELL
- else
- docker run $DOCKER_OPT --privileged \
- -e DISPLAY=${DISPLAY} \
- -e TERM=xterm-256color \
- -v /tmp/.X11-unix:/tmp/.X11-unix:ro \
- -v /dev:/dev \
- -v $PWD/catkin_ws/src:/home/user/catkin_ws/src \
- -v $PWD/res:/home/user/res \
- --network host \
- $CMD_ARGS \
- $DOCKER_TAG \
- $RUN_SHELL
- fi
+ docker run $DOCKER_OPT --privileged \
+ -e DISPLAY=${DISPLAY} \
+ -e TERM=xterm-256color \
+ -v /tmp/.X11-unix:/tmp/.X11-unix:ro \
+ -v /dev:/dev \
+ -v $PWD/src:/home/user/catkin_ws/src \
+ --network host \
+ $CMD_ARGS \
+ $DOCKER_TAG \
+ $RUN_SHELL
last_cont_id=$(docker ps -qn 1)
echo $(docker ps -qn 1) > $PWD/.last_exec_cont_id.txt
diff --git a/src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h b/src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h
index cf7e96eb..15688e0e 100644
--- a/src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h
+++ b/src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h
@@ -115,6 +115,7 @@ namespace ego_planner
bool is_need_replan_ = false;
int reboundReplan_fail_count_ = 0;
Eigen::Vector3d checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &waypoint);
+ Eigen::Vector3d checkEnableStartpoint(const Eigen::Vector3d &start);
public:
EGOReplanFSM(/* args */)
diff --git a/src/planner/plan_manage/launch/advanced_param.xml b/src/planner/plan_manage/launch/advanced_param.xml
index 876a3e93..4ffe364f 100644
--- a/src/planner/plan_manage/launch/advanced_param.xml
+++ b/src/planner/plan_manage/launch/advanced_param.xml
@@ -106,7 +106,7 @@
-
+
diff --git a/src/planner/plan_manage/launch/run_in_flight.launch b/src/planner/plan_manage/launch/run_in_flight.launch
index eb343eeb..7da5b485 100644
--- a/src/planner/plan_manage/launch/run_in_flight.launch
+++ b/src/planner/plan_manage/launch/run_in_flight.launch
@@ -2,7 +2,16 @@
-
+
+
+
+
+
+
+
+
+
+
@@ -32,11 +41,11 @@
-
-
+
+
-
+
@@ -66,7 +75,7 @@
-
+
@@ -76,8 +85,12 @@
-
-
+
+
+
+
+
+
@@ -88,8 +101,16 @@
-
-
+
+
+
+
+
+
+
+
+
+
-
+
+
diff --git a/src/planner/plan_manage/launch/run_in_px4sim.launch b/src/planner/plan_manage/launch/run_in_px4sim.launch
index ebf22b86..aab65174 100644
--- a/src/planner/plan_manage/launch/run_in_px4sim.launch
+++ b/src/planner/plan_manage/launch/run_in_px4sim.launch
@@ -2,7 +2,7 @@
-
+
@@ -78,6 +78,9 @@
+
+
+
diff --git a/src/planner/plan_manage/src/ego_replan_fsm.cpp b/src/planner/plan_manage/src/ego_replan_fsm.cpp
index 881360b9..eb151757 100644
--- a/src/planner/plan_manage/src/ego_replan_fsm.cpp
+++ b/src/planner/plan_manage/src/ego_replan_fsm.cpp
@@ -243,14 +243,26 @@ namespace ego_planner
cout << "Triggered!" << endl;
trigger_ = true;
- init_pt_ = odom_pos_;
+ init_pt_ = checkEnableStartpoint(odom_pos_);
- bool success = false;
end_pt_ << msg.poses[0].pose.position.x, msg.poses[0].pose.position.y, msg.poses[0].pose.position.z;
- end_pt_ << checkEnableWaypoint(odom_pos_, end_pt_);
- success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
+ visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 1.0, 0.25, 0.7), 0.3, 0);
- visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0);
+ Eigen::Vector3d sub_xy(end_pt_.x() - init_pt_.x(), end_pt_.y() - init_pt_.y(), 0);
+ if (sub_xy.norm() < no_replan_thresh_ && abs(end_pt_.z() - init_pt_.z()) > 0.1) {
+ traj_pts_.clear();
+ traj_pts_.push_back(init_pt_);
+ traj_pts_.push_back(end_pt_);
+ traj_pts_.push_back(end_pt_);
+ visualization_->displayTrajList(traj_pts_, 0);
+ visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 0.25, 1.0, 0.7), 0.3, 1);
+ return;
+ }
+
+ end_pt_ << checkEnableWaypoint(init_pt_, end_pt_);
+ visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 0.25, 1.0, 0.7), 0.3, 1);
+
+ bool success = planner_manager_->planGlobalTraj(init_pt_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
if (success)
{
@@ -375,7 +387,7 @@ namespace ego_planner
case GEN_NEW_TRAJ:
{
- start_pt_ = odom_pos_;
+ start_pt_ = init_pt_;
start_vel_ = odom_vel_;
start_acc_.setZero();
traj_pts_.clear();
@@ -449,6 +461,7 @@ namespace ego_planner
{
// cout << "near end" << endl;
traj_pts_.push_back(end_pt_);
+ if (traj_pts_.size() == 2) traj_pts_.push_back(end_pt_);
visualization_->displayTrajList(traj_pts_, 0);
current_traj_ = generateTraj(traj_pts_);
return;
@@ -589,45 +602,159 @@ namespace ego_planner
// }
}
- Eigen::Vector3d EGOReplanFSM::checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &waypoint) {
+ Eigen::Vector3d EGOReplanFSM::checkEnableStartpoint(const Eigen::Vector3d &start) {
+ // first, check is waypoint validate
+ auto map = planner_manager_->grid_map_;
+ auto inflatePointWithoutObs = [map](const Eigen::Vector3d& pt, int step, vector& pts) {
+ pts.clear();
+ double res = map->getResolution();
+ /* ---------- all inflate ---------- */
+ for (int x = -step; x <= step; ++x)
+ for (int y = -step; y <= step; ++y)
+ for (int z = -step; z <= step; ++z) {
+ if (sqrt(x*x + y*y + z*z) > (double)step) continue;
+ Eigen::Vector3d new_pt(pt(0) + (double)x * res, pt(1) + (double)y * res, pt(2) + (double)z * res);
+ if (!map->getInflateOccupancy(new_pt)) {
+ pts.push_back(new_pt);
+ }
+ }
+ };
+
+ int inf_step = 2; // TODO: add inf_step parameter for this function
+ vector inf_pts;
+ inflatePointWithoutObs(start, inf_step, inf_pts);
+
+ if (inf_pts.size() > 0) {
+ double best_score = inf_pts.size();
+ Eigen::Vector3d best_pt = start;
+
+ for (auto inf_pt : inf_pts) {
+ vector inf_pts_sub;
+ inflatePointWithoutObs(inf_pt, inf_step, inf_pts_sub);
+ double score = (double)inf_pts_sub.size();
+ if (score > best_score) {
+ best_pt = inf_pt;
+ best_score = score;
+ }
+ }
+
+ return best_pt;
+ }
+
+ return start;
+ }
+
+ Eigen::Vector3d EGOReplanFSM::checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &end) {
+ Eigen::Vector3d waypoint = end;
+
ros::Time time_1 = ros::Time::now();
+ // first, check is waypoint validate
+ auto map = planner_manager_->grid_map_;
+ auto inflatePointWithoutObs = [map](const Eigen::Vector3d& pt, int step, vector& pts) {
+ pts.clear();
+ double res = map->getResolution();
+ /* ---------- all inflate ---------- */
+ for (int x = -step; x <= step; ++x)
+ for (int y = -step; y <= step; ++y)
+ for (int z = -step/2; z <= step/2; ++z) {
+ if (sqrt(x*x + y*y + z*z) > (double)step) continue;
+ Eigen::Vector3d new_pt(pt(0) + (double)x * res, pt(1) + (double)y * res, pt(2) + (double)z * res);
+ if (!map->getInflateOccupancy(new_pt)) {
+ pts.push_back(new_pt);
+ }
+ }
+ };
+
+ int inf_step = 7; // TODO: add inf_step parameter for this function
+ vector inf_pts;
+ inflatePointWithoutObs(waypoint, inf_step, inf_pts);
+
+ if (inf_pts.size() > 0) {
+ double best_score = inf_pts.size();
+ Eigen::Vector3d best_pt = waypoint;
+
+ for (auto inf_pt : inf_pts) {
+ vector inf_pts_sub;
+ inflatePointWithoutObs(inf_pt, inf_step, inf_pts_sub);
+ double score = (double)inf_pts_sub.size();
+ if (score > best_score) {
+ best_pt = inf_pt;
+ best_score = score;
+ }
+ }
+
+ waypoint = best_pt;
+ }
+
+ // TODO: check A* start path
+ros::Time time_2 = ros::Time::now();
vector> a_star_pathes;
vector a_star_path_H, a_star_path_V, a_star_path_A;
double expected_length = (waypoint - start).norm() / planner_manager_->grid_map_->getResolution();
+
+ auto shrink_path = [&](vector a_star_path){
+ if (a_star_path.size() < 2) return a_star_path;
+
+ vector shrinked_path;
+ Eigen::Vector3d last_ptd = a_star_path.front();
+ Eigen::Vector3i last_pti;
+ Eigen::Vector3i last_dir(0, 0, 0);
+ planner_manager_->grid_map_->posToIndex(last_ptd, last_pti);
+
+ for (size_t i = 0; i < a_star_path.size(); i++) {
+ Eigen::Vector3i cur_pti;
+ planner_manager_->grid_map_->posToIndex(a_star_path[i], cur_pti);
+ Eigen::Vector3i cur_dir = cur_pti - last_pti;
+ if (last_dir != cur_dir) {
+ shrinked_path.push_back(last_ptd);
+ last_ptd = a_star_path[i-1];
+ last_pti = cur_pti;
+ last_dir = cur_dir;
+ } else if (i == a_star_path.size() - 1) {
+ shrinked_path.push_back(last_ptd);
+ } else {
+ last_pti = cur_pti;
+ }
+ }
+
+ shrinked_path.push_back(a_star_path.back());
+ return shrinked_path;
+ };
if (expected_length > 1) {
planner_manager_->grid_map_->setSearchAreaH();
- if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, end_pt_)) {
+ if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, start, waypoint)) {
a_star_path_H = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath();
}
}
- if (a_star_path_H.size() > expected_length * 1.1) {
- planner_manager_->grid_map_->setSearchAreaV(end_pt_);
- if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, end_pt_)) {
+ if (a_star_path_H.size() > expected_length * 1.1 || a_star_path_H.size() == 0) {
+ planner_manager_->grid_map_->setSearchAreaV(waypoint);
+ if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, start, waypoint)) {
a_star_path_V = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath();
}
}
- if (a_star_path_V.size() > expected_length * 1.1 || (a_star_path_V.size() == 0 && a_star_path_H.size() > expected_length * 1.1))
+ if (a_star_path_V.size() > expected_length * 1.1 || (a_star_path_V.size() == 0 && a_star_path_H.size() > expected_length * 1.1) || a_star_path_H.size() == 0)
{
planner_manager_->grid_map_->resetSearchArea();
- if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, end_pt_)) {
+ if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, start, waypoint)) {
a_star_path_A = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath();
}
}
- a_star_pathes.push_back(a_star_path_H);
- a_star_pathes.push_back(a_star_path_V);
- a_star_pathes.push_back(a_star_path_A);
+ a_star_pathes.push_back(shrink_path(a_star_path_H));
+ a_star_pathes.push_back(shrink_path(a_star_path_V));
+ a_star_pathes.push_back(shrink_path(a_star_path_A));
planner_manager_->visualization_->displayAStarList(a_star_pathes, 0);
planner_manager_->grid_map_->resetSearchArea();
-ros::Time time_2 = ros::Time::now();
+ros::Time time_3 = ros::Time::now();
// compute time check
-std::cout << "AstarSearch time : " << (time_2 - time_1).toSec() << std::endl;
-std::cout << "expected : " << (waypoint - start).norm() << ", ";
+std::cout << "move goal time : " << (time_2 - time_1).toSec() << std::endl;
+std::cout << "AstarSearch time : " << (time_3 - time_2).toSec() << std::endl;
+std::cout << "expected : " << expected_length << ", ";
std::cout << "H : " << a_star_path_H.size() << ", ";
std::cout << "V : " << a_star_path_V.size() << ", ";
std::cout << "A : " << a_star_path_A.size() << std::endl;
diff --git a/src/planner/plan_manage/src/traj_server.cpp b/src/planner/plan_manage/src/traj_server.cpp
index 91c6f4bf..60584258 100644
--- a/src/planner/plan_manage/src/traj_server.cpp
+++ b/src/planner/plan_manage/src/traj_server.cpp
@@ -15,6 +15,8 @@ ros::Publisher setpoint_raw_pub;
Eigen::Vector3d odom_pos_, odom_vel_; // odometry state
double odom_yaw_ = 0;
geometry_msgs::Pose control_;
+geometry_msgs::Pose cur_waypoint_;
+bool cur_waypoint_initialized_ = false;
ros::Publisher pos_cmd_pub, ref_target_pub;
@@ -36,9 +38,12 @@ int traj_id_;
// yaw control
double last_yaw_, last_yaw_dot_;
double time_forward_;
-bool use_velocity_control_, enable_rotate_head_;
+bool use_velocity_control_, enable_rotate_head_, use_waypoint_yaw_;
double forward_length_;
+double max_vel_;
+double z_offset_;
+
void bsplineCallback(ego_planner::BsplineConstPtr msg)
{
// parse pos traj
@@ -108,7 +113,7 @@ void customTrajCallback(visualization_msgs::MarkerConstPtr msg)
UniformBspline pos_traj(pos_pts, 3, 1.0);
- if (pos_traj.getTimeSum() > 0 && pos_traj.getLength(0.02) > forward_length_) {
+ if (pos_traj.getTimeSum() > 0) {
start_time_ = ros::Time::now();
traj_.clear();
traj_.push_back(pos_traj);
@@ -184,17 +189,26 @@ void cmdCallback(const ros::TimerEvent &e)
/* check receive traj_ before calculate target */
if (receive_traj_) {
- Eigen::Vector3d z_offset = Eigen::Vector3d(0, 0, 0.1);
+ Eigen::Vector3d z_offset = Eigen::Vector3d(0, 0, z_offset_);
Eigen::Vector3d closestPoint = findClosestPoint(odom_pos_, traj_[0]);
Eigen::Vector3d refTarget = findClosestPoint(closestPoint, traj_[0], forward_length_) + z_offset;
Eigen::Vector3d refTarget_forward = findClosestPoint(closestPoint, traj_[0], forward_length_ * 2.5) + z_offset;
Eigen::Vector3d sub_vector(refTarget.x() - odom_pos_.x(), refTarget.y() - odom_pos_.y(), refTarget.z() - odom_pos_.z());
Eigen::Vector3d sub_vector_xy(refTarget.x() - odom_pos_.x(), refTarget.y() - odom_pos_.y(), 0);
-
+
double ref_yaw = last_yaw_;
if (enable_rotate_head_) {
ref_yaw = atan2(refTarget_forward.y() - odom_pos_.y(), refTarget_forward.x() - odom_pos_.x());
last_yaw_ = ref_yaw;
+ } else if (use_waypoint_yaw_ && cur_waypoint_initialized_) {
+ Eigen::Quaterniond waypoint_orient;
+ waypoint_orient.x() = cur_waypoint_.orientation.x;
+ waypoint_orient.y() = cur_waypoint_.orientation.y;
+ waypoint_orient.z() = cur_waypoint_.orientation.z;
+ waypoint_orient.w() = cur_waypoint_.orientation.w;
+ Eigen::Vector3d rot_x = waypoint_orient.toRotationMatrix().block(0, 0, 3, 1);
+ ref_yaw = atan2(rot_x(1), rot_x(0));
+ last_yaw_ = ref_yaw;
}
// visualize target
@@ -229,29 +243,22 @@ void cmdCallback(const ros::TimerEvent &e)
msg.type_mask |= msg.IGNORE_PX | msg.IGNORE_PY | msg.IGNORE_PZ;
double alpha = 0.5;
- double max_velocity = 1.0;
Eigen::Vector3d refVel = alpha * (refTarget - odom_pos_) + (1.0 - alpha) * (closestPoint - odom_pos_);
- refVel = refVel * (min(sub_vector.norm(), max_velocity) / refVel.norm());
+ refVel = refVel * (min(sub_vector.norm(), max_vel_) / refVel.norm());
msg.velocity.x = refVel.x();
msg.velocity.y = refVel.y();
msg.velocity.z = refVel.z();
-
- if ((ros::Time::now() - start_time_).toSec() < 1.0) {
- msg.velocity.x = 0.0;
- msg.velocity.y = 0.0;
- msg.velocity.z = 0.0;
- }
}
- if (sub_vector_xy.norm() > 0.5) {
+ if (sub_vector_xy.norm() > forward_length_ * 0.6) {
msg.type_mask |= msg.IGNORE_YAW_RATE;
msg.yaw = ref_yaw;
- } else if (sub_vector_xy.norm() < 0.1) {
+ } else if (sub_vector.norm() < 0.1) {
msg.type_mask |= msg.IGNORE_YAW;
receive_traj_ = false;
- last_odom_pos_ = odom_pos_;
+ last_odom_pos_ = refTarget;
} else {
msg.type_mask |= msg.IGNORE_YAW;
last_odom_pos_ = odom_pos_;
@@ -259,7 +266,6 @@ void cmdCallback(const ros::TimerEvent &e)
} else {
msg.type_mask |= msg.IGNORE_VX | msg.IGNORE_VY | msg.IGNORE_VZ;
- last_yaw_ = odom_yaw_;
Eigen::Vector3d waypoint_pose(control_.position.x, control_.position.y, 0);
@@ -277,6 +283,7 @@ void cmdCallback(const ros::TimerEvent &e)
if (abs(waypoint_yaw) > 0.1) {
msg.type_mask |= msg.IGNORE_YAW;
msg.yaw_rate = waypoint_yaw;
+ last_yaw_ = odom_yaw_;
} else {
msg.type_mask |= msg.IGNORE_YAW_RATE;
msg.yaw = last_yaw_;
@@ -309,6 +316,11 @@ void controlCallback(const geometry_msgs::PoseConstPtr &msg) {
control_ = *msg;
}
+void waypointCallback(const nav_msgs::PathConstPtr &msg) {
+ cur_waypoint_ = msg->poses[0].pose;
+ cur_waypoint_initialized_ = true;
+}
+
int main(int argc, char **argv)
{
ros::init(argc, argv, "traj_server");
@@ -318,7 +330,8 @@ int main(int argc, char **argv)
// ros::Subscriber bspline_sub = node.subscribe("planning/bspline", 10, bsplineCallback);
ros::Subscriber trajlist_sub = node.subscribe("/ego_planner_node/traj_list", 10, customTrajCallback);
ros::Subscriber odom_sub = node.subscribe("/odom_world", 10, odom_callback);
- ros::Subscriber waypoint_sub = node.subscribe("/waypoint_generator/waypoint_manual", 10, controlCallback);
+ ros::Subscriber control_sub = node.subscribe("/waypoint_generator/waypoint_manual", 10, controlCallback);
+ ros::Subscriber waypoint_sub = node.subscribe("/waypoint_generator/waypoints", 10, waypointCallback);
pos_cmd_pub = node.advertise("/position_cmd", 50);
setpoint_raw_pub = node.advertise("/mavros/setpoint_raw/local", 1);
@@ -342,6 +355,9 @@ int main(int argc, char **argv)
nh.param("traj_server/use_velocity_control", use_velocity_control_, false);
nh.param("traj_server/forward_length", forward_length_, 1.0);
nh.param("traj_server/enable_rotate_head", enable_rotate_head_, true);
+ nh.param("traj_server/use_waypoint_yaw", use_waypoint_yaw_, false);
+ nh.param("traj_server/max_vel", max_vel_, 1.0);
+ nh.param("traj_server/z_offset", z_offset_, 0.0);
ros::Duration(1.0).sleep();
diff --git a/src/rc_demo/launch/rc_flight.launch b/src/rc_demo/launch/rc_flight.launch
new file mode 100644
index 00000000..66ff9a65
--- /dev/null
+++ b/src/rc_demo/launch/rc_flight.launch
@@ -0,0 +1,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/rc_demo/rc_flight.py b/src/rc_demo/rc_flight.py
index 2d7f135a..7ffb9e3d 100755
--- a/src/rc_demo/rc_flight.py
+++ b/src/rc_demo/rc_flight.py
@@ -2,17 +2,41 @@
import sys
import rospy
+import yaml
+import time
+import threading
+from std_msgs.msg import String
from mavros_msgs.msg import RCIn
-from utils import ControlMessage
+from utils import ControlMessage, WaypointMessage
+
+waypoint_thread_lock = threading.Lock()
class RCHandler:
"""Receieve RC stick data."""
- def __init__(self) -> None:
+ def __init__(self,
+ use_rc_control: bool = True,
+ waypoint_path: str = "",
+ target_waypoint: str = "waypoint_0",
+ distance_tolerance: float = 0.5,
+ angle_tolerance: float = 10.0,
+ offset_x: float = 0.0,
+ offset_y: float = 0.0,
+ offset_z: float = 0.0,
+ offset_Y: float = 0.0,
+ check_yaw: bool = True) -> None:
+ self.use_rc_control = use_rc_control
+ self.target_waypoint = target_waypoint
+
self.rc_sub = rospy.Subscriber("/mavros/rc/in", RCIn, self.callback_rc_in)
+ self.waypoint_trigger_sub = rospy.Subscriber("/custom/waypoint_trigger", String, self.callback_waypoint_trigger)
self.control_msg = ControlMessage()
+ self.waypoint_msg = WaypointMessage(distance_tolerance, angle_tolerance, check_yaw)
+ self.waypoint_msg.set_offset(offset_x, offset_y, offset_z, offset_Y)
+
+ self.is_waypoint_working = False
self.is_calibrated = False
self.calib_count = 0
@@ -26,12 +50,63 @@ def __init__(self) -> None:
self.cmd_magnitude = 1.0
+ if waypoint_path != "":
+ with open(waypoint_path, 'r') as f:
+ self.waypoints = yaml.load(f, yaml.SafeLoader)
+ else:
+ self.waypoints = {
+ 'waypoint_0': [[0.0, 0.0, 1.2]],
+ 'waypoint_1': [[0.0, 0.0, 1.2]],
+ }
+
+ # Debugging log
+ if False:
+ rospy.loginfo(f"[TARGET WAYPOINT]: {self.target_waypoint}")
+
+ def _start_waypoint(self) -> None:
+ if self.is_waypoint_working:
+ return
+
+ waypoint_thread_lock.acquire()
+
+ self.is_waypoint_working = True
+ self.waypoint_msg.clear_waypoint()
+ self.waypoint_msg.add_waypoints(self.waypoints[self.target_waypoint])
+
+ waypoint_thread_lock.release()
+
+ def _terminate_waypoint(self) -> None:
+ waypoint_thread_lock.acquire()
+
+ self.is_waypoint_working = False
+ self.waypoint_msg.clear_waypoint()
+
+ waypoint_thread_lock.release()
+
+ def callback_waypoint_trigger(self, msg: String) -> None:
+ """Test trigger callback for waypoint flight.
+
+ You will need to trigger this by following command.
+ rostopic pub /custom/waypoint_trigger std_msgs/String "data: 'on'"
+ rostopic pub /custom/waypoint_trigger std_msgs/String "data: 'off'"
+ """
+ if msg.data == "on":
+ self._start_waypoint()
+ elif msg.data == "off":
+ self._terminate_waypoint()
def callback_rc_in(self, msg: RCIn) -> None:
"""Receieves /mavros/rc/in
Receieved stick message will be sent
"""
+
+ # Check waypoint switch
+ if 1300 < msg.channels[6] < 1700:
+ self._start_waypoint()
+ elif 1000 < msg.channels[6] < 1300:
+ self._terminate_waypoint()
+
for i in range(4):
self.val_range[0] = min(self.val_range[0], msg.channels[i])
self.val_range[1] = max(self.val_range[1], msg.channels[i])
@@ -64,7 +139,7 @@ def callback_rc_in(self, msg: RCIn) -> None:
else:
self.calib_count = 0
- if not self.is_calibrated:
+ if not self.is_calibrated or not self.use_rc_control:
return
self.control_msg.send_control(-self.roll * self.cmd_magnitude,
@@ -77,7 +152,22 @@ def callback_rc_in(self, msg: RCIn) -> None:
args = rospy.myargv(argv=sys.argv)
rospy.init_node("flight_control_node", anonymous=True)
- rc_handler = RCHandler()
+ use_rc_control = rospy.get_param('~rc_control', False)
+ waypoint_path = rospy.get_param('~waypoint', "")
+ target_waypoint = rospy.get_param('~target_waypoint', "waypoint_0")
+ distance_tolerance = rospy.get_param('~distance_tolerance', 0.5)
+ angle_tolerance = rospy.get_param('~angle_tolerance', 10)
+
+ offset_x = rospy.get_param('~offset_x', 0)
+ offset_y = rospy.get_param('~offset_y', 0)
+ offset_z = rospy.get_param('~offset_z', 0)
+ offset_Y = rospy.get_param('~offset_Y', 0)
+
+ check_yaw = rospy.get_param("~check_yaw", True)
+ distance_tolerance = rospy.get_param('~distance_tolerance', 0.5)
+ angle_tolerance = rospy.get_param('~angle_tolerance', 10)
+
+ rc_handler = RCHandler(use_rc_control, waypoint_path, target_waypoint, distance_tolerance, angle_tolerance, offset_x, offset_y, offset_z, offset_Y, check_yaw)
rospy.spin()
diff --git a/src/rc_demo/res/waypoints.yaml b/src/rc_demo/res/waypoints.yaml
new file mode 100644
index 00000000..43110766
--- /dev/null
+++ b/src/rc_demo/res/waypoints.yaml
@@ -0,0 +1,127 @@
+waypoint_0:
+ # This way point has been set from roslaunch simulation_environment mavros_posix_sitl_lidar.launch world:=$HOME/custom_worlds/add_competition_outfield.world x:=4 y:=0 z:=1 vehicle:=px4vision lidar:=ouster
+ [
+ [10.0, 1.5, 2.0],
+ [14, 0.8, 3.2],
+ [18, 0.8, 3.2],
+ [19, -1.1, 2.0],
+ [23, -1.1, 2.0],
+ [25, -0.5, 2.5],
+ [28, 0.0, 3.2],
+ [33, 0.0, 1.2],
+ ]
+
+waypoint_1:
+ [
+ [0.0, 0.0, 1.0],
+ [0.0, 0.0, 1.0, 180],
+ [0.0, 0.0, -1.0],
+ ]
+
+waypoint_outdoor:
+# This way point has been set from roslaunch simulation_environment mavros_posix_sitl_lidar.launch world:=$HOME/custom_worlds/add_competition_outfield.world x:=10.5 y:=-1 z:=1 vehicle:=px4vision lidar:=ouster
+ [
+ [0, 0, 1.59992909121], # hover
+ [3.72308462946, 2.16384181223, 1.59992909121], # first plants
+ # [7.20119367623, 2.13588568156, 2.61656869656], # second plants -> for simulation
+ [7.20119367623, 2.13588568156, 1.71656869656], # second plants -> for rosbag
+ [8.10119367623, 2.13588568156, 3.21656869656], # before wall
+ [10.70119367623, 2.13588568156, 3.21656869656], # before wall
+ [11.0070259193, 2.08961550795, 2.43913609922], # wall
+ [13.4681804363, -0.525301411499, 1.50505519304], # before net
+ # [18.8681804363, -0.525301411499, 1.50505519304], # after net
+ [18.5681804363, -0.525301411499, 1.50505519304], # after net
+ # [18.8681804363, 0.0, 1.50505519304], # after net
+ [25, 0.00, 1.8005] # car
+ ]
+
+waypoint_indoor:
+ [
+ # x, y, z, yaw(optional)
+ # yaw 0: Y direction
+ # yaw 90: X direction
+ # yaw 180: -Y direction
+ # yaw 270: -X direction
+ # If yaw is not given, previous yaw is maintained.
+ [0, 0, 1.0, 1.35], # hover
+ [3.9, -0.9, 0.7], # Right of stair
+ [4.4, 1.3, 0.8, 45], # In front of stair
+ [9.9, 1.6, 3.4], # Middle of stair left
+ [9.7, -0.3, 3.6], # Middle of stair right
+ [3.6, -0.3, 5.7], # 2nd floor
+ [1.6, -4.0, 5.0], # 1st door
+ ]
+
+waypoint_indoor2:
+ [
+ [0.0, 0.0, 1], # takeoff (for simulation)
+ [1, -0.5, 0.7], # hover
+ [3.5, -0.9, 0.7], # Right of stair
+ [4.4, 1.3, 0.8], # In front of stair
+ [9.7, 1.6, 3.4], # Middle of stair left
+ [9.7, -0.4, 3.6], # Middle of stair right
+ [1.7, 0.2, 5.7], # 2nd floor
+ [1.7, -0.8, 5.0, 90], # In front of first door
+ [1.8, -4.07, 5.0], # first room
+ [1.8, -4.07, 5.0, 0], # first room left turn
+ [1.8, -4.07, 5.0, 90], # first door
+ [3.58 , -7.87, 5.0, 180], # in front of second door
+ [-0.07, -7.44, 5.0] # after second door, hallway
+ ]
+
+waypoint_indoor2_sim:
+# This way point has been generated from roslaunch simulation_environment mavros_posix_sitl_lidar.launch world:=$HOME/custom_worlds/add_competition_map.world x:=6 y:=11 z:=2 Y:=0 vehicle:=px4vision lidar:=ouster
+ [
+ [0, 0, 1.6, 0], # hover
+ [0.9, 3.5, 0.7], # Right of stair
+ [-1.3, 4.4, 0.8], # In front of stair
+ [-1.6, 10.5, 3.4], # Middle of stair left
+ [1.0, 10.5, 4.6, 90], # Middle of stair right
+ [1.0, 3.0, 6.7, 180], # 2nd floor
+ [4.07, 3.5, 7.0, 90], # first door
+ [4.07, 3.5, 7.0, 0], # head rotation
+ [7.7, 3.0, 7.0, 180], # before 2nd door
+ [7.7, 0.7, 7.0, 180], # after 2nd door
+ [7.3, 0.7, 7.0, 90], # after 2nd door
+ [7.3, 0.7, 7.0, 270], # after 2nd door
+ ]
+
+waypoint_merged:
+ [
+ [0, 0, 1.59992909121], # hover
+ [3.72308462946, 2.16384181223, 1.59992909121], # first plants
+ # [7.20119367623, 2.13588568156, 2.61656869656], # second plants -> for simulation
+ [7.20119367623, 2.13588568156, 1.71656869656], # second plants -> for rosbag
+ [8.10119367623, 2.13588568156, 3.21656869656], # before wall
+ [10.70119367623, 2.13588568156, 3.21656869656], # before wall
+ [11.0070259193, 2.08961550795, 2.43913609922], # wall
+ [13.4681804363, -0.525301411499, 1.50505519304], # before net
+ [18.5681804363, -0.525301411499, 1.50505519304], # after net
+ [25, 0.00, 1.8005], # car
+ [28, 1.00, 1.5, 0],
+ [28, 5.0, 1.5],
+ [28.9, 8.5, 1], # Right of stair
+ [26.7, 9.4, 1], # In front of stair
+ [26.7, 15.2, 3.4], # Middle of stair left y: 1.47
+ [28.7, 15.2, 3.7], # Middle of stair right
+ [28, 5.7, 5.4], # 2nd floor
+ [5.7, 28.8, 5.2, 90], # In front of first door
+ [28, 5.7, 5.4], # 2nd floor
+ # Return to base
+ [28.7, 15.2, 3.7], # Middle of stair right
+ [26.7, 15.2, 3.4], # Middle of stair left y: 1.47
+ [26.7, 9.4, 1], # In front of stair
+ [28.9, 8.5, 1], # Right of stair
+ [28, 5.0, 1.5],
+ [28, 1.00, 1.5, 270],
+ [25, 0.00, 1.8005], # car
+ [18.5681804363, -0.525301411499, 1.50505519304], # after net
+ [13.4681804363, -0.525301411499, 1.50505519304], # before net
+ [11.0070259193, 2.08961550795, 2.43913609922], # wall
+ [10.70119367623, 2.13588568156, 3.21656869656], # before wall
+ [8.10119367623, 2.13588568156, 3.21656869656], # before wall
+ [7.20119367623, 2.13588568156, 1.71656869656], # second plants -> for rosbag
+ [3.72308462946, 2.16384181223, 1.59992909121], # first plants
+ [0, 0, 1.59992909121], # hover
+ [0, 0, -1], # landing
+ ]
diff --git a/src/rc_demo/utils.py b/src/rc_demo/utils.py
index 1b12745b..d3e79333 100644
--- a/src/rc_demo/utils.py
+++ b/src/rc_demo/utils.py
@@ -1,8 +1,75 @@
import rospy
+import math
+
+from typing import Tuple, List
from std_msgs.msg import String
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import CommandBool
+from nav_msgs.msg import Path, Odometry
+from geometry_msgs.msg import PoseStamped, Pose
+from visualization_msgs.msg import Marker
+
+
+def compute_distance(x: Pose, y: Pose) -> float:
+ return math.sqrt(math.pow(x.x - y.x, 2) + math.pow(x.y - y.y, 2) + math.pow(x.z - y.z, 2))
+
+
+def get_quaternion_from_euler(roll: float, pitch: float, yaw: float) -> Tuple[float, float, float, float]:
+ """Convert an Euler angle to a quaternion.
+
+ Args:
+ roll: The roll (rotation around x-axis) angle in radians.
+ pitch: The pitch (rotation around y-axis) angle in radians.
+ yaw: The yaw (rotation around z-axis) angle in radians.
+
+ Return:
+ qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format
+ """
+ sin_roll = math.sin(roll/2)
+ cos_roll = math.cos(roll/2)
+
+ sin_pitch = math.sin(pitch/2)
+ cos_pitch = math.cos(pitch/2)
+
+ sin_yaw = math.sin(yaw/2)
+ cos_yaw = math.cos(yaw/2)
+
+
+ qx = sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw
+ qy = cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw
+ qz = cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw
+ qw = cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw
+
+ return (qx, qy, qz, qw)
+
+
+def quaternion_to_euler(x: float, y: float, z: float, w: float) -> Tuple[float, float, float]:
+ """Convert a quaternion to Euler angle.
+
+ Args:
+ x: quaternion x
+ y: quaternion y
+ z: quaternion z
+ w: quaternion w
+
+ Return:
+ roll, pitch, yaw in radians.
+ """
+ t0 = +2.0 * (w * x + y * z)
+ t1 = +1.0 - 2.0 * (x * x + y * y)
+ roll = math.atan2(t0, t1)
+
+ t2 = +2.0 * (w * y - z * x)
+ t2 = +1.0 if t2 > +1.0 else t2
+ t2 = -1.0 if t2 < -1.0 else t2
+ pitch = math.asin(t2)
+
+ t3 = +2.0 * (w * z + x * y)
+ t4 = +1.0 - 2.0 * (y * y + z * z)
+ yaw = math.atan2(t3, t4)
+
+ return (roll, pitch, yaw)
class ControlMessage:
@@ -25,6 +92,199 @@ def send_control(self, roll: float, pitch: float, yaw: float, throttle: float) -
self.control_pub.publish(msg)
+class WaypointMessage:
+ """Send waypoint message via MavROS"""
+
+ def __init__(
+ self,
+ distance_tolerance: float = 0.5,
+ angle_tolerance: float = 10.0,
+ check_yaw: bool = True,
+ ) -> None:
+ self.waypoint_pub = rospy.Publisher('/waypoint_generator/waypoints',
+ Path,
+ queue_size=1)
+ self.odom_sub = rospy.Subscriber('/Odometry',
+ Odometry,
+ self.callback_odometry)
+ self.goal_point_sub = rospy.Subscriber('/ego_planner_node/goal_point',
+ Marker,
+ self.callback_goal_point)
+ self._timer_send_waypoint = rospy.Timer(rospy.Duration(3.0),
+ self._loop_send_waypoint)
+ self.distance_tolerance = distance_tolerance
+ self.check_yaw = check_yaw
+ self.angle_tolerance = angle_tolerance
+
+ self.current_position = Odometry()
+ """Target position is the designated waypoint"""
+ self.target_position = PoseStamped()
+ self.target_position.pose.orientation.w = 1.0
+ """Goal position is modified waypoint from ego-planner"""
+ self.goal_position = Pose()
+ self.goal_position.orientation.w = 1.0
+ self.is_moving = False
+ self.waypoints = []
+ self.send_seq = 0
+
+ self.offset_x = 0.0
+ self.offset_y = 0.0
+ self.offset_z = 0.0
+ self.offset_Y = 0.0
+
+ self.commander = MAVROSCommander()
+
+ def set_offset(self, x: float, y: float, z: float, Y: float) -> None:
+ self.offset_x = x
+ self.offset_y = y
+ self.offset_z = z
+ self.offset_Y = Y
+
+ def clear_waypoint(self) -> None:
+ self.waypoints.clear()
+ self.target_position.pose = self.current_position.pose.pose
+ self.is_moving = False
+
+ def add_waypoint(self, x: float, y: float, z: float) -> None:
+ self.waypoints.append((x, y, z))
+
+ def add_waypoint_yaw(self, x: float, y: float, z: float, yaw: float) -> None:
+ self.waypoints.append((x, y, z, yaw))
+
+ def add_waypoints(self, points: List[Tuple[float, float, float]]) -> None:
+ for point in points:
+ if len(point) > 3:
+ self.add_waypoint_yaw(*point)
+ else:
+ self.add_waypoint(*point)
+
+ def _check_current_location(self) -> bool:
+ if len(self.waypoints) < 1:
+ return False
+
+ # goal_point = self.target_position.pose
+ goal_point = self.goal_position
+ current_point = self.current_position.pose.pose
+
+ distance = compute_distance(goal_point.position, current_point.position)
+
+ rpy_goal = quaternion_to_euler(goal_point.orientation.x,
+ goal_point.orientation.y,
+ goal_point.orientation.w,
+ goal_point.orientation.z)
+ rpy_current = quaternion_to_euler(current_point.orientation.x,
+ current_point.orientation.y,
+ current_point.orientation.w,
+ current_point.orientation.z)
+ yaw_diff = min((2 * math.pi) - abs(rpy_goal[2]-rpy_current[2]), abs(rpy_goal[2]-rpy_current[2]))
+ yaw_diff = math.degrees(yaw_diff)
+
+ # Debug logging
+ if False:
+ rospy.loginfo(f"[Distance to the goal] {distance:.3f}m, [Angle difference]: {yaw_diff:.3f} degree")
+ rospy.loginfo(f"[ GOAL POSE] x: {goal_point.position.x:8.3f}, y: {goal_point.position.y:8.3f}, z: {goal_point.position.z:8.3f}, R: {rpy_goal[0]:8.3f}, P: {rpy_goal[1]:8.3f}, Y: {rpy_goal[2]:8.3f}")
+ rospy.loginfo(f"[CURRENT POSE] x: {current_point.position.x:8.3f}, y: {current_point.position.y:8.3f}, z: {current_point.position.z:8.3f}, R: {rpy_current[0]:8.3f}, P: {rpy_current[1]:8.3f}, Y: {rpy_current[2]:8.3f}")
+
+ if self.check_yaw:
+ if distance < self.distance_tolerance and yaw_diff < self.angle_tolerance:
+ return True
+ else:
+ if distance < self.distance_tolerance:
+ return True
+
+ return False
+
+ def _loop_send_waypoint(self, event = None) -> None:
+ target_point = None
+ if self.is_moving and not self._check_current_location():
+ target_point = self.target_position
+ else:
+ self.is_moving = False
+
+ if len(self.waypoints) < 1:
+ target_point = self.target_position
+
+ if target_point.pose.position.z == -1:
+ """change to land"""
+ self.commander.set_mode("AUTO.LAND")
+
+ self.send_seq += 1
+
+ if target_point == None:
+ target_point = self.waypoints.pop(0)
+
+ # apply offset
+ temp_x = target_point[0] - self.offset_x
+ temp_y = target_point[1] - self.offset_y
+ temp_rad = self.offset_Y / 180.0 * math.pi
+
+ # Ignore offset if waypoint is (0, 0)
+ if target_point[0] == 0 and target_point[1] == 0:
+ temp_x = temp_y = 0
+
+ pose = PoseStamped()
+ pose.header.seq = self.send_seq
+ pose.header.stamp = rospy.Time.now()
+ pose.header.frame_id = "map"
+ pose.pose.position.x = temp_x * math.cos(temp_rad) - temp_y * math.sin(temp_rad)
+ pose.pose.position.y = temp_x * math.sin(temp_rad) + temp_y * math.cos(temp_rad)
+ pose.pose.position.z = target_point[2] - self.offset_z
+ if self.current_position.pose.pose.orientation.x == 0 and \
+ self.current_position.pose.pose.orientation.y == 0 and \
+ self.current_position.pose.pose.orientation.z == 0 and \
+ self.current_position.pose.pose.orientation.w == 0:
+ pose.pose.orientation.w = 1.0
+ else:
+ pose.pose.orientation.x = self.current_position.pose.pose.orientation.x
+ pose.pose.orientation.y = self.current_position.pose.pose.orientation.y
+ pose.pose.orientation.z = self.current_position.pose.pose.orientation.z
+ pose.pose.orientation.w = self.current_position.pose.pose.orientation.w
+
+ if len(target_point) > 3:
+ quaternion = get_quaternion_from_euler(0, 0, math.radians(-(target_point[3] - self.offset_Y)+90))
+ pose.pose.orientation.x = quaternion[0]
+ pose.pose.orientation.y = quaternion[1]
+ pose.pose.orientation.z = quaternion[2]
+ pose.pose.orientation.w = quaternion[3]
+ else:
+ pose = target_point
+
+
+ msg = Path()
+ msg.header.seq = self.send_seq
+ msg.header.stamp = rospy.Time.now()
+ msg.header.frame_id = "map"
+ msg.poses.append(pose)
+ self.target_position = pose
+ self.goal_position.position = pose.pose.position
+ self.goal_position.orientation = pose.pose.orientation
+
+ # Debugging log
+ if False:
+ rospy.loginfo(f"[CURRENT WAYPOINT]: \n{pose.pose.position}")
+ rospy.loginfo(f"[ ORIENTATION ]: \n{pose.pose.orientation}")
+
+ self.waypoint_pub.publish(msg)
+ self.is_moving = True
+
+ def callback_odometry(self, msg: Odometry) -> None:
+ self.current_position = msg
+
+ if self.target_position.pose.position.x == 0 and self.target_position.pose.position.y == 0 and self.target_position.pose.position.z == 0:
+ self.goal_position.position = msg.pose.pose.position
+ self.goal_position.orientation = msg.pose.pose.orientation
+ self.target_position.pose.position = msg.pose.pose.position
+ self.target_position.pose.orientation = msg.pose.pose.orientation
+
+ def callback_goal_point(self, msg: Marker) -> None:
+ if msg.id != 1:
+ return
+
+ if compute_distance(msg.pose.position, self.target_position.pose.position) < (self.distance_tolerance * 2):
+ # self.target_position.pose.position = msg.pose.position
+ self.goal_position.position = msg.pose.position
+
+
class MAVROSCommander:
def __init__(self):
self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)