Skip to content

Commit

Permalink
Update bumper visualization
Browse files Browse the repository at this point in the history 
Signed-off-by: Alberto Tudela <[email protected]>
  • Loading branch information
@ajtudela
ajtudela committed Apr 8, 2024
1 parent 3c1aca5 commit 0bbf769
Show file tree
Hide file tree
Showing 5 changed files with 37 additions and 231 deletions.
4 changes: 3 additions & 1 deletion scitos2_modules/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(nav2_util REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
Expand Down Expand Up @@ -50,6 +51,7 @@ include_mira_packages()
set(dependencies
rclcpp
nav2_util
nav2_costmap_2d
geometry_msgs
nav_msgs
sensor_msgs
Expand Down Expand Up @@ -100,7 +102,7 @@ if(BUILD_TESTING)
# the following line skips the linter which checks for copyrights
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
#add_subdirectory(test)
add_subdirectory(test)
endif()

# ##################################
Expand Down
23 changes: 0 additions & 23 deletions scitos2_modules/include/scitos2_modules/drive.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -261,29 +261,6 @@ class Drive : public scitos2_core::Module
*/
visualization_msgs::msg::MarkerArray createBumperMarkers();

/**
* @brief Create a circular footprint from a given radius
*/
std::vector<geometry_msgs::msg::Point> makeFootprintFromRadius(double radius);

/**
* @brief Make the footprint from the given string.
*
* Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...]
*
*/
bool makeFootprintFromString(
const std::string & footprint_string, std::vector<geometry_msgs::msg::Point> & footprint);

/** @brief Parse a vector of vectors of floats from a string.
* @param error_return If no error, error_return is set to "". If
* error, error_return will describe the error.
* Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...]
*
* On error, error_return is set and the return value could be
* anything, like part of a successful parse. */
std::vector<std::vector<float>> parseVVF(const std::string & input, std::string & error_return);

/**
* @brief Callback executed when a parameter change is detected.
* @param event ParameterEvent message
Expand Down
1 change: 1 addition & 0 deletions scitos2_modules/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>nav2_util</depend>
<depend>nav2_costmap_2d</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>sensor_msgs</depend>
Expand Down
176 changes: 29 additions & 147 deletions scitos2_modules/src/drive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

// ROS
#include "nav2_util/node_utils.hpp"
#include "nav2_costmap_2d/footprint.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
Expand Down Expand Up @@ -160,12 +161,12 @@ void Drive::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, s
use_radius_ = true;
if (footprint_ != "" && footprint_ != "[]") {
// Footprint parameter has been specified, try to convert it
if (makeFootprintFromString(footprint_, unpadded_footprint_)) {
if (nav2_costmap_2d::makeFootprintFromString(footprint_, unpadded_footprint_)) {
// The specified footprint is valid, so we'll use that instead of the radius
use_radius_ = false;
} else {
// Footprint provided but invalid, so stay with the radius
unpadded_footprint_ = makeFootprintFromRadius(robot_radius_);
unpadded_footprint_ = nav2_costmap_2d::makeFootprintFromRadius(robot_radius_);
RCLCPP_ERROR(
logger_, "The footprint parameter is invalid: \"%s\", using radius (%lf) instead",
footprint_.c_str(), robot_radius_);
Expand Down Expand Up @@ -414,98 +415,42 @@ bool Drive::suspendBumper(
visualization_msgs::msg::MarkerArray Drive::createBumperMarkers()
{
// Create markers
visualization_msgs::msg::MarkerArray markers_msg;
visualization_msgs::msg::Marker cylinder;
cylinder.header.frame_id = base_frame_;
cylinder.header.stamp = clock_->now();
cylinder.lifetime = rclcpp::Duration(0, 10);
cylinder.ns = "bumpers";
cylinder.type = visualization_msgs::msg::Marker::CYLINDER;
cylinder.action = visualization_msgs::msg::Marker::ADD;
cylinder.scale.x = 0.05;
cylinder.scale.y = 0.05;
cylinder.scale.z = 0.45;
cylinder.pose.position.z = 0.03;
visualization_msgs::msg::MarkerArray marker_array;
visualization_msgs::msg::Marker marker;
marker.header.frame_id = base_frame_;
marker.header.stamp = clock_->now();
marker.ns = "bumper";
marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
marker.action = visualization_msgs::msg::Marker::ADD;
marker.scale.x = 0.05;
marker.scale.y = 0.05;
marker.scale.z = 0.45;
marker.pose.position.z = 0.03;
marker.pose.orientation.w = 1.0;

// Change color depending on the state: red if activated, white otherwise
if (bumper_activated_) {
cylinder.color.r = 1.0;
cylinder.color.g = 0.0;
cylinder.color.b = 0.0;
cylinder.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.color.a = 1.0;
} else {
cylinder.color.r = 1.0;
cylinder.color.g = 1.0;
cylinder.color.b = 1.0;
cylinder.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.color.a = 1.0;
}

// Create the markers using the footprint
for (size_t i = 0; i < unpadded_footprint_.size(); ++i) {
cylinder.id = i + 4;
cylinder.pose.position.x = unpadded_footprint_[i].x;
cylinder.pose.position.y = unpadded_footprint_[i].y;
markers_msg.markers.push_back(cylinder);
int id = 0;
for (const auto & point : unpadded_footprint_) {
marker.points.push_back(point);
marker.id = id++;
}

return markers_msg;
}

std::vector<geometry_msgs::msg::Point> Drive::makeFootprintFromRadius(double radius)
{
std::vector<geometry_msgs::msg::Point> points;

// Loop over 16 angles around a circle making a point each time
int N = 16;
geometry_msgs::msg::Point pt;
for (int i = 0; i < N; ++i) {
double angle = i * 2 * M_PI / N;
pt.x = cos(angle) * radius;
pt.y = sin(angle) * radius;
marker_array.markers.push_back(marker);

points.push_back(pt);
}

return points;
}

bool Drive::makeFootprintFromString(
const std::string & footprint_string, std::vector<geometry_msgs::msg::Point> & footprint)
{
std::string error;
std::vector<std::vector<float>> vvf = parseVVF(footprint_string, error);

if (error != "") {
RCLCPP_ERROR(logger_, "Error parsing footprint parameter: '%s'", error.c_str());
RCLCPP_ERROR(logger_, "Footprint string was '%s'.", footprint_string.c_str());
return false;
}

// Convert vvf into points.
if (vvf.size() < 3) {
RCLCPP_ERROR(
logger_,
"You must specify at least three points for the robot footprint, reverting to previous footprint."); //NOLINT
return false;
}
footprint.reserve(vvf.size());
for (unsigned int i = 0; i < vvf.size(); i++) {
if (vvf[i].size() == 2) {
geometry_msgs::msg::Point point;
point.x = vvf[i][0];
point.y = vvf[i][1];
point.z = 0;
footprint.push_back(point);
} else {
RCLCPP_ERROR(
logger_,
"Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.", //NOLINT
static_cast<int>(vvf[i].size()));
return false;
}
}

return true;
return marker_array;
}

bool Drive::isEmergencyStopReleased(scitos2_msgs::msg::EmergencyStopStatus msg)
Expand Down Expand Up @@ -600,69 +545,6 @@ scitos2_msgs::msg::BarrierStatus Drive::miraToRosBarrierStatus(
return barrier;
}

std::vector<std::vector<float>> Drive::parseVVF(
const std::string & input,
std::string & error_return)
{
std::vector<std::vector<float>> result;

std::stringstream input_ss(input);
int depth = 0;
std::vector<float> current_vector;
while (!!input_ss && !input_ss.eof()) {
switch (input_ss.peek()) {
case EOF:
break;
case '[':
depth++;
if (depth > 2) {
error_return = "Array depth greater than 2";
return result;
}
input_ss.get();
current_vector.clear();
break;
case ']':
depth--;
if (depth < 0) {
error_return = "More close ] than open [";
return result;
}
input_ss.get();
if (depth == 1) {
result.push_back(current_vector);
}
break;
case ',':
case ' ':
case '\t':
input_ss.get();
break;
default: // All other characters should be part of the numbers.
if (depth != 2) {
std::stringstream err_ss;
err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'.";
error_return = err_ss.str();
return result;
}
float value;
input_ss >> value;
if (!!input_ss) {
current_vector.push_back(value);
}
break;
}
}

if (depth != 0) {
error_return = "Unterminated vector string.";
} else {
error_return = "";
}

return result;
}

} // namespace scitos2_modules

PLUGINLIB_EXPORT_CLASS(scitos2_modules::Drive, scitos2_core::Module)
64 changes: 4 additions & 60 deletions scitos2_modules/test/test_drive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,17 +65,6 @@ class DriveFixture : public scitos2_modules::Drive
emergency_stop_activated_ = false;
}

std::vector<geometry_msgs::msg::Point> makeFootprintFromRadius(double radius)
{
return scitos2_modules::Drive::makeFootprintFromRadius(radius);
}

bool makeFootprintFromString(
const std::string & footprint_string, std::vector<geometry_msgs::msg::Point> & footprint)
{
return scitos2_modules::Drive::makeFootprintFromString(footprint_string, footprint);
}

visualization_msgs::msg::MarkerArray createBumperMarkers()
{
return scitos2_modules::Drive::createBumperMarkers();
Expand Down Expand Up @@ -472,51 +461,6 @@ TEST(ScitosDriveTest, suspendBumper) {
rclcpp::shutdown();
}

TEST(ScitosDriveTest, makeFootprintFromRadius) {
// Create the module
auto module = std::make_shared<DriveFixture>();

// Set a valid radius
auto footprint = module->makeFootprintFromRadius(0.5);

// Check results
EXPECT_EQ(16u, footprint.size());
}

TEST(ScitosDriveTest, makeFootprintFromString) {
// Create the module
auto module = std::make_shared<DriveFixture>();

// Set a valid footprint
std::vector<geometry_msgs::msg::Point> footprint;
bool result = module->makeFootprintFromString(
"[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint);
EXPECT_EQ(result, true);
EXPECT_EQ(4u, footprint.size());
EXPECT_NEAR(footprint[0].x, 1.0, 1e-5);
EXPECT_NEAR(footprint[0].y, 2.2, 1e-5);
EXPECT_NEAR(footprint[1].x, 0.3, 1e-5);
EXPECT_NEAR(footprint[1].y, -4e4, 1e-5);
EXPECT_NEAR(footprint[2].x, -0.3, 1e-5);
EXPECT_NEAR(footprint[2].y, -4e4, 1e-5);
EXPECT_NEAR(footprint[3].x, -1.0, 1e-5);
EXPECT_NEAR(footprint[3].y, 2.2, 1e-5);

// Set a wrong string
result = module->makeFootprintFromString(
"[[bad_string", footprint);
EXPECT_EQ(result, false);

// Set two points
result = module->makeFootprintFromString("[[1, 2.2], [.3, -4e4]]", footprint);
EXPECT_EQ(result, false);

// Set a wrong number of points
result = module->makeFootprintFromString(
"[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint);
EXPECT_EQ(result, false);
}

TEST(ScitosDriveTest, createBumperMarkers) {
rclcpp::init(0, nullptr);
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testDrive");
Expand All @@ -534,16 +478,16 @@ TEST(ScitosDriveTest, createBumperMarkers) {
// Deactivate the bumper
module->deactivateBumper();
// Create the bumper markers
auto markers = module->createBumperMarkers();
auto marker_array = module->createBumperMarkers();
// Check the number of markers
EXPECT_EQ(markers.markers.size(), 4);
EXPECT_EQ(marker_array.markers.front().points.size(), 4);

// Activate the bumper
module->activateBumper();
// Create the bumper markers
markers = module->createBumperMarkers();
marker_array = module->createBumperMarkers();
// Check the number of markers
EXPECT_EQ(markers.markers.size(), 4);
EXPECT_EQ(marker_array.markers.front().points.size(), 4);
rclcpp::shutdown();
}

Expand Down

0 comments on commit 0bbf769

Please sign in to comment.