diff --git a/CMakeLists.txt b/CMakeLists.txt
index 10ee905..b5582b4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -28,6 +28,7 @@ SET(ROS_BUILD_SHARED_LIBS false)
LIST(APPEND CMAKE_PREFIX_PATH "/opt/libtorch")
FIND_PACKAGE(Torch REQUIRED)
FIND_PACKAGE(OpenCV REQUIRED)
+FIND_PACKAGE(zed_interfaces)
IF(NOT ROS_BUILD_TYPE)
SET(ROS_BUILD_TYPE Release)
@@ -52,6 +53,7 @@ ADD_SUBDIRECTORY(src/shared)
INCLUDE_DIRECTORIES(src/shared)
INCLUDE_DIRECTORIES(src/config_reader/include)
INCLUDE_DIRECTORIES(src/third_party)
+INCLUDE_DIRECTORIES(/home/amrl_user/catkin_ws/devel/include/zed_interfaces)
INCLUDE_DIRECTORIES(src)
INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
diff --git a/manifest.xml b/manifest.xml
index 4062633..c9cec87 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -19,7 +19,7 @@
-
+
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
index 3a000d1..5667df2 100644
--- a/src/navigation/navigation.cc
+++ b/src/navigation/navigation.cc
@@ -779,6 +779,10 @@ string Navigation::GetNavStatus() {
}
}
+uint8_t Navigation::GetNavStatusUint8() {
+ return static_cast(nav_state_);
+}
+
vector Navigation::GetPredictedCloud() {
return fp_point_cloud_;
}
@@ -878,7 +882,7 @@ bool Navigation::Run(const double& time,
prev_state = nav_state_;
if (nav_state_ == NavigationState::kGoto &&
local_target_.squaredNorm() < Sq(params_.target_dist_tolerance) &&
- robot_vel_.squaredNorm() < Sq(params_.target_dist_tolerance)) {
+ robot_vel_.squaredNorm() < Sq(params_.target_vel_tolerance)) {
nav_state_ = NavigationState::kTurnInPlace;
} else if (nav_state_ == NavigationState::kTurnInPlace &&
AngleDist(robot_angle_, nav_goal_angle_) <
diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h
index ffb9396..af465e0 100644
--- a/src/navigation/navigation.h
+++ b/src/navigation/navigation.h
@@ -134,6 +134,7 @@ class Navigation {
Eigen::Vector2f GetVelocity();
float GetAngularVelocity();
std::string GetNavStatus();
+ uint8_t GetNavStatusUint8();
std::vector GetPredictedCloud();
float GetCarrotDist();
float GetObstacleMargin();
@@ -182,17 +183,17 @@ class Navigation {
// Publish a status message
void PublishNavStatus(const Eigen::Vector2f& carrot);
- // Current robot location.
+ // Current map frame robot location (LocalizationCallback).
Eigen::Vector2f robot_loc_;
- // Current robot orientation.
+ // Current map frame robot orientation (LocalizationCallback).
float robot_angle_;
// Current robot velocity.
Eigen::Vector2f robot_vel_;
// Current robot angular speed.
float robot_omega_;
- // Odometry-reported robot location.
+ // Current odometry frame robot location (OdometryCallback).
Eigen::Vector2f odom_loc_;
- // Odometry-reported robot angle.
+ // Current odometry frame robot angle (OdometryCallback).
float odom_angle_;
// Newest odometry message received.
Odom latest_odom_msg_;
diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc
index bed5fb9..5b7d733 100644
--- a/src/navigation/navigation_main.cc
+++ b/src/navigation/navigation_main.cc
@@ -38,7 +38,7 @@
#include "config_reader/config_reader.h"
#include "motion_primitives.h"
#include "constant_curvature_arcs.h"
-#include "actionlib_msgs/GoalStatus.h"
+#include "amrl_msgs/NavStatusMsg.h"
#include "amrl_msgs/Pose2Df.h"
#include "glog/logging.h"
#include "gflags/gflags.h"
@@ -72,7 +72,17 @@
#include "motion_primitives.h"
#include "navigation.h"
-using actionlib_msgs::GoalStatus;
+// TODO: Edit CMakeLists.txt to include the zed_interfaces package
+// You should be able to do this by just adding `zed_interfaces`
+// to the `find_package()` line if it is set up properly
+// TODO: Might need to fix these includes as well
+// #include
+// #include
+// Maybe try these if you're not using the ROS wrapper:
+#include "zed_interfaces/Object.h"
+#include "zed_interfaces/ObjectsStamped.h"
+
+using amrl_msgs::NavStatusMsg;
using amrl_msgs::VisualizationMsg;
using amrl_msgs::AckermannCurvatureDriveMsg;
using math_util::DegToRad;
@@ -123,6 +133,7 @@ bool simulate_ = false;
bool enabled_ = false;
bool received_odom_ = false;
bool received_laser_ = false;
+bool override_human_vel_ = false;
Vector2f goal_ = {0, 0};
Vector2f current_loc_ = {0, 0};
Vector2f current_vel_ = {0, 0};
@@ -141,7 +152,6 @@ ros::Publisher twist_drive_pub_;
ros::Publisher viz_pub_;
ros::Publisher map_lines_publisher_;
ros::Publisher pose_marker_publisher_;
-ros::Publisher nav_status_pub_;
ros::Publisher status_pub_;
ros::Publisher fp_pcl_pub_;
ros::Publisher path_pub_;
@@ -255,6 +265,19 @@ void OverrideCallback(const amrl_msgs::Pose2Df& msg) {
navigation_.SetOverride(loc, msg.theta);
}
+void HumanCallback(const zed_interfaces::ObjectsStamped::ConstPtr& msg) {
+ for(size_t i = 0; i < msg->objects.size(); i++) {
+ if (msg->objects[i].label_id == -1) continue;
+ auto velocity = msg->objects[i].velocity; // float32 array of size 3
+ if (velocity[0] > 0.5) { // TODO: Add your own logic here
+ override_human_vel_ = true;
+ }
+ else {
+ override_human_vel_ = false;
+ }
+ }
+}
+
void SignalHandler(int) {
if (!run_) {
printf("Force Exit.\n");
@@ -319,8 +342,9 @@ navigation::Twist ToTwist(geometry_msgs::TwistStamped twist_msg) {
}
void PublishNavStatus() {
- GoalStatus status;
- status.status = 1;
+ NavStatusMsg status;
+ status.stamp = ros::Time::now();
+ status.status = navigation_.GetNavStatusUint8();
status_pub_.publish(status);
}
@@ -807,7 +831,7 @@ int main(int argc, char** argv) {
"ackermann_curvature_drive", 1);
twist_drive_pub_ = n.advertise(
FLAGS_twist_drive_topic, 1);
- status_pub_ = n.advertise("navigation_goal_status", 1);
+ status_pub_ = n.advertise("navigation_goal_status", 1);
viz_pub_ = n.advertise("visualization", 1);
viz_img_pub_ = it_.advertise("vis_image", 1);
fp_pcl_pub_ = n.advertise("forward_predicted_pcl", 1);
@@ -837,13 +861,15 @@ int main(int argc, char** argv) {
ros::Subscriber goto_sub =
n.subscribe("/move_base_simple/goal", 1, &GoToCallback);
ros::Subscriber goto_amrl_sub =
- n.subscribe("/move_base_simple/goal_amrl", 1, &GoToCallbackAMRL);
+ n.subscribe("set_nav_target", 1, &GoToCallbackAMRL);
ros::Subscriber enabler_sub =
n.subscribe(CONFIG_enable_topic, 1, &EnablerCallback);
ros::Subscriber halt_sub =
n.subscribe("halt_robot", 1, &HaltCallback);
ros::Subscriber override_sub =
n.subscribe("nav_override", 1, &OverrideCallback);
+ ros::Subscriber human_vel_sub =
+ n.subscribe("/zed2i/zed_node/obj_det/objects", 1, &HumanCallback);
std_msgs::Header viz_img_header; // empty viz_img_header
viz_img_header.stamp = ros::Time::now(); // time
@@ -884,7 +910,9 @@ int main(int argc, char** argv) {
}
// Publish Commands
- SendCommand(cmd_vel, cmd_angle_vel);
+ // TODO: set updated_human_vel accordingly
+ static float updated_human_vel = 0.5;
+ SendCommand(override_human_vel_ ? Eigen::Vector2f::Constant(updated_human_vel) : cmd_vel, cmd_angle_vel);
}
loop.Sleep();
}