From 8b67248bc60bf31ed146a00f15777215d014a2e5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=EC=9D=B4=EC=8A=B9=EC=A4=80?= Date: Mon, 21 Aug 2023 15:30:00 +0900 Subject: [PATCH 1/4] fix message type conflict and function return type --- lmpcc/config/lmpcc_config_parameter.yaml | 2 +- lmpcc/include/lmpcc/lmpcc_controller.h | 2 +- lmpcc/src/lmpcc_controller.cpp | 2 +- .../include/lmpcc_obstacle_feed/lmpcc_obstacle_feed.h | 2 +- lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/lmpcc/config/lmpcc_config_parameter.yaml b/lmpcc/config/lmpcc_config_parameter.yaml index fb49294..5bf846d 100755 --- a/lmpcc/config/lmpcc_config_parameter.yaml +++ b/lmpcc/config/lmpcc_config_parameter.yaml @@ -5,7 +5,7 @@ gazebo_simulation: false # Debug modes activate_output: true activate_debug_output: false -activate_timing_output: false +activate_timing_output: true activate_visualization: true activate_feedback_message: true diff --git a/lmpcc/include/lmpcc/lmpcc_controller.h b/lmpcc/include/lmpcc/lmpcc_controller.h index 06de9f4..7bd10f4 100755 --- a/lmpcc/include/lmpcc/lmpcc_controller.h +++ b/lmpcc/include/lmpcc/lmpcc_controller.h @@ -185,7 +185,7 @@ class LMPCC bool initialize(); - bool initialize_visuals(); + void initialize_visuals(); /** * @brief StateCallBack: Get current state of the robot diff --git a/lmpcc/src/lmpcc_controller.cpp b/lmpcc/src/lmpcc_controller.cpp index e9856e3..1823af6 100755 --- a/lmpcc/src/lmpcc_controller.cpp +++ b/lmpcc/src/lmpcc_controller.cpp @@ -228,7 +228,7 @@ bool LMPCC::initialize() } } -bool LMPCC::initialize_visuals() +void LMPCC::initialize_visuals() { //ROS_INFO_STREAM("initialize_visuals"); robot_collision_space_pub_ = nh.advertise("/robot_collision_space", 100); diff --git a/lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed.h b/lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed.h index 295c267..ca4936f 100644 --- a/lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed.h +++ b/lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed.h @@ -154,7 +154,7 @@ class ObstacleFeed void clearDataMember(); void detectionsCallback(const vision_msgs::Detection3DArray& objects); void pedestriansCallback(const spencer_tracking_msgs::TrackedPersons& person); - void PedsimCallback(const pedsim_msgs::TrackedPersons& person); + void PedsimCallback(const spencer_tracking_msgs::TrackedPersons& person); void optitrackCallback(const nav_msgs::Path& predicted_path); void updateObstacles(const ros::TimerEvent& event); void publishObstacles(const lmpcc_msgs::lmpcc_obstacle_array& obstacles); diff --git a/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp b/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp index b276e24..f3dc882 100644 --- a/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp +++ b/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp @@ -395,7 +395,7 @@ void ObstacleFeed::detectionsCallback(const vision_msgs::Detection3DArray& objec } -void ObstacleFeed::PedsimCallback(const pedsim_msgs::TrackedPersons& person) +void ObstacleFeed::PedsimCallback(const spencer_tracking_msgs::TrackedPersons& person) { //ROS_INFO_STREAM("PEDSIM callback!"); From d3a76be571e8329382dc17d11060d7488f4e5a28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=EC=9D=B4=EC=8A=B9=EC=A4=80?= Date: Tue, 22 Aug 2023 11:44:30 +0900 Subject: [PATCH 2/4] change goal reach condition and fix loop mode bug --- lmpcc/src/lmpcc_controller.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/lmpcc/src/lmpcc_controller.cpp b/lmpcc/src/lmpcc_controller.cpp index 1823af6..b93ea91 100755 --- a/lmpcc/src/lmpcc_controller.cpp +++ b/lmpcc/src/lmpcc_controller.cpp @@ -483,8 +483,9 @@ void LMPCC::controlLoop(const ros::TimerEvent &event) acadoVariables.u[3] = 0.0000001; //slack variable if(acadoVariables.x[3] > ss[2]) { - - if((std::sqrt(std::pow(current_state_(0)-15,2)+std::pow(current_state_(1),2))<1) || (current_state_(0)>15)){ + double goal_x = lmpcc_config_->ref_x_[lmpcc_config_->ref_x_.size() - 1]; + double goal_y = lmpcc_config_->ref_y_[lmpcc_config_->ref_y_.size() - 1]; + if(std::sqrt(std::pow(current_state_(0) - goal_x, 2) + std::pow(current_state_(1) - goal_y, 2)) < 3){ goal_reached_ = true; ROS_ERROR_STREAM("GOAL REACHED"); @@ -499,6 +500,18 @@ void LMPCC::controlLoop(const ros::TimerEvent &event) acadoVariables.x[3] = referencePath.GetS0(); //ROS_ERROR_STREAM("LOOP STARTED"); + + /** Set task flags and counters **/ + segment_counter = 0; + goal_reached_ = false; + + if (lmpcc_config_->activate_debug_output_) + referencePath.PrintLocalPath(ss,xx,yy); // Print local reference path + if (lmpcc_config_->activate_visualization_) + { + publishLocalRefPath(); // Publish local reference path for visualization + publishGlobalPlan(); // Publish global reference path for visualization + } } } else{ segment_counter++; @@ -667,6 +680,8 @@ void LMPCC::controlLoop(const ros::TimerEvent &event) } if(!enable_output_ || acado_getKKT() > lmpcc_config_->kkt_tolerance_) { + if (acado_getKKT() > lmpcc_config_->kkt_tolerance_) + ROS_INFO_STREAM("Failed to find solution! KKT value: " << acado_getKKT() << ", tolerance: " << lmpcc_config_->kkt_tolerance_); publishZeroJointVelocity(); } else { From d9c49afcfd1201740d0eaa9514e66b3fe822eb47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=EC=9D=B4=EC=8A=B9=EC=A4=80?= Date: Tue, 22 Aug 2023 11:54:18 +0900 Subject: [PATCH 3/4] update README --- README.md | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index fb31c64..3e465a5 100644 --- a/README.md +++ b/README.md @@ -43,21 +43,18 @@ The authors would like to thank Wilko Schwarting for sharing its Matlab implemen ## Software Requirements * ROS installation * Ubuntu -* Pedestrian Simulator: pedsim_ros: https://github.com/bbrito/pedsim_ros +* Pedestrian Simulator: pedsim_ros: https://github.com/weemma/pedsim_ros * Branch: four_persons ## Instalation instructions -This set of instructions were only tested for Ubuntu16 with ROS-Kinetic. Additionally, we assume that you already have a complete ROS installation. +This set of instructions were tested for Ubuntu16.04 with ROS-Kinetic and Ubuntu20.04 with ROS-Noetic. Additionally, we assume that you already have a complete ROS installation. * Please follow the following instructions: ``` -sudo apt-get install ros-kinetic-jackal-control ros-kinetic-jackal-gazebo ros-kinetic-jackal-simulator ros-kinetic-jackal-description ros-kinetic-jackal-desktop ros-kinetic-jackal-navigation ros-kinetic-jackal-viz -sudo apt install ros-kinetic-people-msgs -cd [workspace]/src -git clone https://github.com/bbrito/pedsim_ros.git -b four_persons -git clone https://github.com/spencer-project/spencer_messages.git -git clone https://github.com/srl-freiburg/spencer_tracking_rviz_plugin.git +sudo apt-get install ros-$ROS_DISTRO-jackal-control ros-$ROS_DISTRO-jackal-gazebo ros-$ROS_DISTRO-jackal-simulator ros-$ROS_DISTRO-jackal-description ros-$ROS_DISTRO-jackal-desktop ros-$ROS_DISTRO-jackal-navigation ros-$ROS_DISTRO-jackal-viz ros-$ROS_DISTRO-people-msgs +cd [catkin_workspace]/src +git clone --recursive https://github.com/weemma/pedsim_ros.git -b four_persons git clone https://github.com/tud-amr/amr-lmpcc.git -cc ../ +cd ../ rosdep install --from-paths src --ignore-src -r -y catkin_make source devel/setup.bash @@ -77,7 +74,7 @@ Repository: https://github.com/bbrito/acado/blob/four_persons/examples/ocp/biore 1. roslaunch lmpcc_obstacle_feed lmpcc_obstacle_feed.launch -*Start lmpcc controller +* Start lmpcc controller 1. roslaunch lmpcc lmpcc.launch From 81faba5362802539bbf27533478c0b1c7a590cc1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=EC=9D=B4=EC=8A=B9=EC=A4=80?= Date: Thu, 24 Aug 2023 16:54:46 +0900 Subject: [PATCH 4/4] modify the way of printing solving time --- lmpcc_obstacle_feed/config/lmpcc_obstacle_feed_config.yaml | 3 ++- .../lmpcc_obstacle_feed_configuration.h | 1 + lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp | 7 ++++++- .../src/lmpcc_obstacle_feed_configuration.cpp | 7 +++++++ static_collision_avoidance/src/static_environment.cpp | 4 ++-- 5 files changed, 18 insertions(+), 4 deletions(-) diff --git a/lmpcc_obstacle_feed/config/lmpcc_obstacle_feed_config.yaml b/lmpcc_obstacle_feed/config/lmpcc_obstacle_feed_config.yaml index 8c8e9d1..2234e7f 100644 --- a/lmpcc_obstacle_feed/config/lmpcc_obstacle_feed_config.yaml +++ b/lmpcc_obstacle_feed/config/lmpcc_obstacle_feed_config.yaml @@ -5,6 +5,7 @@ obstacle_feed_mode: 4 # Debugging info activate_debug_output: false activate_visualization: true +activate_timing_output: true # Rate at which ellipse obstacles should be send out update_rate: 20 @@ -30,7 +31,7 @@ subscribe: # Detection criteria detected_obstacles: distance_threshold: 50 - obstacle_number: 4 + obstacle_number: 8 min_obstacle_volume: 0.03 max_obstacle_volume: 1.0 obstacle_size: 0.3 diff --git a/lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed_configuration.h b/lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed_configuration.h index 8e1b085..d187614 100644 --- a/lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed_configuration.h +++ b/lmpcc_obstacle_feed/include/lmpcc_obstacle_feed/lmpcc_obstacle_feed_configuration.h @@ -78,6 +78,7 @@ class lmpcc_obstacle_feed_configuration bool activate_debug_output_; bool activate_visualization_; + bool activate_timing_output_; int update_rate_; int discretization_steps_; diff --git a/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp b/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp index f3dc882..3ce286f 100644 --- a/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp +++ b/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed.cpp @@ -398,7 +398,7 @@ void ObstacleFeed::detectionsCallback(const vision_msgs::Detection3DArray& objec void ObstacleFeed::PedsimCallback(const spencer_tracking_msgs::TrackedPersons& person) { //ROS_INFO_STREAM("PEDSIM callback!"); - + auto start = std::chrono::steady_clock::now(); double Xp, Yp; double q1, q2, q3, q4; double mag_q, distance; @@ -616,6 +616,11 @@ void ObstacleFeed::PedsimCallback(const spencer_tracking_msgs::TrackedPersons& p visualizeObstacles(local_ellipses); } + auto end = std::chrono::steady_clock::now(); + double dynamic_obs = double(std::chrono::duration_cast (end-start).count()); + + if (lmpcc_obstacle_feed_config_->activate_timing_output_) + ROS_INFO_STREAM("Dynamic obstacle solve time " << dynamic_obs << " us"); } void ObstacleFeed::pedestriansCallback(const spencer_tracking_msgs::TrackedPersons& person) diff --git a/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed_configuration.cpp b/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed_configuration.cpp index a1a5cd6..ea53ad8 100644 --- a/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed_configuration.cpp +++ b/lmpcc_obstacle_feed/src/lmpcc_obstacle_feed_configuration.cpp @@ -90,6 +90,13 @@ bool lmpcc_obstacle_feed_configuration::initialize() return false; } + // read parameter from parameter server if not set than terminate code, as this parameter is essential parameter + if (!nh.getParam ("activate_timing_output", activate_timing_output_) ) + { + ROS_WARN(" Parameter 'activate_timing_output' not set on %s node " , ros::this_node::getName().c_str()); + return false; + } + // read parameter from parameter server if not set than terminate code, as this parameter is essential parameter if (!nh.getParam ("update_rate", update_rate_) ) { diff --git a/static_collision_avoidance/src/static_environment.cpp b/static_collision_avoidance/src/static_environment.cpp index 252a74c..04317bd 100644 --- a/static_collision_avoidance/src/static_environment.cpp +++ b/static_collision_avoidance/src/static_environment.cpp @@ -314,10 +314,10 @@ void StaticEnvironment::ComputeCollisionFreeArea() } auto end = std::chrono::steady_clock::now(); - te_collision_free_ = double(std::chrono::duration_cast (end-start).count()); + te_collision_free_ = double(std::chrono::duration_cast (end-start).count()); if (activate_timing_output_) - ROS_INFO_STREAM("Free space solve time " << te_collision_free_ << " ms"); + ROS_INFO_STREAM("Free space solve time " << te_collision_free_ << " us"); } void StaticEnvironment::computeConstraint(int x_i, int y_i, double x_path, double y_path, double psi_path, int N)